diff --git a/.agents/skills/add-robot/SKILL.md b/.agents/skills/add-robot/SKILL.md index 26733d298..d2d71335d 100644 --- a/.agents/skills/add-robot/SKILL.md +++ b/.agents/skills/add-robot/SKILL.md @@ -17,7 +17,7 @@ Every robot config subclasses `RobotCfg` and overrides two hooks: - `_build_defaults(self, init_dict=None)` — read variant fields from `init_dict`, set them on `self`, then populate `urdf_cfg` / `control_parts` / `solver_cfg` / - `drive_pros` / `attrs`. + `joint_drive_props` / `attrs`. - `build_pk_serial_chain(self, device=...)` — return `{control_part: pk.SerialChain}`, reading the PK URDF from a single `_pk_urdf_path` source. @@ -46,8 +46,8 @@ A cfg's `_build_defaults` must populate: - `urdf_cfg` (URDFCfg) or `fpath` - `control_parts` (Dict[str, List[str]]; joint names support regex) - `solver_cfg` (Dict[str, SolverCfg]; keys match `control_parts`) -- `drive_pros` (JointDrivePropertiesCfg) -- `attrs` (RigidBodyAttributesCfg) +- `joint_drive_props` (JointDrivePropertiesCfg) +- `attrs` (RigidBodyPhysicsCfg) `build_pk_serial_chain` must read from `_pk_urdf_path` (a property for constant-path robots, a method for variant-dependent paths). The PK chain's DOF @@ -69,7 +69,7 @@ must match the matching `control_parts` entry (the test stub asserts this). self.urdf_cfg = URDFCfg(components=[...]) self.control_parts = {"arm": ["JOINT[1-6]"]} self.solver_cfg = {"arm": OPWSolverCfg(end_link_name="link6", root_link_name="base_link")} - self.drive_pros = JointDrivePropertiesCfg(stiffness={"JOINT[1-6]": 1e4}) + self.joint_drive_props = JointDrivePropertiesCfg(stiffness={"JOINT[1-6]": 1e4}) ``` Variant-aware template (reads version / arm_kind): @@ -79,7 +79,7 @@ must match the matching `control_parts` entry (the test stub asserts this). init_dict = init_dict or {} self.version = MyRobotVersion(init_dict.get("version", "v1")) self.arm_kind = MyRobotArmKind(init_dict.get("arm_kind", "default")) - ... # then urdf_cfg / control_parts / solver_cfg / drive_pros / attrs + ... # then urdf_cfg / control_parts / solver_cfg / joint_drive_props / attrs ``` 4. **Implement `build_pk_serial_chain`** reading from `_pk_urdf_path`: @@ -149,8 +149,8 @@ Do not copy those semantic declarations into the robot class. | `urdf_cfg` | URDFCfg | URDF file and components | | `control_parts` | Dict[str, List[str]] | Joint groups for control | | `solver_cfg` | Dict[str, SolverCfg] | IK solver configurations | -| `drive_pros` | JointDrivePropertiesCfg | Joint stiffness, damping, force | -| `attrs` | RigidBodyAttributesCfg | Rigid-body physics attributes | +| `joint_drive_props` | JointDrivePropertiesCfg | Joint drive, limits, friction, and armature | +| `attrs` | RigidBodyPhysicsCfg | Rigid-body physics attributes | | variant fields | enum / str / bool | Optional subclass fields | | `_pk_urdf_path` | property or method → str | URDF for the FK/IK serial chain | @@ -160,5 +160,5 @@ Do not copy those semantic declarations into the robot class. - Registry: `embodichain/lab/sim/robots/__init__.py` - Docs: `docs/source/resources/robot/.md` - Tests: `tests/sim/objects/test_robot_cfg.py` -- Base class: `embodichain/lab/sim/cfg.py` (`RobotCfg`) +- Base class: `embodichain/lab/sim/cfg/robot.py` (`RobotCfg`) - Guide: `docs/source/guides/add_robot.rst` · Tutorial: `docs/source/tutorial/add_robot.rst` diff --git a/.agents/skills/add-solver/SKILL.md b/.agents/skills/add-solver/SKILL.md index eabad3e25..0a8ce8565 100644 --- a/.agents/skills/add-solver/SKILL.md +++ b/.agents/skills/add-solver/SKILL.md @@ -219,7 +219,7 @@ exactly: `test_ur_solver.py`) to sample joint configs within limits with a safety margin. - A `BaseSolverTest` class with: - - `setup_simulation(self, sim_device)` — builds a `SimulationManagerCfg`, + - `setup_simulation(self, device)` — builds a `SimulationManagerCfg`, a `RobotCfg` whose `solver_cfg={"arm": SolverCfg(...)}` uses the new solver, and adds the robot via `self.sim.add_robot(cfg=cfg)`. - `test_ik(self)` — the round-trip contract: diff --git a/.agents/skills/add-task-env/SKILL.md b/.agents/skills/add-task-env/SKILL.md index df17d2aec..2e785907a 100644 --- a/.agents/skills/add-task-env/SKILL.md +++ b/.agents/skills/add-task-env/SKILL.md @@ -86,6 +86,8 @@ For new task-first compositions, prefer: This is a component, not a runnable deployment. It owns: - `environment_id`; +- exactly one explicit `physics: default|newton` backend and its optional, + backend-matching `physics_config`; - ordinary environment values such as episode limits and environment count; - physical simulation entities under `simulation`; and - manager configuration under `env`. @@ -117,8 +119,10 @@ embodiment: The original inline Gym format remains supported. When extending an existing inline `env.json` or `env.yaml`, preserve that representation unless the user -asked for component extraction. Never select a component and repeat its owned -inline fields in the same deployment. +asked for component extraction. Every inline runnable config declares exactly +one `physics: default|newton` backend. Never select a component and repeat its +owned inline fields, including `physics` or `physics_config`, in the same +deployment. Use separate environment files for backend-specific settings. ### Optional Python entry point @@ -173,6 +177,7 @@ trainer-routing smoke test when dependencies permit. - [ ] Task family, optional subdomain, task name, and runnable IDs are stable - [ ] Python and config paths share the same task-first hierarchy - [ ] Environment component and runnable deployment ownership are not mixed +- [ ] Each environment file declares one backend and only matching physics fields - [ ] A Python task module exists only when the selected route needs it - [ ] No solution-method directory or same-named task package was introduced - [ ] Existing components and manager functors were reused where possible diff --git a/.agents/skills/add-test/SKILL.md b/.agents/skills/add-test/SKILL.md index 6ff21dc69..1022194b8 100644 --- a/.agents/skills/add-test/SKILL.md +++ b/.agents/skills/add-test/SKILL.md @@ -83,7 +83,7 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg class TestMySimComponent: def setup_method(self): - config = SimulationManagerCfg(headless=True, sim_device="cpu") + config = SimulationManagerCfg(headless=True, device="cpu") self.sim = SimulationManager(config) # ... setup ... diff --git a/AGENTS.md b/AGENTS.md index 689fd82c7..d18d4df5f 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -65,6 +65,12 @@ omit the Python task module. Follow the authoritative [configuration and registration contract](agent_context/topics/env-framework/configuration.md) and `/add-task-env` before adding a deployment. +Every inline runnable Gym config and reusable physical environment declares +exactly one `physics: default|newton` backend. An environment component owns +its optional matching `physics_config`; a deployment cannot repeat or override +either field. Launcher `--physics` may confirm the file-owned backend but does +not switch it. Use a separate environment config for each backend. + ## Code and validation - Run **`black==26.3.1`**, using `black .`, before every commit. diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 2745240de..b672deebd 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -8,22 +8,29 @@ defaults: topics: - id: simulation-system title: Simulation System - aliases: [simulation system, simulation manager, motion module, sim motion, 运动能力模块, 仿真系统, 仿真管理器] + aliases: [simulation system, simulation manager, Newton physics backend, motion module, sim motion, camera attachment, 运动能力模块, 仿真系统, 仿真管理器] keywords: [SimulationManager, SimulationManagerCfg, RenderCfg, DLSSCfg, DLSS, frame_time_delta_ms, - offscreen_dlss_enabled, explicit physics stepping, arena, DexSim, ArticulationJointKinematics, - get_parent_joint_chain, enable_gravity] + offscreen_dlss_enabled, explicit physics stepping, arena, DexSim, PhysicsBackendCfg, + DefaultPhysicsCfg, NewtonPhysicsCfg, AutoSolverCfg, DexUniSolverCfg, DexUni, runtime control, deformable, particle set, + ArticulationJointKinematics, get_parent_joint_chain, asset_physics_mode, joint_drive_props, enable_gravity, quaternion, xyzw] paths: [topics/simulation-system/simulation-system.md] source_of_truth: - embodichain/lab/sim/motion/__init__.py - embodichain/lab/sim/sim_manager.py - embodichain/lab/sim/__init__.py - - embodichain/lab/sim/cfg.py + - embodichain/lab/sim/cfg/ + - embodichain/lab/sim/physics/ + - embodichain/lab/sim/spawn/ - embodichain/lab/sim/common.py + - embodichain/lab/sim/_startup_summary.py + - embodichain/lab/sim/_runtime_controls.py - embodichain/lab/sim/objects/articulation.py + - embodichain/lab/sim/objects/backends/ + - embodichain/lab/sim/objects/deformable/ - embodichain/lab/sim/objects/gizmo.py - embodichain/lab/gym/envs/base_env.py - watch_paths: [embodichain/lab/sim/sim_manager.py, embodichain/lab/sim/cfg.py, embodichain/lab/sim/objects/, - tests/sim/] + watch_paths: [embodichain/lab/sim/sim_manager.py, embodichain/lab/sim/cfg/, embodichain/lab/sim/physics/, + embodichain/lab/sim/spawn/, embodichain/lab/sim/objects/, tests/sim/] related_topics: [env-framework, robot-system, sensor-system, sim-visualization, ik-solvers, motion-planning, atomic-actions, rl-learning, configclass-pattern, robot-workspace] status: active @@ -31,7 +38,8 @@ topics: title: Environment Framework aliases: [env framework, environment framework, task registration, list task, 环境框架, 任务环境] keywords: [BaseEnv, EmbodiedEnv, EnvCfg, register_env, list-task, run-env, target_control_frequency, - sim_steps_per_control, step_dt, environment.component, embodiment.component, ControllerAction, EnvProfiler] + sim_steps_per_control, step_dt, environment.component, embodiment.component, file-owned physics backend, + physics_config, ControllerAction, EnvProfiler, startup_summary, dexsim_startup_info] paths: [topics/env-framework/env-framework.md] source_of_truth: - embodichain/cli/main.py @@ -41,6 +49,7 @@ topics: - embodichain/lab/gym/utils/_component_composition.py - embodichain/lab/gym/utils/registration.py - embodichain/lab/gym/envs/base_env.py + - embodichain/lab/gym/envs/_startup_summary.py - embodichain/lab/gym/envs/embodied_env.py - embodichain/lab/gym/envs/demo.py - embodichain/lab/gym/utils/profiler.py @@ -91,21 +100,23 @@ topics: - id: robot-system title: Robot System aliases: [robot system, robot config, 机器人配置, 机器人系统] - keywords: [RobotCfg, control_parts, build_pk_serial_chain, DexforceW1Cfg, CobotMagicCfg, FrankaPandaCfg, - URRobotCfg, DualArmRobotCfg, merge_robot_cfg, JointDrivePropertiesCfg] + keywords: [RobotCfg, RobotPresetCfg, control_parts, build_pk_serial_chain, DexforceW1Cfg, CobotMagicCfg, + FrankaPandaCfg, URRobotCfg, DualArmRobotCfg, merge_robot_cfg, JointDrivePropertiesCfg, + asset_physics_mode, target_mode, mimic joint, quaternion, xyzw] paths: [topics/robot-system/robot-system.md] source_of_truth: - embodichain/lab/sim/objects/robot.py - embodichain/lab/sim/robots/ - - embodichain/lab/sim/cfg.py - watch_paths: [embodichain/lab/sim/robots/, embodichain/lab/sim/objects/robot.py, embodichain/lab/sim/cfg.py] + - embodichain/lab/sim/cfg/ + - embodichain/lab/gym/envs/embodied_env.py + watch_paths: [embodichain/lab/sim/robots/, embodichain/lab/sim/objects/robot.py, embodichain/lab/sim/cfg/] related_topics: [simulation-system, ik-solvers, motion-planning, sensor-system, sim-visualization, robot-workspace] status: active - id: sensor-system title: Sensor System aliases: [sensor system, camera attachment, 传感器, 相机挂载] - keywords: [CameraCfg, StereoCameraCfg, ContactSensorCfg, SensorCfg, get_sensor_obs, resolve_parent_nodes, - scatter_contact_data, tiled image] + keywords: [CameraCfg, StereoCameraCfg, ContactSensorCfg, ContactQuery, Newton contact, SensorCfg, + get_sensor_obs, resolve_parent_nodes, scatter_contact_data, tiled image, quaternion, xyzw] paths: [topics/sensor-system/sensor-system.md] source_of_truth: - embodichain/lab/sim/sensors/base_sensor.py @@ -122,7 +133,7 @@ topics: title: Simulation Visualization aliases: [browser visualization, viser, sim visualization, 仿真可视化] keywords: [VisualizationCfg, ViserServerCfg, SceneManifest, visualization, EntityGizmoManipulator, robot_ik_gizmo, - GizmoCfg, backpressure, capture_visualization] + GizmoCfg, backpressure, capture_visualization, Newton deformable, particle set, render topology, quaternion, xyzw] paths: [topics/sim-visualization/sim-visualization.md] source_of_truth: - embodichain/lab/visualization/protocol.py @@ -130,6 +141,7 @@ topics: - embodichain/lab/visualization/ - embodichain/lab/sim/sim_manager.py - embodichain/lab/sim/objects/gizmo.py + - embodichain/lab/sim/objects/deformable/ - embodichain/lab/scripts/preview_asset.py watch_paths: [embodichain/lab/visualization/, embodichain/lab/sim/objects/gizmo.py, embodichain/lab/sim/sim_manager.py, tests/visualization/] @@ -140,7 +152,7 @@ topics: aliases: [motion planning, trajectory planning, motion expansion, trajectory expansion, trajectory augmentation, fixed scene trajectory augmentation, 轨迹扩增, 运动规划, 轨迹规划] keywords: [BasePlanner, PlanState, PlanResult, MotionGenerator, ToppraPlanner, CuroboPlanner, NeuralPlanner, expansion, GenerationSession, TrajectoryAugmentationCfg, CandidateTrajectoryBatch, - collision world, compute trajectory, trajectory resampling, trajectory warping] + collision world, compute trajectory, trajectory resampling, trajectory warping, quaternion, xyzw, wxyz] paths: [topics/motion-planning/motion-planning.md] source_of_truth: - embodichain/lab/sim/motion/planners/base_planner.py @@ -163,7 +175,8 @@ topics: title: RL Learning Pipeline aliases: [rl learning, rl config, reinforcement learning, 强化学习, 训练配置] keywords: [train-rl, Trainer, trainer.gym_config, learning_env, RolloutKind, DifferentiableTrainer, - PPO, GRPO, APG, SyncCollector] + PPO, GRPO, APG, SyncCollector, ScheduledDifferentiableVecEnv, DifferentiableRolloutSpec, + complete rollout, full horizon, gradient accumulation, action adjoint, observation normalization] paths: [topics/rl-learning/rl-learning.md] source_of_truth: - embodichain/learning/rl/train.py @@ -172,12 +185,28 @@ topics: - embodichain/learning/rl/utils/trainer.py - embodichain/learning/rl/differentiable_trainer.py - embodichain/learning/rl/evaluation.py + - embodichain/learning/rl/gradients.py + - embodichain/learning/rl/normalization.py - embodichain/learning/rl/utils/config.py - embodichain/learning/rl/algo/ - embodichain_tasks/configs/tasks/ watch_paths: [embodichain/learning/rl/, embodichain_tasks/configs/tasks/, tests/learning/] related_topics: [simulation-system, env-framework, manager-functor, configclass-pattern] status: active +- id: differentiable-env + title: Differentiable Environment (APG) + aliases: [differentiable env, DifferentiableEnv, apg, analytic policy gradient, differentiable rl, + Warp tape autograd, NewtonStepFunc, 可微环境] + keywords: [differentiable, gradient, apg, autograd, warp tape, requires_grad, semi_implicit, + DifferentiableEnv, NewtonStepFunc, quaternion, xyzw] + paths: [topics/differentiable-env/differentiable-env.md] + source_of_truth: + - embodichain/lab/gym/envs/differentiable_env.py + - embodichain/lab/sim/cfg/simulation.py + - embodichain/lab/sim/diff/ + - embodichain_tasks/embodichain_tasks/special/franka_reach_apg.py + related_topics: [env-framework, rl-learning] + status: active - id: configclass-pattern title: Configclass Pattern aliases: [configclass pattern, 配置类] @@ -227,7 +256,7 @@ topics: title: Embodied Task Program aliases: [task program, semantic call, 任务程序, 语义调用] keywords: [TaskProgramCfg, program.yaml, integration.yaml, scene_binding, SceneManifest, SkillPolicyPreset, - EffectAssurance, CompiledTaskProgram, TaskProgramDemoBridge, execution_policy] + EffectAssurance, CompiledTaskProgram, TaskProgramDemoBridge, execution_policy, SemanticPose, quaternion_xyzw] paths: [topics/task-programs/task-programs.md] source_of_truth: - embodichain/lab/task_program/ diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 27c728519..fba5fd2ad 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -71,6 +71,12 @@ a physical effect. - [Planning and execution](execution.md): invocation resolution, control grid, row-local recovery and verification. - [Articulation geometry](articulation-geometry.md): topology/mesh ownership and directional affordance contracts. +The `scripts/tutorials/atomic_action/place.py` tutorial configures matching +Newton contact stiffness and damping on the cube and gripper collision links +before `SimulationManager.prepare()`. MuJoCo-Warp's default response is too +compliant for this force-closure replay and otherwise lets the cube slip near +its pickup pose instead of reaching the place target. + ## Semantic integration boundary `semantics` contains: diff --git a/agent_context/topics/differentiable-env/differentiable-env.md b/agent_context/topics/differentiable-env/differentiable-env.md new file mode 100644 index 000000000..4a14a4161 --- /dev/null +++ b/agent_context/topics/differentiable-env/differentiable-env.md @@ -0,0 +1,97 @@ +# differentiable-env + +> Topic: Newton-backed kinematic environments for analytic policy gradient +> (APG) through the Warp-tape ↔ PyTorch-autograd bridge. + +## Public entry point + +Use +`embodichain.lab.gym.envs.differentiable_env.DifferentiableEnv`. +It inherits `EmbodiedEnv`, preserves its scene lifecycle, and replaces the +normal physics step with a task-defined kinematics callback recorded on a +Warp tape. + +The resolution path is: + + DifferentiableEnv.step(action) + → NewtonStepFunc.apply(action, sim_state) + → _apply_action_kernel(action_wp, tape) + → _make_kinematic_step_fn()() + → _read_outputs(final_state) + → Warp tape backward → action.grad + +## Invariants + +- The configured physics backend must be `NewtonPhysicsCfg`. +- `NewtonPhysicsCfg.requires_grad` must be `True`. +- Default physics and other backends fail during `DifferentiableEnv` + construction. +- `DifferentiableEnv` always supplies a named kinematics callback; its bridge + contract has no dynamics mode, solver substeps, or control buffer. +- The environment never invokes the configured Newton solver or collision + pipeline. +- Newton gradient configuration still selects the semi-implicit solver, but + `DifferentiableEnv` never advances it. +- Gradient mode disables Newton CUDA graph capture. + +## Subclass contract + +Task authors implement three hooks: + +- `_apply_action_kernel(action_wp, tape)` launches Warp work that maps the + PyTorch action bridge array into task-owned kinematic state. +- `_make_kinematic_step_fn()` returns a zero-argument callback such as + `newton.eval_fk(...)`. The callback returns the state consumed by the output + hook. +- `_read_outputs(final_state)` returns `obs`, `reward`, `terminated`, and + `truncated`, plus `_order` and `_grad_track` metadata used by + `NewtonStepFunc`. + +There is no public `_apply_dynamics_action_kernel` or +`differentiable_step_mode` extension point. Differentiable dynamics are a +future feature, not a `DifferentiableEnv` capability. + +## Autograd and reset rules + +Action, kinematics, and gradient-producing output kernels must execute while +the Warp tape is open. Each tracked output names its backing Warp array in +`_grad_track`; an output mapped to `None` does not seed Warp backward. + +A grad-tracked terminal step returns the terminal observation and exposes +`requires_reset_after_backward` plus `deferred_reset_ids` in `info`. Reset +those rows only after backward. A no-grad terminal step resets them +synchronously. + +## Franka reference task + +`embodichain_tasks.special.franka_reach_apg.FrankaReachApgEnv` is the canonical +example. Its path is: + + action → new_joint_q → newton.eval_fk → body_q → reward kernel → action.grad + +The task snapshots live joint positions before opening the tape and writes the +detached next joint state back after the bridge returns. It does not exercise +Newton dynamics. + +## Dynamics boundary + +The public differentiable package exposes no solver stepper, trajectory, or +gradient-rollout API. Add those capabilities as a separate future design when +Newton dynamics are ready for end-to-end validation. + +## Source of truth + +- `embodichain/lab/gym/envs/differentiable_env.py` +- `embodichain/lab/sim/cfg/simulation.py` +- `embodichain/lab/sim/diff/` +- `embodichain_tasks/embodichain_tasks/special/franka_reach_apg.py` + +## Focused validation + +- `tests/gym/envs/test_differentiable_embodied_env.py` +- `tests/sim/test_sim_manager_cfg.py` + +## Related topics + +- env-framework +- rl-learning diff --git a/agent_context/topics/env-framework/configuration.md b/agent_context/topics/env-framework/configuration.md index 1d06e8628..d2493c18f 100644 --- a/agent_context/topics/env-framework/configuration.md +++ b/agent_context/topics/env-framework/configuration.md @@ -37,7 +37,7 @@ RL tasks sometimes drop the `-v` suffix (`CartPoleRL`, `PushCubeRL`). | File / component | Owns | Must not own | |---|---|---| -| Reusable physical `env.yaml` | Scene entities and ordinary environment values; `environment_id` identifies the environment | Runnable `id`, robot, sensor or Task Program fields | +| Reusable physical `env.yaml` | Exactly one `physics: default|newton` backend, its optional matching `physics_config`, scene entities, and ordinary environment values; `environment_id` identifies the environment | Runnable `id`, robot, sensor or Task Program fields | | Runnable `task..yaml` | Gym `id`, component selections, task-local run/deployment settings | Duplicate inline fields owned by selected components | | `configs/components/embodiments/*.yaml` | One robot, its sensors, optional Task Program-facing `skill_profile` | Task-local semantic scene binding | | `task_program/program.yaml` | Embodiment-independent program | Trusted runtime provider implementations | @@ -64,6 +64,21 @@ inline `robot`, `sensor`, and scene fields continue to parse unchanged. launcher arguments so environment-owned run controls such as `max_episodes` remain visible to the run loop while explicit CLI values retain precedence. +An inline runnable config must declare exactly one +`physics: default|newton` backend. A reusable environment component also owns +exactly one backend and its optional `physics_config`; the thin deployment +cannot repeat either field. `config_to_cfg()` constructs the backend-specific +typed physics config and rejects fields from the other backend. Launcher +`--physics` may confirm the declared value but cannot switch the file-owned +backend. Use separate environment files when one logical task needs both. + +Device selection is one shared runtime value. The typed physics config supplies +the backend default (`cpu` for Default, `cuda:0` for Newton), an optional +top-level Gym `device` overrides it, and an explicitly supplied CLI `--device` +wins last. Config-backed launchers leave `--device` unset by default, so +omission preserves the authored/backend value. `BaseEnv` tensors use the +manager's resolved device; there is no separate environment-device setting. + The component boundary is implemented in `gym/utils/_component_composition.py`. An embodiment's optional `skill_profile` is consumed only by a configured Task Program deployment. Scene components are @@ -77,8 +92,9 @@ configured Task Program deployments reuse that same embodiment. A simple supported Task Program does not require a task subclass. Its thin Gym deployment selects a reusable environment and embodiment and declares all -three Task Program component paths. The environment component owns only the -physical scene and ordinary environment values. After the generic resolver +three Task Program component paths. The environment component owns the +physical scene, one physics backend and its settings, and ordinary environment +values. After the generic resolver lowers the environment, robot, and sensors into the existing `EmbodiedEnvCfg` fields, it checks every semantic root's `simulation_uid` against the physical scene. The Task Program layer then checks diff --git a/agent_context/topics/env-framework/execution.md b/agent_context/topics/env-framework/execution.md index 5aad0290a..228c6015d 100644 --- a/agent_context/topics/env-framework/execution.md +++ b/agent_context/topics/env-framework/execution.md @@ -105,7 +105,7 @@ EmbodiedEnv.__init__(cfg) │ │ ├── _setup_robot() → Robot + single_action_space │ │ ├── _prepare_scene() → lights, background, objects │ │ └── _setup_sensors() → sensors dict - │ ├── init GPU physics (if CUDA) + │ ├── SimulationManager.prepare() (backend-neutral readiness boundary) │ ├── open window (if not headless) │ └── _init_sim_state() │ ├── _apply_functor_filter() (strip visual rand if configured) diff --git a/agent_context/topics/env-framework/profiling.md b/agent_context/topics/env-framework/profiling.md index 9b63c3432..c0651354f 100644 --- a/agent_context/topics/env-framework/profiling.md +++ b/agent_context/topics/env-framework/profiling.md @@ -70,3 +70,23 @@ minus measured children). It is also flushed automatically in `close()` **before parent; the first `warmup_steps` samples are discarded. --- + +## Startup information table + +`BaseEnv._setup_scene()` constructs `SimulationManager` with +`defer_startup_summary=True`, preserving the requested headless state while the +scene is assembled. `_log_initialization_summary()` reuses the shared +`sim/_startup_summary.py` simulation and scene rows, then adds seed, control +timing, episode limit, robot identity, metadata, and manager counts. +`EmbodiedEnv` keeps its later initialization-complete boundary so all managers +are available. Each environment emits the tables once and consumes the +simulation-owned startup and scene snapshots. + +`SimulationManagerCfg.startup_summary` accepts `compact`, `full`, and `off`. +Compact and full keep manager status/counts in the main table and append a +separate **Functor Details** table in configured execution order. Full also +shows qualified callable paths and parameters; large containers, tensors, and +arbitrary objects get bounded descriptions without evaluating functors or +transferring tensor data. `off` disables both tables. Gym JSON/YAML accepts +top-level `startup_summary` and `dexsim_startup_info` and forwards them through +`config_to_cfg()`. diff --git a/agent_context/topics/ik-solvers/ik-solvers.md b/agent_context/topics/ik-solvers/ik-solvers.md index bff1d9c56..1d10e3763 100644 --- a/agent_context/topics/ik-solvers/ik-solvers.md +++ b/agent_context/topics/ik-solvers/ik-solvers.md @@ -11,7 +11,7 @@ |---|---| | `embodichain/lab/sim/motion/solvers/__init__.py` | Public re-exports for all solver classes and configs | | `embodichain/lab/sim/motion/solvers/base_solver.py` | `BaseSolver` ABC + `SolverCfg` base config | -| `embodichain/lab/sim/cfg.py` | `RobotCfg.solver_cfg` — where solver config is wired into a robot | +| `embodichain/lab/sim/cfg/robot.py` | `RobotCfg.solver_cfg` — where solver config is wired into a robot | | `embodichain/lab/sim/motion/solvers/qpos_seed_sampler.py` | `QposSeedSampler` — random joint-seed generation | | `embodichain/lab/sim/motion/solvers/null_space_posture_task.py` | `NullSpacePostureTask` — Pink null-space posture objective | | `embodichain/lab/sim/utility/solver_utils.py` | Helpers: `create_pk_serial_chain`, `build_reduced_pinocchio_robot`, `validate_iteration_params`, `compute_pinocchio_fk` | diff --git a/agent_context/topics/motion-planning/collision-worlds.md b/agent_context/topics/motion-planning/collision-worlds.md index 74b57a62a..ab1c40695 100644 --- a/agent_context/topics/motion-planning/collision-worlds.md +++ b/agent_context/topics/motion-planning/collision-worlds.md @@ -4,6 +4,12 @@ Read this when the request needs these details. [Topic overview](motion-planning ### CuroboPlanner collision worlds +EmbodiChain planner inputs and robot FK results use `xyz + xyzw`. CuRobo's +native pose representation uses `xyz + wxyz`; `curobo_planner.py` and +`curobo_yaml.py` perform that conversion exactly once when constructing CuRobo +goals and obstacle YAML. Dynamic obstacle inputs expressed as homogeneous +matrices do not need a quaternion-order convention until that boundary. + `CuroboWorldCfg.rigid_objects` accepts either a mapping or a sequence. Use `Mapping[registry_id, RigidObject]` for a registry-backed integration. The mapping key is the authoritative logical/source obstacle ID used by the diff --git a/agent_context/topics/randomization/randomization.md b/agent_context/topics/randomization/randomization.md index 97115b813..8bc511335 100644 --- a/agent_context/topics/randomization/randomization.md +++ b/agent_context/topics/randomization/randomization.md @@ -25,12 +25,20 @@ The `__init__.py` of the randomization package re-exports everything via `from . | Function | Target | Key params | |---|---|---| -| `randomize_rigid_object_mass` | `RigidObject` mass | `mass_range`, `relative` | +| `randomize_rigid_object_mass` | Dynamic `RigidObject` mass/inertia | `mass_range`, `relative`, `recompute_inertia`, `min_mass` | | `randomize_rigid_object_center_of_mass` | `RigidObject` CoM offset | `com_pos_offset_range` | -| `randomize_articulation_mass` | `Articulation` link masses | `mass_range` (uniform or per-link dict), `link_names` (regex), `relative` | - -- `relative=True` adds sampled value to the initial/default mass instead of replacing. -- `randomize_articulation_mass` supports a `dict[str, tuple]` for per-link ranges; when used, `link_names` is ignored. +| `randomize_articulation_mass` | `Articulation` link mass/inertia | `mass_range` (uniform or per-link dict), `link_names` (regex), `relative`, `recompute_inertia`, `min_mass` | + +- `relative=True` adds the sampled value to the backend-resolved initial mass + stored in the target object's `default_mass` snapshot; repeated calls + therefore do not accumulate for either rigid objects or articulations. +- Rigid-object and articulation mass samples are clamped to positive + `min_mass`. By default, inertia is recomputed from the corresponding + initialization snapshot using the mass ratio; set `recompute_inertia=False` + only when inertia is managed separately. +- Non-dynamic rigid objects are skipped with a warning. +- `randomize_articulation_mass` supports a `dict[str, tuple]` for per-link + ranges; when used, `link_names` is ignored. - Link names are resolved via `resolve_matching_names` (regex matching). ### Visual (`visual.py`) @@ -121,12 +129,15 @@ Workspace-aware spatial sampling uses ### Sampling -Basic uniform randomizers use `embodichain.utils.math.sample_uniform(lower, upper, size)` for uniform sampling. +Randomizers use `embodichain.utils.math.sample_uniform(...)` for uniform +sampling where applicable. Physics samples are allocated on the target object's +device, not assumed to share `env.device`. ## Common Failure Modes | Symptom | Likely cause | |---|---| -| Randomizer silently does nothing | Check UID resolution and the selected randomizer: unsupported target types or no-op parameters can skip work | +| Randomizer silently does nothing | `entity_cfg.uid` not found in `sim.get_rigid_object_uid_list()` — all randomizers early-return on UID mismatch | +| Rigid-object mass is clamped | The sampled absolute mass or relative result was below positive `min_mass` | | `ValueError` on link name | `mass_range` dict key doesn't match any `articulation.link_names` | | Camera randomization error | Extrinsics config has neither `parent` nor `eye` set — unsupported mode | | Light randomization not per-env | By design: `randomize_light` applies same values across all envs | diff --git a/agent_context/topics/rl-learning/rl-learning.md b/agent_context/topics/rl-learning/rl-learning.md index 7f47bf712..08df2b086 100644 --- a/agent_context/topics/rl-learning/rl-learning.md +++ b/agent_context/topics/rl-learning/rl-learning.md @@ -83,6 +83,11 @@ as a mapping with `name` and `cfg`. `DifferentiableVecEnv.detach_state()` as the truncated-backpropagation boundary. +Variable-horizon APG environments may also implement +`ScheduledDifferentiableVecEnv.prepare_differentiable_rollout()`. Its +`DifferentiableRolloutSpec` selects the next reset's complete horizon, +per-environment objective scale, and scalar rollout metadata. + This path supports both standard algorithms and differentiable algorithms, but currently rejects distributed training and environment profiling. @@ -114,9 +119,11 @@ use it as padding. The collector writes into the preallocated rollout and the algorithm consumes it after collection. The differentiable path does not copy transitions into the standard buffer. -It preserves the action-to-reward autograd graph across short segments. -`segment_length` sets TBPTT boundaries, while `update_horizon` controls -how many environment steps contribute to one optimizer update. +It supports explicitly configured `segmented` TBPTT rollouts and `complete` +rollouts that reset before each independent microbatch, retain the full +environment-provided horizon, mask post-terminal rewards, and accumulate full +trajectory gradients before one optimizer step. Read the training detail for +the complete-rollout safety and normalization contracts. ## Component Ownership @@ -127,6 +134,8 @@ how many environment steps contribute to one optimizer update. | Standard rollout storage and views | `buffer/` | | Standard and differentiable collection | `collector/` | | Policy interface, actor-critic, actor-only, MLP builder | `models/` | +| Running observation statistics | `normalization.py` | +| Batched action-adjoint stabilization | `gradients.py` | | Standard collect/update loop | `utils/trainer.py` | | Differentiable TBPTT/update loop | `differentiable_trainer.py` | | Shared completed-episode evaluation | `evaluation.py` | @@ -151,6 +160,10 @@ distributed ownership, official examples and adding algorithms/policies/envs. - The standard buffer holds at most one unconsumed rollout. - APG must retain differentiable rewards until its optimizer boundary; `detach_state()` must not reset or resample the task. +- Complete APG must reset once per independent rollout, detach only after + backward, and exclude post-terminal auto-reset rewards from its objective. +- Observation statistics stay frozen during each complete rollout; semantic + mask/type fields remain unnormalized. - GRPO environment count must satisfy its grouping contract. - Evaluation must use completed episodes and an independent environment. - Only rank zero owns external logging and checkpoints in distributed runs. @@ -166,6 +179,8 @@ distributed ownership, official examples and adding algorithms/policies/envs. | Policy dimension mismatch | Policy config disagrees with the built environment's observation or action space | | Standard buffer is already full | A rollout was started before the previous one was consumed with `get()` | | APG gradients disappear | Actions were sampled under `no_grad`, transitions were copied/detached, or the state was detached too early | +| Long-horizon APG accuracy is lower than the reference | `rollout_mode` is still segmented, the scheduled horizon was truncated, return scaling is missing, or observation normalization differs | +| One environment poisons every APG row | Action-adjoint clipping is disabled or non-finite row filtering is bypassed | | GRPO reshape or grouping fails | `num_envs` is not divisible by `group_size` | | Evaluation never completes | The environment does not emit completed asynchronous episodes or terminal metrics correctly | | Output/checkpoint directories diverge across ranks | Distributed run metadata was not coordinated through rank zero | diff --git a/agent_context/topics/rl-learning/training.md b/agent_context/topics/rl-learning/training.md index b3473c72a..50a006719 100644 --- a/agent_context/topics/rl-learning/training.md +++ b/agent_context/topics/rl-learning/training.md @@ -17,8 +17,27 @@ Evaluation uses an independent environment and terminal metrics, temporarily switches the policy to evaluation mode, and restores its prior mode. -Checkpoints include policy parameters, trainer counters, best-evaluation -state, and optimizer or LR-scheduler state when present. +Checkpoints include policy parameters, trainer and complete-rollout counters, +best-evaluation state, observation-normalizer state when enabled, and optimizer +or LR-scheduler state when present. Evaluation reuses the frozen training +normalizer without updating its statistics. + +`trainer.seed` seeds Python, NumPy, Torch, CUDA, and Warp before environment +construction. Set `trainer.torch_deterministic: true` when a reference run +also requires deterministic PyTorch algorithms. + +Complete APG mode resets before every independent microbatch and preserves one +graph across the entire environment-provided horizon. It masks rewards after +the first done, applies the rollout's objective scale, and averages +`gradient_accumulation_steps` full trajectories before one optimizer step. It +never shortens a scheduled horizon to satisfy `total_timesteps`; without an +explicit timestep budget, CLI `iterations` maps to an exact optimizer-update +budget. + +Complete mode can clamp actions to the environment space and install a +per-environment action-adjoint norm hook. Non-finite adjoint rows are zeroed; +finite rows are clipped independently with an overflow-safe norm. APG also has +a pre-clip policy-gradient safety limit that skips unsafe updates. On the simulator path, distributed mode initializes NCCL, assigns one CUDA device per local rank, wraps the policy in @@ -63,7 +82,11 @@ example is an experimental gradient reference, not a general simulator task. 2. Register the factory with `@register_learning_env`. 3. Ensure finished rows auto-reset while returning terminal reward/done with the next initial observation. -4. Add an official config under +4. For variable complete APG rollouts, implement + `ScheduledDifferentiableVecEnv` and return the full non-truncated horizon. +5. Expose `observation_normalize_mask` when semantic dimensions must remain + raw during normalization. +6. Add an official config under `embodichain_tasks/configs/tasks///agents/` when it is a bundled task. diff --git a/agent_context/topics/robot-system/robot-system.md b/agent_context/topics/robot-system/robot-system.md index dba892316..71f8acd2d 100644 --- a/agent_context/topics/robot-system/robot-system.md +++ b/agent_context/topics/robot-system/robot-system.md @@ -5,10 +5,13 @@ | What | Path | |---|---| | Robot runtime class | `embodichain/lab/sim/objects/robot.py` → `Robot` | -| RobotCfg base config | `embodichain/lab/sim/cfg.py` → `RobotCfg` | -| ArticulationCfg parent | `embodichain/lab/sim/cfg.py` → `ArticulationCfg` | -| JointDrivePropertiesCfg | `embodichain/lab/sim/cfg.py` → `JointDrivePropertiesCfg` | +| RobotCfg base config | `embodichain/lab/sim/cfg/robot.py` → `RobotCfg` | +| Replace-only backend preset | `embodichain/lab/sim/cfg/robot.py` → `RobotPresetCfg` | +| Environment robot declaration | `embodichain/lab/gym/envs/embodied_env.py` → `EmbodiedEnvCfg.robot` | +| ArticulationCfg parent | `embodichain/lab/sim/cfg/articulation.py` → `ArticulationCfg` | +| Joint drive/dynamics config | `embodichain/lab/sim/cfg/articulation.py` → `JointDrivePropertiesCfg` | | Robot registry (all robots) | `embodichain/lab/sim/robots/__init__.py` | +| Robot executable smoke entry points | Each specified robot module's ``__main__`` block | | DexforceW1 config package | `embodichain/lab/sim/robots/dexforce_w1/` | | CobotMagic config | `embodichain/lab/sim/robots/cobotmagic.py` | | Solver APIs | `embodichain/lab/sim/motion/solvers/__init__.py` | @@ -26,6 +29,10 @@ A `Robot` is instantiated with a `RobotCfg` and a list of DexSim `Articulation` entities. +Robot FK/IK and end-effector pose APIs use the EmbodiChain convention: +quaternions are `xyzw`, and 7D poses are `xyz + xyzw`. Solver or planner +adapters convert only when their external library uses another order. + ## Motion and Workspace Integration - `Robot` imports solver APIs and runtime workspace types from @@ -49,11 +56,10 @@ Inheritance chain: ``` ObjectBaseCfg uid, init_pos, init_rot, init_local_pose - └─ ArticulationCfg fpath, drive_pros, attrs, link_attrs, fix_base, - │ disable_self_collision, enable_gravity, init_qpos, - │ body_scale, - │ build_pk_chain, use_usd_properties - └─ RobotCfg control_parts, urdf_cfg, solver_cfg, drive_pros (override default to "force") + └─ ArticulationCfg fpath, joint_drive_props, attrs, link_attrs, root_props, + │ init_qpos, qpos_limits, body_scale, build_pk_chain, + │ asset_physics_mode + └─ RobotCfg control_parts, urdf_cfg, solver_cfg, joint_drive_props (position+velocity force default) ├─ DexforceW1Cfg version, hand_versions, with_default_eef └─ CobotMagicCfg (dual-arm defaults) ``` @@ -65,8 +71,10 @@ Key fields on `RobotCfg`: | `control_parts` | `Dict[str, List[str]] \| None` | Part name → joint names (supports regex like `JOINT[1-6]`) | | `urdf_cfg` | `URDFCfg \| None` | Multi-component URDF assembly (e.g. left_arm + right_arm) | | `solver_cfg` | `SolverCfg \| Dict[str, SolverCfg] \| None` | IK solver config; dict keys must match `control_parts` keys | -| `drive_pros` | `JointDrivePropertiesCfg` | Default drive type is `"force"` (overrides Articulation's `"none"`) | -| `attrs` | `RigidBodyAttributesCfg` | Rigid-body physics attributes (mass, friction, damping, ...) | +| `joint_drive_props` | `JointDrivePropertiesCfg` | Single joint-property entry point for target mode, gains, effort/velocity limits, passive friction, and armature. Robot supplies the established `drive_type="force"`; unspecified fields remain source-owned | +| `asset_physics_mode` | `AssetPhysicsMode` | Robot defaults to `overlay`; generic articulations default to `preserve` | +| `attrs` | `RigidBodyPhysicsCfg` | Grouped rigid-body physics. Flat attribute keys are rejected; COM quaternions use `xyzw`. | +| `root_props` | `ArticulationRootPropertiesCfg` | Sole root-property interface. Fixed-base/self-collision are portable; root sleep and paired solver-iteration fields are Default-only | | variant fields | `enum \| str \| bool` | Optional subclass fields (e.g. `version`, `with_default_eef`) | | `_pk_urdf_path` | `property \| method → str` | URDF for the FK/IK serial chain (one source, so it can't drift from sim) | @@ -85,7 +93,8 @@ def from_dict(cls, init_dict): - **`_build_defaults(self, init_dict=None)`** — read variant fields from `init_dict`, set them on `self`, then populate `urdf_cfg`, `control_parts`, `solver_cfg`, - `drive_pros` and `attrs`. (Base `RobotCfg._build_defaults` is a no-op.) + `joint_drive_props` and `attrs`. (Base + `RobotCfg._build_defaults` is a no-op.) - **`build_pk_serial_chain(self, device=...)`** — return `{control_part: pk.SerialChain}`, reading the PK URDF from a single `_pk_urdf_path` source (a property for constant-path robots, a method when the path depends on a variant). @@ -99,6 +108,35 @@ restores it. Every config, including this exception, must satisfy `type(cfg).from_dict(cfg.to_dict())` without changing the selected components or applying a derived transform twice. +### Physics backend portability + +Keep backend-neutral intent in one ordinary `RobotCfg`. In particular, +`CollisionPropertiesCfg.contact_offset/rest_offset` compile directly to +Default and to Newton's `margin=rest_offset`, +`gap=contact_offset-rest_offset`. Use `DefaultCollisionPropertiesCfg` only as a +Default-native extension point; those two inherited fields are portable. +Default-only articulation sleep and solver iterations belong directly in +`ArticulationRootPropertiesCfg` under `root_props`; `sleep_threshold`, +`min_position_iters`, and `min_velocity_iters` no longer exist as flat +`ArticulationCfg` fields. EmbodiChain applies these values to the Default-native +articulation root before the first reset, while Newton ignores them. Use +`DefaultRigidBodyPropertiesCfg` under `attrs` or +`link_attrs` only when the intended target is an individual rigid body/link. +Keep portable rigid-body values and one selected backend subtype in the single +matching `RigidBodyPhysicsCfg` slot. Backend-specific whole-robot alternatives +belong in `RobotPresetCfg`; there are no coexisting per-property backend blocks. + +When a backend truly needs a different asset or complete actuator/physics +definition, subclass `RobotPresetCfg` and declare complete alternatives. The +required `default` field selects the Default backend and is the Newton fallback; +optional names include `newton`, `newton_mujoco_warp`/`newton_mjwarp`, and other +`newton_` profiles. `SimulationManager.add_robot()` selects from its +existing `physics_cfg` and active Newton solver, returns a deep-copied complete +`RobotCfg`, and never merges fields across alternatives. `EmbodiedEnvCfg.robot` +accepts either form and delegates selection to that same boundary. Prefer a +single portable `RobotCfg`; use a preset only for irreducible backend +differences. + W1 robot and hand releases use separate types and registries: - `DexforceW1Version` selects body/arm assets, kinematics, and flange calibration @@ -136,21 +174,82 @@ control_parts = { - `Robot.get_link_names(name)` returns child link names for a part. - Internal `ControlGroup` dataclass stores `joint_names`, `joint_ids`, `link_names` per part. +For Spawn-bound robots, control-part IDs are resolved by name against the +final batch `qpos` layout. Newton may use a different source-articulation +traversal order, so do not derive control-part IDs by enumerating native joint +names. `init_qpos` keeps its source-articulation order and is remapped by name +when the robot resets. + +### Mimic joints across physics backends + +`Articulation.mimic_ids` and `mimic_parents` use the final batch-state joint +order, just like `qpos`, `qvel`, and control-part IDs. Spawn source metadata is +normalized by joint name before these properties are exposed. Newton initial +positions are also projected onto each URDF relation +`child = multiplier * parent + offset` before the first simulation step. + +Newton's MuJoCo-Warp solver lowers URDF mimic joints to native joint equality +constraints, whose default solver reference is underdamped compared with +Default's PhysX mimic. For position-driven mimic parents, the Spawn-bound +articulation keeps those native equality rows enabled and tunes their MuJoCo +`solref` from the authored parent gains. A weak follower drive (1% of the +parent gains for the W1 hand) stabilizes the equality between solver updates, +and target `set_qpos()`/`set_qvel()` writes propagate the authored +`child = multiplier * parent + offset` relation. Never copy measured follower +state or disable the native equality: either change would turn mimic into an +independent servo and lose the mechanical coupling. Other Newton solvers, +gradient mode, and Default keep their native behavior. + ## Drive Properties -`JointDrivePropertiesCfg` controls the physics drive for joints: +`JointDrivePropertiesCfg` is the single joint-property config: | Field | Type | Default | Notes | |---|---|---|---| -| `drive_type` | `"force" \| "acceleration" \| "none"` | `"force"` (on RobotCfg) | `"none"` means no applied force | +| `drive_type` | `"force" \| "acceleration" \| "none"` | `"force"` (on RobotCfg) | Original drive response; active `"acceleration"` is Default-only | +| `target_mode` | `"none" \| "position" \| "velocity" \| "position_velocity" \| "effort"` or per-joint mapping | Derived from `drive_type` | Portable actuator intent; integer values 0–4 are accepted. `force` defaults to `position_velocity` | | `stiffness` | `float \| Dict[str, float]` | `1e4` | Per-joint via dict; keys support regex | | `damping` | `float \| Dict[str, float]` | `1e3` | Same | -| `max_effort` | `float \| Dict[str, float]` | `1e10` | Max torque/force | -| `max_velocity` | `float \| Dict[str, float]` | `1e10` | rad/s or m/s | -| `friction` | `float \| Dict[str, float]` | `0.0` | Joint friction | +| `max_effort` | `float \| Dict[str, float]` | `None` | Max torque/force | +| `max_velocity` | `float \| Dict[str, float]` | `None` | rad/s or m/s | +| `friction` | `float \| Dict[str, float]` | `None` | Passive joint friction | +| `armature` | `float \| Dict[str, float]` | `None` | Added joint-space inertia | When using a dict, keys are joint names or regex patterns matching joint names. Control-part names can also be used as keys (resolved via `ArticulationCfg` logic). +Target mode is backend-neutral and belongs directly on +`JointDrivePropertiesCfg`. Default emulates the target selection with its drive +mode and effective gains; Newton authors `JointTargetMode` values for +`"none"`, `"position"`, `"velocity"`, `"position_velocity"`, and +`"effort"` (integer values 0–4). `NewtonJointDrivePropertiesCfg` remains only +to round-trip older `joint_drive_props.backend: newton` dictionaries; do not use it in +new specified robots. + +`drive_type` retains its original meaning. With no explicit `target_mode`, +`force` and `acceleration` select `position_velocity`, while `none` selects a +passive target. An explicit target mode overrides that target default. Active +acceleration drives are rejected on Newton because Newton has no equivalent +mass-independent response. + +For solver-independent safety, `none` and `effort` clear Kp/Kd, while +`velocity` clears Kp. MuJoCo Warp consumes the target-mode enum natively. Other +Newton solvers use the gain fallback; their position-only fallback assumes the +velocity target remains zero. Direct generalized effort continues through +`Articulation.set_qf()` and can also act as feed-forward effort with an active +PD drive. + +These rules are resolved to exact joint names after URDF/USD source resolution +and before Spawn finalization. Common effort/velocity/armature values are +authored on `JointDesc`; the portable target intent lowers to Default drive +mode/gains and Newton's integer target mode. The dual-arm builder preserves the +config type and mirrors regex-keyed values to the generated `left_`/`right_` +names. + +`qpos_limits` accepts either joint-name/regex rules or a flattened +`(num_dofs, 2)` array. Both forms are resolved into common `JointDesc` +limits before the Default or Newton model is built; do not add a post-bind +Newton rebuild for initial limits. + ## Extension route Use `/add-robot` for configuration hooks, registry exports, docs and focused @@ -165,19 +264,28 @@ The authoritative inventory is `robots/__init__.py`; exported configs include: | Robot | Config Class | Module | Structure | Notes | |---|---|---|---|---| | DexForce W1 | `DexforceW1Cfg` | `embodichain/lab/sim/robots/dexforce_w1/` | Package (`cfg.py`, `types.py`, `specs.py`, `hand_specs.py`, `params.py`, `utils.py`) | Humanoid; robot and hand versions are independently registered | -| CobotMagic | `CobotMagicCfg` | `embodichain/lab/sim/robots/cobotmagic.py` | Single file | Dual-arm; 6-DOF arms + 2-DOF grippers; uses OPW solver | +| CobotMagic | `CobotMagicCfg` | `embodichain/lab/sim/robots/cobotmagic.py` | Single file | Dual-arm; 6-DOF arms + 2-DOF grippers; portable collision envelope, Default-native root iterations, OPW solver | | Franka Panda | `FrankaPandaCfg` | `embodichain/lab/sim/robots/franka_panda.py` | Single file | Panda preset | | Universal Robots | `URRobotCfg` | `embodichain/lab/sim/robots/ur_robot.py` | Single file | UR family presets | | Composed dual arm | `DualArmRobotCfg`, `build_dual_arm_cfg` | `embodichain/lab/sim/robots/dual_arm.py` | Single file | Reusable dual-arm assembly | +## Executable smoke programs + +Every specified robot module accepts ``--physics {default,newton}`` in its +``__main__`` smoke program and resolves the selection through +``physics_cfg_for_backend()``. CobotMagic, Franka, UR, and DualArm retain the +Default backend as their command-line default; DexforceW1 retains Newton as its +default. These entry points exercise the same ordinary ``RobotCfg`` definitions +on either backend rather than maintaining backend-specific demo configs. + ## Common Failure Modes - **`solver_cfg` keys don't match `control_parts` keys** — solver init silently uses wrong part or errors at IK time. - **Regex joint names not expanded** — if robot is not properly initialized, regex patterns like `JOINT[1-6]` remain unexpanded. Always construct via `from_dict()` or let `Robot.__init__` handle expansion. -- **`drive_type="none"` inherited from ArticulationCfg** — if you inherit `ArticulationCfg` directly instead of `RobotCfg`, the default drive type is `"none"` (no forces applied). Override to `"force"`. +- **No drive config on generic `ArticulationCfg`** — its `joint_drive_props=None` keeps source drives. Use `RobotCfg` for the standard position+velocity force-drive defaults, or provide an explicit sparse drive overlay. - **Missing `urdf_cfg` for multi-component robots** — single-file robots use `fpath`; multi-component robots (e.g. dual-arm) require `urdf_cfg` with component transforms. - **Mimic joints not excluded** — `get_joint_ids(remove_mimic=False)` includes mimic joints by default. Pass `remove_mimic=True` for active-only joints. -- **`init_qpos` shape mismatch** — must be `(num_joints,)`. A wrong-length array causes silent truncation or index errors at sim start. +- **`init_qpos` shape mismatch** — must match active DOFs. A wrong-length array causes initialization errors. - **`all` instead of `__all__`** — lowercase `all` does not work with `from module import *`; use `__all__`. - **`solver_cfg` set in multiple places** — set it once in `_build_defaults` only; setting it elsewhere (e.g. a build helper) gets overwritten and is dead code. - **PK URDF drifts from the sim URDF** — route `build_pk_serial_chain` through `_pk_urdf_path` and keep the DOF drift-guard test so silent drift is caught. diff --git a/agent_context/topics/sensor-system/sensor-system.md b/agent_context/topics/sensor-system/sensor-system.md index 266ae4d7f..70f18bdbd 100644 --- a/agent_context/topics/sensor-system/sensor-system.md +++ b/agent_context/topics/sensor-system/sensor-system.md @@ -43,7 +43,17 @@ BatchEntity |---|---|---|---|---| | Camera | `CameraCfg` | `"Camera"` | color, depth, mask, normal, position | Single RGB-D camera; configurable intrinsics/extrinsics | | StereoCamera | `StereoCameraCfg` | `"StereoCamera"` | color/depth/mask/normal/position (left + right), disparity | Extends Camera; adds right camera with baseline transform | -| ContactSensor | `ContactSensorCfg` | `"ContactSensor"` | contact data tensors | Collision detection between rigid bodies and articulation links; uses Warp kernels | +| ContactSensor | `ContactSensorCfg` | `"ContactSensor"` | contact data tensors | Backend-neutral rigid/link contacts on Default and Newton; uses DexSim `ContactQuery` plus a Warp scatter kernel | + +### Backend support + +Camera and stereo-camera creation are backend-neutral render features and are +supported with both Default and Newton physics. `ContactSensor` currently +depends on the Default backend's native contact-query path. The manager checks +`PhysicsBackend.supports_contact_sensor` and rejects it on Newton before sensor +construction with `NotImplementedError`; do not infer support merely because +Newton itself computes contacts. A Newton contact sensor requires an explicit +backend-neutral query adapter and parity tests before enabling that capability. ## Sensor Configuration @@ -54,7 +64,7 @@ Defines the sensor pose relative to its parent frame: | Field | Type | Default | Notes | |---|---|---|---| | `pos` | `Tuple[float, float, float]` | `(0, 0, 0)` | Position in parent frame | -| `quat` | `Tuple[float, float, float, float]` | `(1, 0, 0, 0)` | Orientation as `(w, x, y, z)` quaternion | +| `quat` | `Tuple[float, float, float, float]` | `(0, 0, 0, 1)` | Orientation as `(x, y, z, w)` quaternion | | `parent` | `str \| None` | `None` | Parent frame name (e.g. robot link); `None` = arena frame | The `transformation` property returns a `4×4 torch.Tensor` homogeneous matrix. @@ -79,6 +89,14 @@ only Task Program deployments additionally require the component's semantic ## Camera System +`Camera` and `StereoCamera` are created through +`SimulationManager.add_sensor()`. The owning manager is passed explicitly so +each camera resolves its World and ordered per-environment Arenas through that +manager even when multiple simulation managers are active. The manager also +owns semantic parent resolution and deferred attachment; cameras only attach +to concrete per-environment render nodes and report attachment after that +operation succeeds. + ### CameraCfg | Field | Type | Default | Notes | @@ -154,13 +172,59 @@ Properties `left_to_right` and `right_to_left` return `4×4` transform tensors. `ArticulationContactFilterCfg` specifies `articulation_uid` and `link_name_list` to filter which links report contacts. +### Contact query lifecycle + +Create contact sensors through `SimulationManager.add_sensor()`. The manager +crosses `prepare()` first and passes itself as the explicit owner. The sensor +resolves every configured UID to per-Arena Spawn handles and creates one +`spawn_result.create_contact_query(...)`: + +- `filter_need_both_actor=True` maps to `match="all"`; `False` maps to + `match="any"`. +- `max_contacts_per_env` is passed as the query's per-Arena quota, while the + total query capacity remains `num_envs * max_contacts_per_env`. This keeps a + busy Arena from consuming every row before other Arenas are represented. +- The query returns positions in each Arena frame and supplies an explicit + `env_ids` row for every contact. Environment assignment therefore works + when the selected actor is either actor 0 or actor 1 and when the other + actor is global. +- Query targets and actor IDs survive Newton topology rebuilds by semantic + Spawn path/link identity. +- `contact_capabilities` reports whether the backend supplies geometry, + normal impulse, and friction impulse. Newton MuJoCo-Warp provides all three; + other supported Newton rigid solvers may provide geometry only. DexUni does + not currently publish rigid contacts through `ContactQuery`. + +The existing TensorDict shape and field names remain stable. `user_ids` now +contains backend-neutral contact actor IDs rather than PhysX render user IDs; +resolve one with `ContactSensor.get_actor_info()`. `item_user_ids` contains the +IDs selected by the sensor, and `filter_by_user_ids()` accepts those same IDs. +Normals consistently point from `user_ids[..., 0]` toward +`user_ids[..., 1]`. Force-capable backends preserve every backend-emitted row +that passes the positive-impulse filter: Default CPU uses total impulse norm +greater than `1e-7`, while Direct GPU and force-reporting Newton solvers use +normal impulse greater than `1e-7`. Geometry-only Newton solvers retain all +candidate rows with zero impulse. The sensor does not synthesize a common +contact manifold; solver options such as MuJoCo-Warp's `enable_multiccd` +control how many points the backend emits for a geometry pair. Several contact +points and shape pairs may therefore map to the same actor pair. Only +`is_valid` and the per-environment counts are reset on each update, so values +in invalid fixed-buffer slots are unspecified. +PhysX Direct GPU does not identify static counterparts in its raw contact +buffer; those rows use actor ID `-1`. Monitor the dynamic/link side with +`filter_need_both_actor=False` when contacts against arbitrary static geometry +are required. Default CPU and Newton identify registered static shapes. + ## Common Failure Modes - **`sensor_type` string mismatch** — `SensorCfg.from_dict()` looks up `sensor_type + "Cfg"` in the sensors module. A typo (e.g. `"camera"` instead of `"Camera"`) causes `AttributeError`. - **Depth not enabled** — `enable_depth` defaults to `False`. Accessing depth data without enabling it returns empty tensors. -- **Invalid camera parent** — Missing or ambiguous registered links raise `ValueError`; missing per-arena links or render nodes raise `RuntimeError`. Parent nodes must resolve in every arena before attachment. +- **Invalid camera parent** — `OffsetCfg.parent` must match a link in a Spawn-bound robot or articulation. Missing or ambiguous registered links raise `ValueError`; missing per-arena links or render nodes raise `RuntimeError` during attachment or `SimulationManager.prepare()`. - **Stereo baseline sign** — `left_to_right_pos` defines translation from left to right camera. Flipping the sign inverts the disparity. - **Contact sensor buffer overflow** — `max_contacts_per_env` caps the contact count. Exceeding it silently drops contacts; increase if the scene has dense collisions. +- **Using native object user IDs with contact data** — `user_ids` is now a + query-local, backend-neutral actor identity. Use `get_actor_info()` or + `item_user_ids`, not `RigidObject.get_user_ids()`. - **View attribute flags** — `Camera.get_view_attrib()` computes `dr.ViewFlags` from enabled booleans. Adding a new data type requires both the `enable_*` flag and the corresponding `ViewFlags` bit. ## Contact computation ownership diff --git a/agent_context/topics/sim-visualization/native-gizmos.md b/agent_context/topics/sim-visualization/native-gizmos.md index cfd1134a1..f0026a40b 100644 --- a/agent_context/topics/sim-visualization/native-gizmos.md +++ b/agent_context/topics/sim-visualization/native-gizmos.md @@ -50,7 +50,7 @@ step ratio. CLI and task config loaders may override runtime fields before constructing the environment. Trace those overrides through the caller rather than changing a default in the manager blindly. -Object-specific configuration belongs in `lab/sim/cfg.py` or the +Object-specific configuration belongs in the relevant `lab/sim/cfg/` module or the corresponding robot/sensor module. Scene composition belongs in `EmbodiedEnv` or a task config, not in `SimulationManagerCfg`. diff --git a/agent_context/topics/sim-visualization/runtime.md b/agent_context/topics/sim-visualization/runtime.md index ca5f30a3d..7a845ccf1 100644 --- a/agent_context/topics/sim-visualization/runtime.md +++ b/agent_context/topics/sim-visualization/runtime.md @@ -22,7 +22,7 @@ Drawing markers and capturing visualization do not advance physics; interactive loops call `SimulationManager.update(step=1)` to process Gizmos and step the world. Manager add methods mark topology dirty for rigid objects, rigid-object groups, -soft bodies, cloth, robots, articulations, and `Camera` sensors. Supported +volume/surface deformables, robots, articulations, and `Camera` sensors. Supported `remove_asset()` branches do the same. The next simulation update refreshes the browser automatically. Use `refresh_visualization()` when the refresh must happen before another physics step. Code that changes mesh topology outside @@ -37,12 +37,14 @@ frames. Mesh geometry is identified by a SHA-256 hash of local vertices and faces. Static nodes sharing geometry are sent through one Viser batched-mesh handle. -Normal frames update only positions, `wxyz` quaternions, and visibility. +Normal frames update only positions, Viser-native `wxyz` quaternions, and visibility. Identifiers are URL-escaped before becoming Viser path components. -EmbodiChain pose vectors use `(x, y, z, qw, qx, qy, qz)`. The protocol uses -normalized `wxyz` quaternions. `pose_to_position_wxyz()` is the conversion -boundary and also accepts homogeneous `(..., 4, 4)` matrices. +EmbodiChain pose vectors use `(x, y, z, qx, qy, qz, qw)`. The visualization +protocol follows Viser and stores normalized `wxyz` quaternions. +`pose_to_position_wxyz()` converts EmbodiChain `xyz + xyzw` pose vectors at +that boundary and also accepts homogeneous `(..., 4, 4)` matrices. Protocol +fields already named `wxyz` remain protocol-native. Arena offsets are added to rigid, robot, articulation, and camera poses. Deformable vertices are stored relative to the corresponding arena node. @@ -55,8 +57,8 @@ Deformable vertices are stored relative to the corresponding arena node. | `RigidObjectGroup` | One node and pose per constituent object | | `Robot` | One mesh node per non-empty link | | `Articulation` | One mesh node per non-empty link | -| `SoftObject` | Live collision vertices with a cached convex-hull surface | -| `ClothObject` | Live physical vertices with render triangles mapped onto the welded physical vertex buffer | +| `VolumeDeformableObject` | Live Newton render-surface vertices and triangles | +| `SurfaceDeformableObject` | Live Newton render-surface vertices and triangles | | `Camera` | Frustum plus optional low-frequency RGB preview | | Default ground | 1000 m × 1000 m XY grid, 1 m cells, 10 m sections | | `SceneOverlays` | Frames, targets, trajectories, and point clouds | @@ -84,16 +86,22 @@ slow rendering or clients cannot accumulate an image backlog. ## Deformables -Soft bodies and cloth require GPU physics. Their live vertices are sampled at -`soft_body_fps`, independently from `scene_fps`. - -- DexSim does not expose soft-body collision triangle connectivity. - `SoftBodyData.collision_surface_triangles` therefore caches a SciPy - `ConvexHull` over rest collision vertices. The preview follows deformation - but cannot preserve concave render detail. -- Cloth maps all render-mesh triangles onto DexSim's welded rest-vertex buffer - with `cKDTree`. Construction raises `RuntimeError` if the mapping distance - exceeds the scale-relative tolerance. +Volume and surface deformables require the Newton backend, CUDA, and a +particle-capable solver. Their live vertices are sampled at `soft_body_fps`, +independently from `scene_fps`. `SceneExporter` reads the manager's single +deformable registry through `get_surface_vertices()` and +`get_surface_triangles()`; it does not branch on legacy buffer APIs. The facade +returns world-frame render vertices, and the exporter subtracts the arena +offset before publishing below the arena node. + +- Both kinds publish the live render surface from DexSim 0.5 typed Newton + particle-set handles. Visualization does no convex-hull reconstruction or + nearest-neighbor welding. +- A volume deformable separately exposes its tetrahedral collision surface via + `get_collision_surface_triangles()`; visualization intentionally uses render + topology. +- Spawn binding requires identical render vertex/triangle counts across + replicated instances. A clone topology mismatch fails during preparation. - Viser does not update mesh vertices in place. `ViserBackend` removes and recreates a deformable mesh handle only when a dynamic vertex sample arrives. Pose-only frames reuse the current handle. diff --git a/agent_context/topics/sim-visualization/sim-visualization.md b/agent_context/topics/sim-visualization/sim-visualization.md index 57fe15eba..92fc856b0 100644 --- a/agent_context/topics/sim-visualization/sim-visualization.md +++ b/agent_context/topics/sim-visualization/sim-visualization.md @@ -83,8 +83,7 @@ this invariant. | Startup timeout or address-in-use error | The Viser worker did not become ready or the configured port is occupied. Select another port and inspect `visualization_health.worker_error`. | | Asset added after startup is missing | Step once, call `refresh_visualization()`, or mark topology dirty if the change bypassed manager APIs. | | Browser stops updating after an exporter/backend exception | `capture_visualization_safely()` latches the first error to protect simulation. Inspect health/logs, then stop and restart after fixing the cause. | -| Soft body looks inflated or loses cavities | The surface is a collision-vertex convex hull, not the render topology. | -| Cloth construction raises a mapping error | Render vertices do not match the welded physical rest vertices within tolerance. | +| Replicated soft body fails with a render-vertex-count mismatch | DexSim produced clone render topology different from the source. Use one environment or a compatible mesh until replication preserves the topology. | | Camera frustum exists but preview is blank | Color capture is disabled, no image has been captured yet, or the selected camera/environment is hidden. | | Stereo/contact sensor is absent | Current camera export accepts only `sensor_type == "Camera"`; non-mesh sensors are not exported. | | Browser lags or upload cost is high | Reduce scene/image/deformable FPS, select fewer environments, or lower point-cloud limits. | diff --git a/agent_context/topics/simulation-system/simulation-system.md b/agent_context/topics/simulation-system/simulation-system.md index f62c19bdd..92c873ead 100644 --- a/agent_context/topics/simulation-system/simulation-system.md +++ b/agent_context/topics/simulation-system/simulation-system.md @@ -8,7 +8,9 @@ | Motion capability namespace | `embodichain/lab/sim/motion/__init__.py` | | World and scene owner | `embodichain/lab/sim/sim_manager.py` → `SimulationManager` | | Global simulation config | `embodichain/lab/sim/sim_manager.py` → `SimulationManagerCfg` | -| Object and physics configs | `embodichain/lab/sim/cfg.py` | +| Spawn lifecycle coordinator | `embodichain/lab/sim/spawn/scene.py` → `SpawnScene` | +| EmbodiChain-to-Spawn translation | `embodichain/lab/sim/spawn/descriptors.py` | +| Object and physics configs | `embodichain/lab/sim/cfg/` (public facade: `cfg/__init__.py`) | | Gym lifecycle integration | `embodichain/lab/gym/envs/base_env.py` | | Task scene construction | `embodichain/lab/gym/envs/embodied_env.py` | @@ -19,11 +21,33 @@ workspace, and trajectory-augmentation APIs live under `embodichain.lab.sim.moti ## Ownership -`SimulationManager` owns one DexSim `World`, its global environment, -parallel arenas, and the Python registries for scene resources: +`SimulationManager` owns one DexSim `World`, a `SpawnScene`, and the Python +registries for scene resources. DexSim's `SceneBuilder` and finalized `Scene` own +descriptor revisions, native materialization, replicated arenas, and backend +handles. `SimulationManager` owns the readiness boundary for each committed +Spawn topology revision. EmbodiChain registry objects are stable facades: +`add_*()` returns a declared facade and `prepare()` binds that same object in +place. Newton kinematic trajectory controls are registered through +`SimulationManager.register_kinematic_joint_trajectory()` and +`SimulationManager.register_kinematic_nodal_trajectory()`; the manager expands +the arena batch to concrete Spawn paths without exposing its private +`SpawnScene` or `SceneBuilder`. Nodal trajectories move configured inactive +deformable particles relative to their finalized initial positions at every +Newton substep. Time-varying rigid/articulation and scene-wide particle contact +properties use `register_contact_material_schedule()` and +`register_particle_contact_material_schedule()` at the same pre-`prepare()` +boundary. + +Backend-neutral contact access follows the same ownership boundary. +`ContactSensor` resolves configured logical UIDs through `SpawnScene.handles()` +after `prepare()`, then creates a DexSim `Scene.create_contact_query(...)`. +PhysX user IDs and Newton shape/body IDs are backend-binding details; neither +the sensor nor `SimulationManager` reads them directly. + +The registries cover: - rigid objects and rigid-object groups; -- soft and cloth objects; +- volume and surface deformables in one deformable-object registry; - articulations and robots; - rigid constraints, sensors, lights, gizmos, and markers; - visual materials and texture caches; @@ -42,9 +66,21 @@ The environment-owned lifecycle is: EnvCfg.sim_cfg → BaseEnv._setup_scene() → SimulationManager(SimulationManagerCfg) - → create World, global environment, defaults, and N arenas - → EmbodiedEnv adds robot, objects, lights, and sensors - → initialize GPU physics after scene construction when using CUDA + → create World and a replicated Spawn scene declaration + → EmbodiedEnv declares robot, objects, lights, and physical sensors + → Default may materialize native handles eagerly + → Newton keeps physical descriptors deferred + → optionally register Newton trajectory or contact-material controls on the manager + → SimulationManager.prepare() + → for Newton, resolve source metadata and configure exact-name overlays + → finalize/rebuild pending Spawn descriptors once + → for Default, apply pending source overlays to materialized handles + → apply Default articulation-root runtime properties to native handles + → delegate runtime-buffer preparation to the active PhysicsBackend + → bind declared EmbodiChain facades in place + → publish bound state through the backend render-sync hook + → attach parented cameras for the committed topology revision + → initialize metadata-dependent robot, action, and render-only resources → BaseEnv.step() → preprocess/apply action → SimulationManager.update(physics_dt, sim_steps_per_control) @@ -57,21 +93,172 @@ EnvCfg.sim_cfg → SimulationManager.destroy() ``` +`destroy(exit_process=False)` queues native cleanup; callers flush that queue +only after their scene/object locals have unwound. During +`SimulationManager._deferred_destroy()`, the manager stops recording and the +native window, invokes `PhysicsBackend.prepare_for_teardown()`, then runs GC +before closing the Spawn result, environment, and World. Default backends use +the no-op hook. Newton synchronizes its resolved Warp CUDA device and clears +its render bridge while Spawn still owns the parent skeletons, so cached link +views cannot be destructed after their native parents. + +After backend materialization, dynamic `RigidObject`, `Articulation`, and +`RigidObjectGroup` facades capture their resolved mass, inertia diagonal, and +local center-of-mass pose in their data objects. The layouts are `[env]` in +`RigidBodyData`, `[env, link]` in `ArticulationData`, and `[env, object]` in +`RigidBodyGroupData`. Each data object exposes current `mass`, `inertia`, and +`com_pose` values plus immutable `default_*` initialization snapshots. Runtime +property writes do not change these snapshots. During reset, only the selected +environment rows are restored before dynamics are cleared and the configured +pose is reapplied; reset-mode event functors then run from this clean physical +baseline in the episode-initialization hook. + +## Quaternion and pose convention + +All EmbodiChain-owned public and runtime quaternion tensors use +`(x, y, z, w)` (`xyzw`). A 7D pose or state therefore uses +`(px, py, pz, qx, qy, qz, qw)` (`xyz + xyzw`), and the identity quaternion is +`(0, 0, 0, 1)`. This includes object/root/link/COM state, robot FK and IK, +sensor offsets, manager observations/actions, semantic poses, and task +configuration. `embodichain.utils.math` follows the same convention. + +Backend and library adapters must preserve the external API's native order and +convert exactly once at that boundary. DexSim/Spawn rigid and articulation pose +buffers are native `xyzw + xyz`, so their adapters only permute pose layout. +DexSim mass-property and COM descriptors are native `wxyz`, so those adapters +use `convert_quat()` explicitly. Newton/Warp transforms expose position plus an +`xyzw` quaternion and therefore need no component-order conversion. Use a +non-symmetric rotation when testing an adapter; an identity or 180-degree +single-axis rotation can hide an incorrect order. + +Deformables use the same public hierarchy for both topologies: +`DeformableObjectCfg` is specialized by `VolumeDeformableObjectCfg` and +`SurfaceDeformableObjectCfg`. `objects/deformable/` owns the common +`DeformableObject`/`DeformableObjectData` contract and the Newton particle-set +volume and surface implementations. Both use one concrete `DeformableObjectData`. +Consumers should use `data.nodal_pos_w`, +`data.nodal_vel_w`, `data.nodal_state_w`, `get_surface_vertices()`, and +`get_surface_triangles()`. +At the Spawn boundary, volume and surface configs translate to DexSim's typed +`SoftBodyDesc` and `ClothDesc` particle-set descriptors; volume voxel +settings use `SoftBodyMeshingDesc`. +Deformable object configs use `attrs` (`VolumeDeformablePhysicsCfg` or +`SurfaceDeformablePhysicsCfg`) and volume-only `meshing` +(`VolumeDeformableMeshingCfg`). Both physics configs compose +`SurfaceElementPropertiesCfg` through `surface_props`. Volume elasticity +uses `youngs` and `poissons`; density remains kg/m³ for volumes +and kg/m² for surfaces. Surface coefficients default to zero for volumes and +None (Newton defaults) for cloth. `DeformableObjectCfg.from_dict()` owns nested +parsing; the rigid-specific `ObjectBaseCfg.attrs` parser must not decode these +physics groups. Only the current field names are accepted; no legacy aliases +or migration layer is maintained. + +`SimulationManager` stores both topologies once in `_deformable_objects` and +exposes `add/get_deformable_object()` for both topologies. +Only the Newton backend is registered; the Default backend intentionally +reports both deformable capabilities as unsupported. Declaration requires CUDA +and a particle-capable Newton solver (`xpbd`, `semi_implicit`, `vbd`, or +`dexuni`), rejects gradient mode and post-finalization additions, and compiles +directly to DexSim 0.5 `SoftBodyDesc` or `ClothDesc`. Runtime state is fetched +and applied through `Scene.create_particle_set_batch()`; direct DexSim +`SoftBody`/`ClothBody` buffers and the old utility loaders are not supported. +Volume collision topology comes from the typed particle-set handle, while +render topology and vertices remain a separate visualization surface. + `BaseEnv._setup_scene()` temporarily constructs the manager headlessly so the scene can be assembled before a native window is opened. It sets `SimulationManagerCfg.num_envs` from `EnvCfg.num_envs`. `SimulationManager` fixes the native world to explicit physics updates before -enabling physics or assembling scene resources. It creates the configured arenas, -installs default plane/background/lighting resources, and starts configured -visualization during initialization. A Viser backend -forces `headless=True`; Viser and the native DexSim window are mutually -exclusive. - -`SimulationManager.update()` initializes GPU physics lazily if needed and -then advances the world for the requested number of physics steps. Each -environment control step normally calls it with -`sim_steps_per_control`. +enabling physics, prepares the configured Arena layout, and owns a thin Spawn +scene coordinator. With the +Default backend, preparing the Arena layout lets `add_*` materialize native +entities immediately, so articulation metadata and render nodes are available +before finalization. A source-backed articulation added to an eager Default +result is loaded first and then receives its exact-name typed properties on the +live native articulation. Newton defers physical materialization until +`prepare()`: EmbodiChain first reads exact URDF metadata through a disposable +render-only skeleton, applies the source-name overlays, and then builds the +immutable Newton model once. A Viser backend forces `headless=True`; Viser and +the native DexSim window are mutually exclusive. + +`SpawnScene` always requests DexSim replication with +`collision_policy="isolated"`. Consequently, when `num_envs > 1`, all +per-environment dynamic, kinematic, and static rigid shapes and every +articulation link shape collide only with entities in the same Arena. Global +`per_env=False` physics resources still collide with every Arena. EmbodiChain +owns this policy choice; DexSim's `ReplicatePlan` and backend adapters own the +effective Default filter data and Newton collision groups. Do not duplicate +the backend-specific group calculation in object facades or task configs. + +The default ground plane authors its repeated texture coordinates in the Spawn +render descriptor before materialization, so native and offscreen render paths +receive identical UV data on their first GPU upload. + +`SimulationManager.prepare()` is the backend-neutral readiness boundary for +Default CPU, Direct GPU, and Newton. It is idempotent. Topology is committed +only when dirty. Newton source resolution and exact-name configuration precede +the first commit; Default source configuration follows native materialization. +A failed resolver or configurator remains pending and retryable. Runtime +preparation is recorded by committed topology revision and delegated through +`PhysicsBackend.prepare_spawn_runtime(result)`: Default CUDA initializes Direct +GPU buffers, while Default CPU and Newton use the base no-op. After facade +binding/reset, the active backend publishes current state through +`sync_render_state(result)` once per committed topology revision. This is a +no-op for Default and invokes Newton's render bridge without advancing +simulation time. Facade binding, render publication, and sensor attachment +remain retryable; already completed declarations are not reconfigured or +rebound. `init_gpu_physics()` and `finalize_newton_physics()` remain +compatibility aliases, but new code should call `prepare()`. + +`CameraCfg.extrinsics.parent` accepts either an unambiguous articulation link +name or `"/"`; the resolver returns the corresponding +public render node in every Arena. Cameras continue to use the manager-owned +`CameraGroup` plus one native camera view per Arena—attachment only reparents +those views, and extrinsics are local to the resolved link. + +`prepare()` records camera attachment completion by committed Spawn +`topology_revision`; a new revision reparents all configured cameras to the +rebuilt render nodes. The revision marker advances only after every attachment +succeeds, so a partial failure is retried on the next `prepare()`. The camera +registry remains the only source of attachment intent; there is no separate +pending list or physical dependency graph. `LightCfg` has no parent-attachment +contract in EmbodiChain. + +Standalone callers must call `prepare()` after their last `add_*()` and before +reading link/joint metadata, object state, or advancing physics. A Newton +caller that needs a substep-interpolated kinematic articulation must call +`register_kinematic_joint_trajectory(uid, joint_positions, ...)` after +declaring that robot/articulation and before `prepare()`. Its public trajectory +layout is `[num_envs, frames, dof]` in the articulation's public qpos order; +the optional root-pose layout is +`[num_envs, frames, 4, 4]`. `BaseEnv` provides the readiness boundary +automatically between `_setup_scene()` and metadata-dependent setup. +`SimulationManager.update()` still calls the readiness path defensively before +advancing the requested physics steps. + +A caller that needs selected deformable nodes to follow a kinematic path must +clear the Newton `ACTIVE` bit for those nodes through +`DeformableObjectCfg.particle_flags`, then call +`register_kinematic_nodal_trajectory(uid, node_indices, position_offsets, ...)` +before `prepare()`. Offsets use the batched layout +`[num_envs, samples, selected_nodes, 3]` and are relative to the world positions +captured when the finalized runtime control initializes. With `fps`, targets +are linearly interpolated at substep times; without it, each substep consumes +one sample. Surface indices can follow an array-backed source mesh directly. +Volume indices address the generated tetrahedral simulation particles and do +not correspond to source-mesh vertices. The host-side particle writes +intentionally disable CUDA Graph replay for that scene. + +Time-varying Newton shape contact properties belong on +`register_contact_material_schedule(uid, keyframes, ...)`; valid targets are a +declared rigid object, robot, or articulation. The manager creates one control +per Arena and accepts piecewise-constant `dynamic_friction`, `stiffness`, and +`damping` tracks. Scene-wide particle-versus-rigid values use +`register_particle_contact_material_schedule(keyframes)`. That control performs +host-side writes and disables CUDA Graph replay, while shape-material and joint +trajectory controls remain graph-compatible. Register all controls before +`prepare()` and never reach into `_spawn_scene.builder` from a task or demo. `scripts/tutorials/sim/gizmo_robot.py` supports only manual physics. It initializes GPU physics after robot creation when needed, sets both current and target @@ -82,19 +269,48 @@ calls `sim.update(step=1)`; the manager owns native IK updates and Viser commands/capture. The loop is paced by `physics_dt` and has no automatic physics polling path. -`ArticulationCfg.enable_gravity` defaults to `True`. During articulation -construction, `Articulation` applies this explicit runtime flag to every native -entity before the first physics update, including when -`use_usd_properties=True`. Use `Articulation.set_gravity(...)` to change the -flag later for all or selected environment indices. +Default object-level gravity uses +`attrs.rigid_props=DefaultRigidBodyPropertiesCfg(has_gravity=False)`. +`None` inherits source/backend intent; `True` enables world gravity. The same +field applies to every articulation/robot link through `attrs`, with +`link_attrs` providing more specific overrides. Source-backed assets require +`asset_physics_mode="overlay"`; preserve mode retains source physics. + +After `prepare()`, `RigidObject.set_gravity(enable, env_ids=None)` and +`Articulation.set_gravity(enable, env_ids=None, link_ids=None)` change selected +bodies without clearing velocity or replacing mass/inertia. Link indices use +`link_names` order, not joint-DOF order. Omitted selections mean all; invalid +indices are rejected before any write. Dynamic/kinematic rigid bodies and +articulations capture resolved gravity flags during binding and restore them +on reset for selected environments. Gravity control uses the DexSim Scene +handles, whose Default backend identifier is `"dexsim"`. + +Newton's current Scene integration does not implement object-level gravity: +explicit gravity overlays and runtime calls raise `NotImplementedError`. +Upstream MuJoCo's per-body `mujoco:gravcomp` is a solver-specific capability, +not an implemented cross-solver EmbodiChain contract. The manager passes a +Newton solver type to descriptor compilation only when Newton is active; +Default's TGS/PGS names are not Newton solver selections. + +Newton deformable demos use the current polymorphic rigid-property slots: +`attrs.collision_props=NewtonCollisionPropertiesCfg(...)` and +`attrs.material_props=NewtonRigidBodyMaterialCfg(...)`. The optional +`has_particle_collision` collision property preserves per-shape particle-contact +participation, including the W1 debugging toggle; arena isolation stays +scene-owned. The `auto` solver route accepts deformable declarations before +Spawn selects its concrete solver at finalization. ## Module Boundaries | Area | Owner | Routed topic | |------|-------|--------------| | World, arenas, asset registries, physics update, cleanup | `sim_manager.py` | `simulation-system` | -| Shared object, render, physics, drive, and URDF configs | `cfg.py` | `configclass-pattern` for config mechanics | -| Rigid, deformable, articulation, robot, light, constraint, gizmo | `objects/` | `robot-system` for robots | +| Backend activation and configured/resolved solver state | `physics/` | `simulation-system` | +| Spawn declaration, source resolution, commit/rebuild, and facade binding | `spawn/scene.py`, `spawn/source.py`, `spawn/descriptors.py` | `simulation-system` | +| Backend-neutral batched state/property access | `objects/backends/scene.py` | `simulation-system` | +| Shared object, render, physics, drive, and URDF configs | `cfg/` domain modules; `cfg/__init__.py` preserves the public import surface | `configclass-pattern` for config mechanics | +| Rigid, articulation, robot, light, constraint, gizmo | `objects/` | `robot-system` for robots | +| Common deformable contract and Newton particle-set adapters | `objects/deformable/` | `sim-visualization` for export | | Camera, stereo camera, contact sensor | `sensors/` | `sensor-system` | | Robot-specific configuration | `robots/` | `robot-system` | | Inverse kinematics | `motion/solvers/` | `ik-solvers` | @@ -109,6 +325,14 @@ Use the narrow topic when a request names one of these subsystems. Use `simulation-system` for the overall `lab/sim` architecture, manager lifecycle, scene ownership, or cross-module flow. +`SceneRigidBodyView.from_entities()` and +`SceneArticulationView.from_entities()` are the only object-layer owners of +DexSim's rigid-body and articulation batch-factory calls. Object facades use +those constructors and otherwise depend on the shared view contracts. +`RigidBodyData` and `ArticulationData` require a finalized `Scene`; the former +raw `PhysicsScene`/native-entity adapters and direct materialized-object +construction path have been removed. + `motion/__init__.py` resolves its public subpackages and `motion_generator` module lazily. Keep it free of eager planner imports: `Robot` needs solver and runtime workspace types during initialization, while planners resolve robots through @@ -141,11 +365,81 @@ asset registry and environment count, then coordinates creation and attachment. `qpos_joint_names`. Stochastic surface sampling and Atomic Action geometry keys do not belong to the simulation object; use `atomic_actions.sample_initial_articulation_geometry()` for that adaptation. - -Entity/IK gizmo configuration is owned by [native gizmos](../sim-visualization/native-gizmos.md). - ## Configuration Flow +`SimulationManagerCfg.physics_cfg` is the backend selector as well as the +backend config. `PhysicsBackendCfg` owns common timing, device, and gravity; +`DefaultPhysicsCfg` adds default-backend scene settings, while +`NewtonPhysicsCfg` adds the Newton solver, substeps, gradient/CUDA-graph +behavior, and an optional grouped `NewtonCollisionPipelineCfg`. Its +`update_interval` schedules external contact generation in both the ordinary +DexSim runtime and EmbodiChain's manual differentiable trajectory: `None` +updates at the first solver substep of each physics step, while `k >= 1` +updates at local substeps `0, k, 2k, ...`; intervening solver calls reuse the +most recent contact buffer. `collision_cfg=None` omits the external pipeline, +which the differentiable trajectory rejects because it requires contacts. +Do not add a second backend string that can disagree with the config type. +`physics_cfg.device` owns the backend's default (`cpu` for Default and +`cuda:0` for Newton). An explicit `SimulationManagerCfg.device` or legacy +`sim_device` value overrides it uniformly for either config type; omission +preserves it. Gym configs pass `device=None` when the field and CLI override +are both absent. Thus a config-backed launcher preserves the backend default, +while an explicit CPU override remains authoritative even for Newton. +Environment tensors derive from `sim.device`; do not introduce a second +tensor-device selector. +Leaving `NewtonPhysicsCfg.solver_cfg=None` preserves DexSim's +`AutoSolverCfg` default. A DexSim build exporting `AutoSolverCfg` is required; +EmbodiChain does not substitute a concrete solver. DexSim resolves that +placeholder from the complete Spawn scene during finalization: rigid-only +scenes select XPBD, scenes with an articulation select MuJoCo Warp, and +supported particle families select their matching particle/deformable solver. +A mapping with `solver_type: auto` or +`class_type: AutoSolverCfg` is the explicit equivalent. Gradient mode must +still select `semi_implicit` explicitly because AutoSolver does not choose a +differentiable solver. Before finalization, EmbodiChain treats `auto` as +unresolved; after finalization, `NewtonPhysicsBackend.solver_type` reads the +concrete type from DexSim's World-owned backend. +Explicit coupled articulation/deformable configurations use `dexuni` or +`DexUniSolverCfg`. DexUni owns collision detection and its particle-shape +contacts, so explicit configurations set `collision_cfg=None` instead of +tuning the external pipeline. +MuJoCo-Warp mappings may set `enable_multiccd: true`; EmbodiChain forwards it +to DexSim's `MJWarpSolverCfg`, which passes it to Newton `SolverMuJoCo`. +Enabling it changes contact generation (up to four contacts per geometry pair) +without changing the collision geometry authored by EmbodiChain. DexSim must +export an `MJWarpSolverCfg` version that declares the field. +The `open_drawer.py` tutorial combines this option with 20 Newton substeps per +10 ms control step, while keeping its authored robot gains, collision geometry, +pull trajectory, success criteria, and push trajectory identical to Default. +Atomic-action tutorials configure their shared Newton simulation in +`scripts/tutorials/atomic_action/tutorial_utils.py` with 10 solver substeps +per 10 ms physics step. They select `mujoco_warp` with +`use_mujoco_contacts=True` and set the external `collision_cfg` to `None`. +MuJoCo-Warp then generates and solves contacts internally at every solver +substep; external-pipeline settings such as `update_interval`, contact +reduction, and the external `rigid_contact_max` do not apply. DexSim derives +the native per-world contact capacity from the finalized scene, avoiding the +oversized fixed buffers formerly inherited from external-pipeline examples. +The shared factory leaves the Default backend configuration unchanged. +The package dependency must identify the exact DexSim dev build containing +this API; a base `==0.4.3` requirement also accepts older local-version wheels +that do not export `AutoSolverCfg` and is therefore insufficient. +Newton's `suppress_warp_kernel_logs=True` suppresses Warp's one-time runtime +banner plus module compile/load chatter during manager startup, build, facade +initialization, and physics updates, then restores the process-wide setting. +It does not suppress genuine Warp/Newton warnings and errors. Native startup +information is controlled separately by `dexsim_startup_info`. + +EmbodiChain-authored Newton collision shapes use a default margin and gap of +`0.001 m` each only when no portable or Newton-native envelope is authored. +`CollisionPropertiesCfg.contact_offset/rest_offset` are portable: Default uses +them directly, while the Spawn compiler maps `rest_offset → margin` and +`contact_offset - rest_offset → gap`. Both values must be present to derive a +Newton gap; an active Newton configuration rejects an ambiguous standalone +`contact_offset` unless a native margin or gap completes the intent. Explicit +`NewtonCollisionPropertiesCfg.margin/gap` values take precedence over this +translation. + ### Gizmo ownership Native entity manipulation belongs to DexSim 0.5.0. The first successful native @@ -190,9 +484,18 @@ selection, arena count and spacing, physics timestep, physics and GPU-memory settings, recording, profiling, and browser visualization. `EnvCfg` embeds `SimulationManagerCfg` and supplies the control-to-physics -step ratio. CLI and task config loaders may override runtime fields before -constructing the environment. Trace those overrides through the caller rather -than changing a default in the manager blindly. +step ratio. Gym configuration has no implicit physics backend: an inline +runnable config, or its selected reusable environment component, declares one +exact `physics: default|newton` value. The same file owns the optional flat +`physics_config`, and `config_to_cfg()` validates that mapping by constructing +the matching `DefaultPhysicsCfg` or `NewtonPhysicsCfg`. An environment +deployment cannot override component-owned physics fields, and launcher +`--physics` cannot switch the file-owned backend. Use separate environment +files for backend-specific settings. Other runtime fields may still be +overridden before constructing the environment. An omitted CLI `--device` +preserves the file/backend device, while an explicit value overrides it. Trace +those overrides through the caller rather than changing a manager default +blindly. `RenderCfg.apply_to_dexsim_config()` owns renderer, sampling, tone mapping, and `DLSSCfg` conversion into `WorldConfig`. DLSS settings apply to `hybrid`, @@ -215,20 +518,252 @@ and are rechecked before native conversion after mutable config edits. Focused coverage lives in `tests/sim/test_cfg.py`, `tests/sim/test_sim_manager.py`, and `tests/gym/utils/test_gym_utils.py`. -Object-specific configuration belongs in `lab/sim/cfg.py` or the -corresponding robot/sensor module. Scene composition belongs in +Object-specific configuration belongs in the matching `lab/sim/cfg/` domain +module or the corresponding robot/sensor module. Scene composition belongs in `EmbodiedEnv` or a task config, not in `SimulationManagerCfg`. -For mesh collision decomposition, `MeshCfg.acd_method` defaults to `"visacd"` -with DexSim 0.5.0; it requires CUDA support. `"coacd"` and `"vhacd"` remain -supported explicit options. +Deformable configs use an explicit `deformable_type: volume|surface` +discriminator. Common source mesh and pose fields stay on +`DeformableObjectCfg`; tetrahedral voxelization/soft-body attributes stay on +the volume subclass, and Newton triangle/edge/spring attributes stay on the +surface subclass. `particle_radius` and `validate_mesh` are common particle-set +options. `particle_flags` accepts one Newton bitmask or a +simulation-particle-order array; nodes driven by a kinematic nodal trajectory +must have their `ACTIVE` bit cleared before the immutable solver model is +constructed. `MeshCfg` accepts either a file path or explicit vertex/triangle +arrays. For surface deformables, use the array-backed form when flags or +trajectories require stable source node indices: native file importers may +reorder vertices, while array-backed descriptors preserve the supplied order +through Newton model construction. A surface deformable may additionally set +`visual_shape` to an independently indexed render mesh and choose +`visual_binding_mode="auto"` or `"nearest_vertex"`; physics always follows +`shape`, and visual material/UV configuration belongs on `visual_shape` when it +is present. This supports seam-duplicated textured meshes without changing +particle topology. Volume deformables are voxelized into a +separate tetrahedral simulation mesh, so their particle indices are not source +mesh indices. The volume descriptor converts Young's modulus and Poisson's +ratio to Newton Lamé coefficients. Removed Default-only fields are deliberately +not translated or silently ignored. Do not add backend conditionals to one +monolithic deformable config; a future implementation belongs at the manager +dispatch boundary. + +New rigid-body configs use `RigidBodyPhysicsCfg`. Portable intent is organized +by physical concept: + +- `mass_props`: `MassPropertiesCfg` (`mass`, `density`, inertia, COM, and the + source-inertia recomputation policy); +- `rigid_props`: `DefaultRigidBodyPropertiesCfg`; Newton currently exposes no + additional body-level property group beyond common mass properties; +- `collision_props`: common collision enablement and the portable + `contact_offset/rest_offset` envelope, optionally specialized by + `DefaultCollisionPropertiesCfg` or `NewtonCollisionPropertiesCfg`; +- `material_props`: common friction/restitution or a backend material subclass. + +Each concept has exactly one slot. Backend-native fields are represented by the +slot's concrete subclass or its local `backend: default|newton` discriminator; +`default_props` and `newton_props` were removed. Every grouped field defaults to +`None`, meaning “do not author this field”; source USD/URDF values and backend +defaults therefore survive partial overlays. Dynamic and kinematic mass +priority is explicit inertia with positive mass, then mass, then density; +static descriptors omit mass properties. + +Mesh collision construction is geometry-owned. `MeshCfg.collision` contains a +`MeshCollisionCfg` with an explicit `convex_hull`, `convex_decomposition`, +`triangle_mesh`, or `sdf` approximation. Strategy-specific fields are validated +when the config is constructed; numerical values never infer the strategy in +the canonical schema. Newton SDF and hydroelastic mesh settings share this +single owner. `RigidBodyPhysicsCfg` and articulation link overlays do not carry +mesh cooking. An imported articulation retains its source mesh approximation +until a named source-shape overlay API is introduced. + +`MassPropertiesCfg.recompute_inertia=True` discards source-authored inertia so +the backend derives it from collision geometry and the effective mass or +density. The default `None` inherits an outer per-body overlay and otherwise +preserves source inertia. Explicit inertia and recomputation are mutually +exclusive. The policy lives with mass properties so global articulation, +per-link articulation, and rigid USD overlays share the same behavior; +`LinkPhysicsOverrideCfg` only selects links and carries their partial `attrs`. +For a source-backed link with valid inertia, a positive `mass` can be changed +without changing that tensor; configuring `density` instead requires +`recompute_inertia=True`, because density necessarily derives a replacement +tensor. A joint-only overlay must not issue a link-physics write, since the +Default native setter otherwise derives geometry inertia even when no +mass-property field was configured. Conversely, an all-zero/invalid source +tensor is never retained: if source collision geometry exists, the shared +descriptor forces both backends through geometry-derived fallback properties. + +Polymorphic collision and material slots use a local +`backend: common|default|newton` discriminator; a unique native field may infer +the subtype. `rigid_props` currently accepts only `backend: default`. +`MeshCfg.from_dict()` temporarily normalizes the deprecated flat +`max_convex_hull_num`, `acd_method`, and `sdf_resolution` inputs to +`MeshCfg.collision` with a deprecation warning. `RigidObjectCfg.from_dict()` +also migrates the former `attrs.mesh_collision_props` input when it has the +owning mesh shape. Serialization emits only the new nested geometry form. + +`RigidBodyPhysicsCfg` is the only user-facing rigid-body physics schema. +Flat `attrs` keys such as `mass`, `dynamic_friction`, and `enable_collision` +are rejected at the parsing boundary; place them in `mass_props`, +`material_props`, or `collision_props` instead. `LinkPhysicsOverrideCfg.attrs` +uses the same partial schema, so global and per-link overlays share one model. +COM quaternions in every EmbodiChain config and public runtime API are `xyzw`. +The Spawn/Default adapter alone converts them to DexSim's native `wxyz` order. + +Robot configs normally keep these portable values on one ordinary `RobotCfg`. +For a genuine backend-specific asset or actuator difference, subclass +`RobotPresetCfg` and declare complete `default`, `newton`, or +`newton_` alternatives. +`SimulationManager.add_robot()` derives the +selection from its existing `physics_cfg`, deep-copies the selected complete +robot config, and never merges alternatives. While AutoSolver is unresolved, +only the generic `newton` and `default` alternatives are eligible; do not guess +a solver-specific preset before DexSim has inspected the complete scene. This +is the only robot preset selection boundary; do not add a second backend +selector to robot configs. + +File-backed rigid objects and articulations share one source-independent +physics policy: `asset_physics_mode="preserve"` keeps properties resolved from +the asset, while `asset_physics_mode="overlay"` applies only non-`None` +EmbodiChain fields after DexSim has translated the real materialized source. +This policy applies equally to USD rigid objects and USD/URDF articulations. +Generic `RigidObjectCfg` and `ArticulationCfg` default to `preserve`; `RobotCfg` +defaults to `overlay` to retain its established configured-drive behavior. +If an articulation in preserve mode contains explicit `attrs`, `link_attrs`, +`joint_drive_props`, or `qpos_limits`, configuration emits a warning +naming the ignored overlay fields instead of silently discarding them. +Import concerns that the source format does not author, such as URDF root +fixation and body scale, remain controlled by their dedicated fields. An +articulation defaults to `root_props.fixed_base=True` and +`root_props.self_collision_enabled=False`, so both URDF and USD assets are +fixed to the world with self-collision disabled unless configured otherwise. +Setting either field explicitly to `None` preserves the corresponding USD +property and selects the established URDF import default. + +`ArticulationRootPropertiesCfg` is the single root-property definition. Spawn +consumes its portable fixed-base and self-collision intent through common +articulation descriptor fields. Its `sleep_threshold`, `min_position_iters`, +and `min_velocity_iters` fields are Default-only: EmbodiChain applies them to +the materialized native articulation before Direct GPU initialization and the +first reset, while Newton ignores them. PhysX Direct GPU runtime setup captures +the articulation solver iteration counts; applying them only during facade +binding leaves the active GPU solver at its source/default values and can make +mimic constraints much softer than CPU. The preparation is idempotent per +Spawn topology revision. The two iteration counts must be configured together +because the Default native API exposes one atomic setter. This remains distinct +from `DefaultRigidBodyPropertiesCfg`, whose same-named values configure +individual rigid bodies or articulation links. `root_props` is the only +root-property interface; `fix_base`, `disable_self_collision`, and the former +flat root solver fields are removed. `JointDrivePropertiesCfg` keeps the +original `drive_type` (`force`, Default-only `acceleration`, or `none`) and adds +the portable actuator `target_mode` (`none`, `position`, `velocity`, +`position_velocity`, or `effort`), stiffness/damping gains, effort/velocity +limits, passive friction, and armature. `ArticulationCfg.joint_drive_props` is +the single joint-property entry point. Every field is optional; `None` means source-owned, +which permits sparse overlays without resetting unrelated source values. If +`target_mode` is unset, +`drive_type="force"` or `"acceleration"` defaults it to `position_velocity`, +while `drive_type="none"` defaults it to `none`. +`NewtonJointDrivePropertiesCfg` is only a serialized configuration +compatibility subtype; new robot definitions use the common class. Common +effort/velocity/armature values stay on `JointDesc` instead of being duplicated +in both backend blocks. `link_attrs` accepts the same grouped rigid-body schema +for partial per-link overrides. + +Spawn resolves drive intent per source-resolved joint before lowering it. +Default selects its force/acceleration enum and masks inactive gains; Newton +authors `JointTargetMode` values 0 through 4. `none` and `effort` always clear +both target gains, and `velocity` clears the position gain, so Newton solvers +that ignore `joint_target_mode` still receive deterministic passive, +effort-only, and velocity-only behavior. Non-MuJoCo Newton position mode is an +explicit gain-based emulation that assumes a zero velocity target. An active +`drive_type="acceleration"` is rejected for Newton because it has no exact +equivalent. + +For articulations, `SimulationManager._declare_spawn_articulation()` supplies +`configure_articulation_desc()` as the source-configuration callback. Preserve +mode leaves source descriptors untouched, while overlay mode applies +exact-name link/joint fields. Both regex dictionaries and flattened +`(num_dofs, 2)` arrays in `qpos_limits` are compiled into the resolved +joint descriptors before either backend builds. Default obtains those names +from its loaded native articulation and applies the typed properties live. +Newton resolves the same metadata first and consumes the configured descriptor +during its initial immutable-model build, so initial source configuration must +not be implemented as finalize-then-rebuild. Do not duplicate these link/joint +descriptor writes in `Articulation._apply_spawn_config()`; that hook is +reserved for Default-native root setters, finalized Newton runtime adaptation, +and render work requiring finalized resources. + +Scene articulation state IDs follow the final batch `qpos`/`qvel` layout, which +can differ from Newton's source-articulation traversal order. Initial `qpos`, +mimic child/parent metadata, control groups, and every batch mutation must be +mapped by joint name into that state layout. Public `joint_names` uses this +same state-buffer order; use the Spawn handle's source-name query only when +resolving source topology. Newton solvers without configured mimic compliance +project reset positions onto the authored relation before the first step. The +MuJoCo-Warp compliance path preserves the authored current position, matching +Default's initial hand state. +`SceneArticulationView` filters Newton root-pose rows that already match the +requested translation and rotation before calling the Scene batch write. This +keeps ordinary fixed-root resets from invalidating a captured CUDA graph while +still forwarding genuine root-pose changes, which refresh Newton solver +constants and recapture the graph as required. +Initialization code that intentionally changes fixed-root poses should do so +after `prepare()` but before the first `update()`, allowing the first Newton +CUDA graph to capture the final anchors instead of immediately invalidating a +graph captured from transient poses. + +MuJoCo-Warp lowers URDF mimic joints to native joint equality constraints, but +its default equality solver reference is underdamped compared with Default's +PhysX mimic. During +`Articulation._apply_spawn_config()`, +`_configure_newton_mimic_compliance()` in `objects/backends/newton.py` resolves +only that articulation's constraint rows and approximates Default's natural +frequency/damping ratio with MuJoCo's positive, effective-mass-scaled +`(timeconst, dampratio)` `solref`; the time constant observes MuJoCo's +two-solver-timestep safety floor. The native rows remain enabled, preserving +contact force coupling between follower and leader joints. A very weak +follower drive (one percent of its leader's target gains; `ke=1`, `kd=0.1` +for the W1 hand) stabilizes the equality between solver updates; target +`set_qpos()` and `set_qvel()` writes propagate the authored leader relation to +that drive. Never copy measured follower state or disable +the native equality: doing either turns mimic into an independent servo and +loses the Default backend's mechanical coupling. Other Newton solvers, +gradient mode, and Default retain native behavior. Keep private Newton +runtime/solver access inside this backend helper; the generic `Articulation` +owns state-order metadata, reset behavior, and target propagation only. + +DexSim 0.4.3's Newton `RigidBodyBatch.apply_pose()` writes maximal `body_q` +state but does not update the standalone body's reduced FREE-joint state read +by MuJoCo-Warp on the next step. `SceneRigidBodyView` therefore caches the +selection returned by the Newton-specific hook in `objects/backends/newton.py` +and projects both Newton state buffers after pose or velocity writes. Invalidate +that cache on a Scene topology revision; remove the compatibility path once +DexSim's public batch operation guarantees the same synchronization. + +The `grasp_cup_to_caffe.py` comparison demo seeds its XY perturbations after +`prepare()` (default seed `0`). This placement makes the scene independent of +random numbers consumed by backend initialization. Pass a negative `--seed` +to restore non-deterministic perturbations. + +Rigid USD objects follow the same overlay rule: parsed source descriptors are +updated field-by-field, never replaced wholesale by a partial config. The +former flat `RigidBodyAttributesCfg` and `RigidBodyAttributesOverrideCfg` +types have been removed. New and migrated definitions use the grouped schema, +where `None` means “leave the source/backend value unchanged.” + +Entity/IK gizmo configuration is owned by [native gizmos](../sim-visualization/native-gizmos.md). ## Where to Make Changes | Change | Primary location | |--------|------------------| | Global world, renderer, device, arena, or physics lifecycle | `sim_manager.py` | -| Shared object or physics config type | `cfg.py` | +| Spawn source translation or typed link/joint overrides | `spawn/descriptors.py` plus the DexSim Spawn descriptor/adapter boundary | +| Declaration-to-result binding or retry behavior | `spawn/scene.py` and the object's `bind_spawn()` | +| Batched row/DOF selection or backend property parity | `objects/backends/scene.py` and the DexSim Scene batch facade | +| Newton object/runtime adaptation | `objects/backends/newton.py` | +| Shared object or physics config type | Matching domain module under `cfg/`, then re-export from `cfg/__init__.py` | +| Deformable nodal/surface contract or topology-specific buffers | `objects/deformable/` | | Add/get/remove behavior for a scene entity | `sim_manager.py` plus its `objects/` implementation | | Task scene composition | `embodied_env.py` or the task config | | Environment timing, reset, or control-step behavior | `base_env.py` and `env-framework` | @@ -239,23 +774,66 @@ supported explicit options. - Configure `num_envs`, device, renderer, and physics settings before constructing `SimulationManager`. +- Express manager-level backend differences through `PhysicsBackend` hooks, + runtime properties, or `supports_*` capabilities. Use `physics.name` only for + diagnostics, dispatch registries, and the public compatibility predicates. +- Keep unsupported features explicit: Default owns native rigid constraints and + `ContactSensor`; Newton currently advertises neither. Both backends support + rigid-object groups, articulations, and robots. +- Treat `add_*()` as declaration. Call `prepare()` before consuming native + handles, link/joint metadata, batched state, or physics results. +- Keep `prepare()` convergent and retryable: do not mark a declaration bound + until its full facade construction succeeds. +- Keep backend render-state publication free of physics steps. Newton's initial + state sync must not advance its simulation step or time. - Treat resource UIDs as registry identities; retrieve and mutate resources through the manager instead of maintaining a parallel scene registry. - Keep batched object and sensor state aligned with the manager's arena count. -- Build scene assets before explicitly initializing GPU physics. The manager - will warn and initialize lazily on the first update if this was missed. -- Physics advances through explicit `SimulationManager.update()` calls. - Interactive loops own their fixed physics timestep and optional wall-clock - pacing. +- Create deformables only with the Newton backend on CUDA and a supported + particle solver; Default-backend soft/cloth compatibility is intentionally + absent. +- Treat deformable render meshes and physical particle topology as distinct; + state mutation uses the particle batch and never writes renderer/native + soft-body buffers directly. +- Add the initial physical scene before `prepare()`. Calls to the legacy + `init_gpu_physics()` and `finalize_newton_physics()` aliases are equivalent to + `prepare()` and do not cause a second build. +- Register Newton kinematic joint trajectories through + `SimulationManager.register_kinematic_joint_trajectory()` before `prepare()`; + callers must not access `SimulationManager._spawn_scene` or its builder. +- Register deformable kinematic-node trajectories through + `SimulationManager.register_kinematic_nodal_trajectory()` before `prepare()`; + mark every selected node inactive in `particle_flags` before model + construction, and keep Spawn/runtime-control access inside the manager. +- Register rigid/articulation contact schedules through + `SimulationManager.register_contact_material_schedule()` and scene-wide + particle schedules through + `SimulationManager.register_particle_contact_material_schedule()` before + `prepare()`; account for the latter disabling CUDA Graph replay. +- Delegate environment and DOF selections to DexSim Scene batches instead of + full-batch read/modify/write loops in object facades. +- Newton descriptor or topology mutations that cannot update the immutable + runtime model live remain pending until the next `prepare()` rebuild. +- Apply Newton collision and articulation-joint configuration to the + source-translated Spawn descriptors before the first model build; post-bind + object initialization is only for state and supported live batch properties. +- Manual update is the default; normal environment stepping must advance + physics through `SimulationManager.update()`. - Drawing markers and publishing visualization do not advance physics. Use `capture_visualization(force=True)` to publish marker edits while paused. - Reset only the requested environment rows and honor `excluded_uids` for resources detached from automatic reset. +- Keep the `default_mass`, `default_inertia`, and `default_com_pose` values in + `RigidBodyData`, `ArticulationData`, and `RigidBodyGroupData` as immutable + initialization snapshots; runtime setters and randomizers must not mutate + them. - `destroy()` queues deferred cleanup. Tests and non-exiting standalone callers that use `exit_process=False` must call `SimulationManager.flush_cleanup_queue()`. - Resolve articulation ancestry through `get_parent_joint_chain()`; keep DexSim topology access encapsulated by `Articulation`. +- Resolve rigid contacts through the Spawn result's `ContactQuery`; do not add + a second PhysicsScene/Newton contact path in `SimulationManager` or sensors. - Keep articulation mesh access, FK, and topology domain-neutral. Perform affordance sampling and semantic-key conversion in the Atomic Action adapter. @@ -264,9 +842,51 @@ supported explicit options. | Symptom | Likely cause | |---------|--------------| | Scene resource cannot be found or the wrong object is returned | UID mismatch or code bypassed the manager registry | -| CUDA physics data is stale on the first step | GPU physics was initialized before all assets were added, or not initialized explicitly | +| Link/joint metadata is empty or state access fails after `add_*()` | The declared facade has not crossed `SimulationManager.prepare()` yet | +| CUDA/Newton physics data is stale after a topology or descriptor mutation | Call `prepare()` so the dirty Spawn result can rebuild and rebind runtime views | +| Adding a soft or cloth object fails immediately on the Default backend | Deformables are Newton-only; select a supported Newton particle solver on CUDA | +| A replicated file-backed soft body fails render upload because clone vertex counts differ | DexSim's cloned render mesh does not match the template embedding topology; use a compatible mesh/single environment while the DexSim 0.5 clone path is corrected | +| Warp module compile/load lines appear during Newton initialization | `NewtonPhysicsCfg.suppress_warp_kernel_logs` was explicitly disabled, or compilation happened outside the managed preparation scope | | Native window does not open | `headless=True`, often forced by the Viser backend | -| Device and renderer use the wrong GPU | `sim_device` and `gpu_id` disagree; the device index takes precedence for CUDA simulation | +| Device and renderer use the wrong GPU | `device`/legacy `sim_device` selects physics and tensor execution, while `gpu_id` selects the render GPU and fills an unindexed CUDA device; make indexed values agree when both are explicit | | Simulation advances at the wrong control rate | `physics_dt` and `sim_steps_per_control` were configured inconsistently; see `env-framework` | | A test leaks a DexSim world | `destroy(exit_process=False)` was called without flushing the cleanup queue | | Python exits during cleanup | `destroy()` used its process-exit default; pass `exit_process=False` for embedded or test lifecycles | + +## Startup summaries + +`SimulationManagerCfg.startup_summary` selects `compact` (default), `full`, or +`off`. `dexsim_startup_info=False` is forwarded to DexSim's +`WorldConfig.log_startup_info` before World construction; the matching DexSim +build is required. This hides only native startup information, not warnings or +errors. The switch is independent of Newton's Warp kernel-log suppression. + +`sim/_startup_summary.py` owns read-only row collection and PrettyTable output. +A standalone manager emits its engine table after construction, then one scene +snapshot after its first successful update, camera render, or native-window +open with a prepared scene. When Newton CUDA Graph capture is pending, opening +the native window or rendering cameras does not consume that snapshot; the +first successful physics update emits it after capture resolves to `CAPTURED` +or `DISABLED`. `prepare()` does not print, and neither snapshot steps or +finalizes the scene. The readiness revision is invalidated at each `prepare()` +entry and published only after every preparation stage succeeds; a finalized +but unprepared or dirty scene cannot emit `READY`. An owning Gym environment +passes `defer_startup_summary=True` and emits one combined table at its existing +initialization-complete boundary. Reset/step do not repeat the tables. + +Render and compute devices, native-window state and browser visualization are +separate fields. Closed windows do not imply disabled offscreen rendering. +Environment collision isolation appears as `Physics / Collision policy`; +the compact table does not include an `External collisions` row. +The Default display is `Default` with `TGS` or `PGS`, read from the native +`enable_tgs` setting. Newton displays requested versus resolved solver and +reads `NewtonBackend.cuda_graph_status` through the physics adapter; pending +capture is never labelled captured. Scene counts are per environment and +exclude the separately identified global ground. Full mode adds rendering, +backend and system diagnostics; compact mode omits thread and stepping rows. + +Device names reuse the metadata already enumerated by Warp startup and never +initialize PyTorch CUDA for diagnostics. + +Summary colors are enabled only on terminals without `NO_COLOR`; the default +console handler also strips ANSI escapes from redirected ordinary logs. diff --git a/agent_context/topics/task-programs/configuration.md b/agent_context/topics/task-programs/configuration.md index fbf99ffa5..55fbe6da5 100644 --- a/agent_context/topics/task-programs/configuration.md +++ b/agent_context/topics/task-programs/configuration.md @@ -20,6 +20,12 @@ Unknown fields, duplicate keys, non-finite values, invalid exact types, excessive depth/nodes/repeats, cyclic or executable registered payloads, and unresolved references fail before live providers are touched. +Task Program poses follow the EmbodiChain quaternion contract. `PoseCfg` and +serialized targets require `quaternion_xyzw`; `SemanticPose` stores and reports +the same order. The configured hand-over service uses +`final_quaternion_xyzw`. Identity is `[0, 0, 0, 1]`; legacy `*_wxyz` keys are +unknown fields and are rejected rather than silently reinterpreted. + `TaskProgramCompiler` resolves canonical scene references, expands bounded repeats and cyclic targets, assigns stable segment/call indices, preserves parallel branches, and returns an immutable `CompiledTaskProgram`. diff --git a/design/newton-backend-design.md b/design/newton-backend-design.md new file mode 100644 index 000000000..2ac748bc1 --- /dev/null +++ b/design/newton-backend-design.md @@ -0,0 +1,465 @@ +# EmbodiChain Newton Backend Integration Design + +> **Status:** Supplementary implementation record, not a normative API +> specification. For current contracts use +> `agent_context/topics/simulation-system/simulation-system.md` and the Sphinx +> simulation guides; code and tests remain the source of truth when this record +> lags them. Dated files under `docs/superpowers/` are historical plans/specs. + +This document summarizes the EmbodiChain integration state for the DexSim +Newton physics backend and records remaining work. + +Use these EmbodiChain backend names consistently: + +- `default`: the existing DexSim default physics backend. +- `newton`: the DexSim Newton physics backend. + +Avoid exposing lower-level DexSim implementation names in EmbodiChain-facing +configuration, docs, and conditionals. + +## Current State + +### Configuration + +Backend selection is inferred from `SimulationManagerCfg.physics_cfg`: + +- `DefaultPhysicsCfg` selects the `default` backend. +- `NewtonPhysicsCfg` selects the `newton` backend. +- `physics_cfg_for_backend("default" | "newton")` returns the matching config. +- `physics_backend_from_cfg(...)` maps a config instance to its backend name. + +`DefaultPhysicsCfg` owns default-backend settings and GPU-memory settings. +`NewtonPhysicsCfg` owns Newton settings: `physics_dt`, `device`, `num_substeps`, +`requires_grad`, `use_cuda_graph`, `debug_mode`, `solver_cfg` (mapping or +`NewtonSolverCfg` selecting `mujoco_warp` / `xpbd` / `semi_implicit` / +`featherstone` / `vbd`), `broad_phase`, and `visualizer_enabled`. +`NewtonPhysicsCfg.to_dexsim_cfg(...)` builds a DexSim `NewtonCfg`, disables +CUDA graph when gradient mode is enabled, and requires +`solver_type="semi_implicit"` for gradient mode. + +The typed physics config owns its device default (`cpu` for Default and +`cuda:0` for Newton). `SimulationManagerCfg(device=...)` and legacy +`sim_device=...` are explicit, backend-neutral overrides; omission preserves +the typed config value. Config-backed CLI launchers likewise preserve the +file/backend device unless `--device` is supplied, including honoring an +explicit Newton CPU selection. + +### PhysicsBackend abstraction + +`SimulationManager` delegates backend-specific behavior to a +`PhysicsBackend` instance held as `self.physics` (selected by `physics_cfg` +type via `physics_backend_from_cfg`). The backend package lives at +`embodichain/lab/sim/physics/`: + +```text +embodichain/lab/sim/physics/ + __init__.py # registry + make_physics_backend(physics_cfg, manager) + base.py # PhysicsBackend ABC + default.py # DefaultPhysicsBackend (name = "default") + newton.py # NewtonPhysicsBackend (name = "newton") +``` + +`PhysicsBackend` is constructed with a back-reference to its owning +`SimulationManager` (an instance member, not a class singleton — this preserves +EmbodiChain's multiton, which IsaacLab's class-singleton approach would break). +The manager delegates through `self.physics.*` instead of branching on a backend +name for operational decisions: + +- `configure_world(world_config, sim_config)` applies backend-specific + `WorldConfig` fields (default tolerances/GPU flags, or `world_config.newton_cfg`). +- `activate(sim_config)` runs post-world-creation setup (default + `set_physics_config` / GPU-memory config; Newton registration already comes + from `WorldConfig.newton_cfg`). +- `prepare_spawn_runtime(result)` performs backend runtime work once per + committed topology revision. Default initializes Direct GPU buffers on CUDA; + the base implementation is a no-op used by Default CPU and Newton. +- `sync_render_state(result)` publishes bound state without stepping. Newton + syncs its World-owned runtime to render resources; Default is a no-op. +- `prepare_for_teardown()` releases backend-owned views before Spawn releases + their native parents. +- `get_scene()` returns the active physics scene. +- `solver_type` and `differentiable_runtime` expose optional runtime services + without manager-side type checks. + +Capability predicates drive the `add_*` guards (see Parity Matrix below): +`supports_robot`, deformable topology flags and their soft/cloth compatibility +aliases, `supports_rigid_object_group`, `supports_rigid_constraints`, +`supports_contact_sensor`, and `can_disable_manual_update`. + +`SimulationManager.prepare()` owns the convergent readiness sequence: commit or +rebuild the dirty Spawn scene, apply runtime config, call the backend runtime +hook, bind stable facades, publish render state, and attach deferred sensors. +The legacy `init_gpu_physics()` and `finalize_newton_physics()` methods both +delegate to this same backend-neutral boundary. + +Public `SimulationManager` accessors are preserved as thin delegators for +back-compat: `physics_backend`, `is_default_backend`, `is_newton_backend`, +`newton_manager`, `init_gpu_physics()`, `finalize_newton_physics()`, +`get_physics_scene()`. + +`newton_manager` is retained only as a compatibility diagnostic: Newton raises +an actionable error because Spawn owns the World-level backend and no independent +`NewtonManager` exists. Scene dirtiness and topology revisions belong to +`SceneBuilder`/finalized `Scene`, not to a second backend lifecycle state machine. + +### Object Backend Adapters + +Rigid-body and articulation data access is routed through: + +```text +embodichain/lab/sim/objects/backends/ + base.py # Stable RigidBodyViewBase / ArticulationViewBase contracts + scene.py # Backend-neutral Scene rigid-body/articulation batch adapters + newton.py # Newton-only state synchronization and mimic hooks +``` + +Normal `SimulationManager` construction binds both Default and Newton facades +to the same Scene batch views. `RigidBodyData` and `ArticulationData` require a +finalized Scene; the raw `PhysicsScene`/native-entity adapter path has been +removed. Scene and its batches own backend dispatch, stable selection +rebinding, and topology revisions. `Scene*View.from_entities()` centralizes the +two public batch-factory calls instead of repeating them in each object facade. + +EmbodiChain public rigid-body tensor convention is `(x, y, z, qx, qy, qz, qw)`; +the Scene adapters convert to/from DexSim's `(qx,qy,qz,qw,x,y,z)` batch layout. + +Newton rigid-object support includes dynamic/kinematic/static creation, local +pose, body state, linear/angular velocity+acceleration, force/torque at COM, +clear dynamics, reset, COM local pose, mass/friction/inertia-diagonal/ +restitution/contact-offset get+set, dynamic/kinematic collision filters, and +visual material/visibility/geometry/scale/user-id APIs. The common Scene view +implements `RigidBodyViewBase`, including contact-offset access. + +Static Newton bodies do not have `RigidBodyData`; runtime collision-filter +writes are therefore unavailable and must be configured before materialization. + +### Grouped Newton and Default physics attributes + +`RigidBodyPhysicsCfg` is the single public schema for rigid-object and +articulation link physics. It separates portable values into `mass_props`, +`rigid_props`, `collision_props`, and `material_props`. Each concept has one +slot, and backend-native values use that slot's concrete subtype; parallel +`default_props`/`newton_props` blocks are not supported. Every field is +optional, so source-authored values survive sparse USD/URDF overlays. The same +partial schema is used by `LinkPhysicsOverrideCfg`, eliminating the former flat +compatibility/override type pair. + +Mesh collision construction is owned by `MeshCfg.collision`, whose explicit +approximation selects convex hull, convex decomposition, triangle mesh, or SDF. +Newton SDF/hydroelastic cooking fields live there rather than in rigid-body +physics. Imported articulation links keep their source mesh approximation until +a named source-shape overlay is available. + +Spawn compiles these groups into its backend-neutral rigid-body and shape +descriptors, then projects Default- or Newton-specific values at the selected +backend boundary. The remaining raw Default path uses a private +`PhysicalAttr` adapter only at that boundary. User-facing COM quaternions stay +in `xyzw` order; adapters convert to DexSim's `wxyz` order when writing native +attributes and convert back on reads. + +### Runtime attribute mutation on Newton + +`RigidObject.set_attrs`/`set_damping`/`set_body_type` are no longer warn-and-skip: + +- `set_attrs`: when finalized, applies the Newton-supported subset (mass, + dynamic_friction, restitution, contact_offset) via the batch view and mirrors + all fields to the attr meta; before finalization, mirrors only. +- `set_damping`: documented runtime no-op that mirrors to meta (Newton does not + model per-body damping) so `get_damping`/rebuild stay consistent. +- `set_body_type`: no-op with a clearer message — body type is fixed at + registration on Newton and cannot change at runtime without a rebuild. + +`set_mass`/`set_friction`/`set_inertia` use the batch view when finalized; their +not-ready `else` paths mirror the single field to meta on Newton (the default-bound +`get_physical_body().set_*` are not Newton-patched). `Articulation.set_link_physical_attr` +pushes per-link **mass** live on Newton via `set_link_mass` (mirroring the +dedicated `set_mass`); friction/restitution/contact_offset remain rebuild-time- +only for articulation links. + +### add_robot / add_articulation on Newton + +Robots are URDF articulations; the Newton `load_urdf` patch builds a +`NewtonArticulation`. `add_robot` and `add_articulation` are now **supported** on +Newton (`supports_robot = True`). This required an upstream dexsim fix +(`NewtonArticulation._joint_metas_from_ids`): explicit `joint_ids` were +raw-dict-indexed (including fixed joints) instead of active-joint-indexed, +conflicting with `get_dof()`/`get_actived_joint_names()` and breaking +mimic-jointed robots (dexforce_w1) at spawn. The fix indexes into active joints; +the `joint_ids=None` path is unchanged so existing callers are unaffected. The +dexsim fix lives on dexsim branch `yueci/adapt-embodichain` (commit `d0e86bb02`) +— `add_robot`-on-Newton depends on it being present. + +### Backend capability parity matrix + +`tests/sim/test_backend_parity.py` is the single source of truth for which +features each backend supports (`BACKEND_CAPABILITIES` table). It pins that each +backend's `supports_*`/`can_disable_manual_update` flags match the table, every +manager feature guards raise `NotImplementedError` iff their capability is +false, and the matrix covers every flag and backend. Current matrix: + +| feature | default | newton | +|--------------------------|---------|--------| +| robot | yes | yes | +| soft_bodies | yes | no | +| cloth | yes | no | +| rigid_object_group | yes | yes | +| rigid_constraints | yes | no | +| contact_sensor | yes | no | +| can_disable_manual_update| yes | no | + +### Currently Unsupported Newton APIs + +`SimulationManager` explicitly rejects these asset types on Newton (per the +parity matrix): + +- `add_soft_object(...)` +- `add_cloth_object(...)` +- `create_rigid_constraint(...)` +- `add_sensor(ContactSensorCfg(...))` + +Newton does support `RigidObjectGroup`; it is an env-major view over the same +Scene rigid-body batch used by individual objects. + +`RigidObject.add_force_torque(pos=...)` ignores `pos` and applies force/torque at +the center of mass. + +Newton kinematic pose locking is not complete. The rigid-object test suite keeps +a Newton-specific allowance for kinematic bodies changing after stepping. + +Newton SDF rigid mesh support is not validated in EmbodiChain. The SDF rigid +object test is skipped for Newton. Procedural SDF and CoACD geometry is compiled +from `MeshCfg.collision` through the Spawn descriptor path. + +Articulation Newton-native **per-link** contact/shape params (`ke`/`kd`/`margin`/ +...) are accepted in config but not applied (dexsim `NewtonArticulation` exposes +no per-link contact-material setter); a warning fires at spawn. Common fields are +applied. + +### Verified Tests + +Newton integration is covered across headless and GPU suites: + +```bash +pytest -q tests/sim/objects/test_rigid_object.py +pytest -q tests/sim/objects/test_rigid_object_group.py +pytest -q tests/sim/objects/test_articulation.py::TestArticulationNewton +pytest -q tests/sim/objects/test_robot.py::TestRobotNewton +pytest -q tests/sim/test_physics_attrs.py tests/sim/test_backend_parity.py +pytest -q tests/sim/test_sim_manager.py tests/sim/test_sim_manager_cfg.py +``` + +Do not copy historical pass counts into this document; report results from the +current checkout and dependency build. + +## Improvements To Make + +### API Clarity + +- Manager-level operational selection is routed through `PhysicsBackend` hooks, + runtime properties, and capability flags. Backend names remain only for + diagnostics, explicit implementation registries, and compatibility + predicates. Object-view `is_newton_backend` checks are adapter-level storage + and lifecycle distinctions; move them only when a backend-neutral view + operation can express the same contract without hiding behavior. +- `is_use_gpu_physics` still conflates selected tensor/device location, + default-backend GPU API availability, and Newton GPU execution; consider + splitting when a consumer needs to distinguish them. + +### Newton Lifecycle + +- `SimulationManager.prepare()` is the single readiness API for Default CPU, + Default CUDA, and Newton. Compatibility aliases delegate to it. +- Track dirty scene/model state more explicitly so mutations after finalization + can choose between live batch updates and model rebuilds. +- Keep teardown World-owned and deferred; release backend views through + `prepare_for_teardown()` before closing Spawn/native parents. + +### RigidObject + +- Implement force-at-position when DexSim Newton exposes the needed API. +- Validate SDF rigid mesh creation and collision behavior on Newton. +- Fix or document kinematic pose-lock semantics. + +### Object Groups, Soft, Cloth + +- Maintain Newton rigid-object-group parity through the Scene rigid-body batch. +- Keep soft and cloth fail-fast until there is an explicit Newton design and + test coverage. dexsim exposes `SoftBodyObject`/`add_softbody`/`add_clothbody` + (requires the VBD solver) — feasible but substantial. + +### Articulation / Robot + +- Apply Newton-native per-link contact/shape params once dexsim exposes a + `NewtonArticulation` per-link shape-material setter. +- Add runtime `Articulation.set_link_physical_attr` Newton live push for + friction/restitution/contact_offset once a live per-link API exists (mass is + already live). + +### Gym Env Integration + +Use the backend-neutral readiness boundary after declaring the complete scene: + +```python +self.sim.prepare() +``` + +For stepping, keep the existing high-level flow: + +```python +self._preprocess_action(action) +self._step_action(action) +self.sim.update(self.sim_cfg.physics_dt, self.cfg.sim_steps_per_control) +``` + +For reset, call object/manager reset methods through the normal BaseEnv flow; +do not introduce a backend-specific second initialization path. + +## Completion Plan + +Done: + +1. Single-rigid-object Newton API stabilized; `test_rigid_object.py` green. +2. Backend capability declarations (`PhysicsBackend.supports_*`) drive `add_*` + guards, pinned by `test_backend_parity.py`. +3. Newton `RigidObject` parity for attributes, damping, body type — implemented + (`set_attrs` live subset + meta-mirror, `set_damping` no-op+meta, + `set_body_type` documented no-op). +4. Tests for Newton lifecycle rebuild and runtime property mutation after + finalization — present (`test_sim_manager.py`, `spawn/test_scene.py`, and + `test_rigid_object.py::TestRigidObjectNewton`). +5. Newton `RigidObjectGroup` support uses the Scene rigid-body batch and is + covered by `test_rigid_object_group.py::TestRigidObjectGroupNewton`. +6. Gym environment construction uses the unified `SimulationManager.prepare()` + boundary after scene declaration. +9. Articulation and robot support on Newton — implemented (incl. upstream + dexsim joint-active-indexing fix); `TestArticulationNewton` and + `TestRobotNewton` green. +13. Multi-env parallel simulation on Newton — already complete via the + spawn-time prototype+clone path (`spawn_rigid_object_entities` / + `spawn_articulation_entities` → dexsim's `clone_actor_to`, + Newton-patched). Newton object views accept multi-entity lists and + resolve one body ID per env. Covered by `TestRigidObjectNewton` + (`NUM_ARENAS=2`, `test_spawn_clones_distinct_entities`), + `TestArticulationNewton` (`num_envs=2`), `TestRobotNewton` + (`num_envs=10`). Implementation plan: + `docs/superpowers/plans/2026-06-22-newton-backend-pr.md`. +14. Differentiable env for APG — implemented. + `embodichain.lab.sim.diff` provides `NewtonStepFunc` + (`torch.autograd.Function`) bridging task-defined Newton kinematics + recorded on a `wp.Tape` into PyTorch autograd. `DifferentiableEnv` + validates `NewtonPhysicsCfg(requires_grad=True)` and never advances the + configured semi-implicit solver. The Franka FR3 + reach APG example (`franka_reach_apg.py`) exercises the bridge end-to-end + with `newton.eval_fk`, a Warp action kernel, and a Warp reward kernel + computed inside the tape. Solver steppers, differentiable trajectories, + and gradient rollouts are not part of this kinematics-only stage; they need + a separate future dynamics design. Agent context: + `agent_context/topics/differentiable-env/`. + + .. note:: + The Franka task uses a task-defined FK callback + (``newton.eval_fk``). This is the only supported differentiable + environment route in the current stage; the configured + ``semi_implicit`` solver is not advanced. + +Remaining: + +7. Add rigid-only Newton gym smoke tests. +10. Add soft/cloth support after a dedicated Newton object design and tests. +11. Newton-native per-link contact params for articulations (after dexsim + exposes a per-link shape-material setter). +12. Full migration off legacy `PhysicalAttr` to dexsim's spawn descriptors + (Phase 3 follow-up `3b`) — defer until a third backend appears or dexsim's + attr-path deletion lands. + +## Tests To Maintain + +Configuration: + +- `SimulationManagerCfg(physics_cfg=DefaultPhysicsCfg())` preserves current + default-backend behavior. +- `SimulationManagerCfg(physics_cfg=NewtonPhysicsCfg())` creates a Newton world. +- `physics_cfg_for_backend(...)` and `physics_backend_from_cfg(...)` return the + expected backend mapping. + +PhysicsBackend abstraction: + +- `PhysicsBackend` ABC contract enforced (abstract methods; concrete backends + implement them). `test_backend_parity.py` pins the capability matrix and the + `add_*` guard mapping. +- `SimulationManager.prepare()` delegates backend runtime preparation and render + publication once per topology revision (`test_sim_manager.py`), while + `SpawnScene`/DexSim own commit and rebuild state (`spawn/test_scene.py`). + +Simulation: + +- Newton world can be created, finalized, stepped, destroyed, and recreated. +- Default-backend GPU initialization does not run for Newton. +- Newton finalization does not call default-backend GPU fetch/apply APIs. +- Destroying a Newton simulation does not break subsequent default-backend + simulation creation. + +Newton-native attributes (`test_physics_attrs.py`, headless): + +- `from_dict` parses local property-slot discriminators; the Spawn compiler + projects common and Newton-native fields; per-solver warnings (`xpbd` ignores + `ke`/`kd`; `mujoco_warp` ignores `restitution`) fire correctly. + +Rigid object: + +- Dynamic/static/kinematic rigid bodies under Newton. +- Pose, velocity, acceleration, force/torque, reset, COM pose, mass, friction, + inertia, restitution, contact offset, collision filters, geometry APIs behave + consistently with the documented support matrix. +- Single-slot physics properties and `MeshCfg.collision` spawn through the + descriptor path; the body registers with the Newton manager after finalize; + common fields round-trip via the batch view. +- `set_attrs`/`set_damping`/`set_body_type` produce the documented behavior + (live subset / meta no-op / no-op). + +Articulation / Robot: + +- `TestArticulationNewton`: control API, setters, drive, per-link mass live via + `set_link_physical_attr`, remove. +- `TestRobotNewton`: spawn (URDF assembly), finalize, control-part resolution, + qpos round-trip via the Newton articulation view. + +Gym: + +- Rigid-only Newton env initializes, steps, resets, and reads observations. + +Gradient: + +- `requires_grad=True` plus `solver_type="semi_implicit"` can create a gradient + rollout. +- A simple loss can backpropagate through a rollout without CPU/NumPy observation + paths. + +## Known Risks + +- The `add_robot`-on-Newton path depends on the upstream dexsim fix + (`_joint_metas_from_ids` active-joint indexing, dexsim + `yueci/adapt-embodichain` `d0e86bb02`). If dexsim is rebuilt from a different + ref, `supports_robot` would need re-gating. +- dexsim's Newton path hardcodes `density=0.0` in its desc resolver; + EmbodiChain's Spawn compiler authors a positive configured density on the + rigid-body descriptor to avoid the mass gap for dynamic bodies without an + explicit mass and inertia. Watch for dexsim changing this. +- DexSim Newton monkey-patches global classes. Global teardown can affect other + worlds if used at the wrong time. +- Public body/articulation ID mapping APIs may still need DexSim improvements. +- Newton gravity and contact configuration may not yet match every default-backend + setting. +- Some object constructors still contain default-backend assumptions such as + warmup updates; Newton is guarded from those paths. +- Runtime shape/property mutations may require model rebuilds rather than live + updates; Newton-native per-link contact params are build-time only. +- Newton `RigidObjectGroup` partial reset does not currently restore the + initialization-time inertia diagonal after the same row's COM orientation was + mutated at runtime. The focused + `TestRigidObjectGroupNewton::test_reset_restores_default_mass_properties` + regression remains open at the DexSim Newton mass-property boundary. +- Standalone and embedded callers should use `destroy(exit_process=False)` plus + `SimulationManager.flush_cleanup_queue()` after local scene/object references + unwind; the manager's deferred teardown releases backend views before Spawn + closes their native parents. diff --git a/docs/scripts/check_api_docs.py b/docs/scripts/check_api_docs.py index 24a0735ee..56a271f69 100644 --- a/docs/scripts/check_api_docs.py +++ b/docs/scripts/check_api_docs.py @@ -221,7 +221,7 @@ def discover_public_modules( ) if relative_parts[-1] == "__init__": relative_parts.pop() - if any(part.startswith("_") for part in relative_parts): + if any(part.startswith(("_", ".")) for part in relative_parts): continue tree = ast.parse( diff --git a/docs/source/api_reference/embodichain/embodichain.compute.rst b/docs/source/api_reference/embodichain/embodichain.compute.rst index 375e51004..bfe4d3986 100644 --- a/docs/source/api_reference/embodichain/embodichain.compute.rst +++ b/docs/source/api_reference/embodichain/embodichain.compute.rst @@ -10,7 +10,7 @@ own robots, scenes, or environment lifecycle. Importing the root package does not load Torch or Warp. Domain packages load their required dependencies. - ``kinematics/_warp`` implements analytical OPW, SRS, and UR computations. - Stateful solver interfaces remain in ``embodichain.lab.sim.solvers``. + Stateful solver interfaces remain in ``embodichain.lab.sim.motion.solvers``. - ``trajectory`` provides the public tensor interfaces below. Its private ``_warp`` implementation supports path resampling and trajectory warping. - ``geometry/_warp/convex_query.py`` evaluates maximum halfspace values for diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst index 79d1b3614..9d38dea1c 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst @@ -21,6 +21,7 @@ through :func:`~embodichain.lab.gym.utils.registration.make`. .. autosummary:: demo + differentiable_env task_program managers types @@ -56,6 +57,22 @@ Environment Classes :members: :exclude-members: __init__, class_type +Differentiable Environment +-------------------------- + +``DifferentiableEnv`` keeps the standard environment lifecycle while bridging +task-defined Newton kinematics into PyTorch autograd for analytic +policy-gradient tasks. Subclasses provide action, kinematics, and output +kernels; the base class owns tape-aware stepping and deferred resets without +advancing the Newton solver. + +.. currentmodule:: embodichain.lab.gym.envs.differentiable_env + +.. autoclass:: DifferentiableEnv + :members: + :inherited-members: + :show-inheritance: + Controller-ready Actions ------------------------ diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst index 38c731efa..c128fea1f 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst @@ -53,4 +53,3 @@ embodichain.lab.gym.envs.task_program .. autoclass:: EnvironmentStepTimingError .. autoclass:: UnsupportedRuntimeTransportError - diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst index 14b832079..aa5ff9025 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst @@ -15,34 +15,92 @@ drive properties, and the per-entity configs consumed by factory in :mod:`embodichain.lab.sim.utility.sim_utils`. Entity configs form a small inheritance hierarchy rooted at ``ObjectBaseCfg`` -(``LightCfg``, ``RigidObjectCfg``, ``SoftObjectCfg``, ``ClothObjectCfg``, +(``LightCfg``, ``RigidObjectCfg``, ``VolumeDeformableObjectCfg``, ``SurfaceDeformableObjectCfg``, ``ArticulationCfg`` and its ``RobotCfg`` subclass), while ``URDFCfg`` and ``RigidConstraintCfg`` describe multi-component assembly and constraints. +``RobotPresetCfg`` provides replace-only complete robot alternatives when a +backend-specific asset or actuator definition is unavoidable. +Public backend selectors use only ``default`` and ``newton``. Nested physical +property groups may additionally use ``common`` for backend-neutral intent; +DexSim names belong to the runtime and Spawn SDK adapter boundary. +Surface deformables may keep a stable low-resolution simulation topology in +``shape`` while binding an independently indexed ``visual_shape`` for authored +UV seams and render detail. + +.. rubric:: Type aliases + +.. autosummary:: + + AssetPhysicsMode + MeshCollisionApproximation .. rubric:: Classes .. autosummary:: RenderCfg - PhysicsCfg + PhysicsBackendCfg + DefaultPhysicsCfg + NewtonPhysicsCfg + NewtonCollisionPipelineCfg MarkerCfg WindowRecordCfg WindowCameraPoseCfg GPUMemoryCfg - RigidBodyAttributesCfg - RigidBodyAttributesOverrideCfg + MassPropertiesCfg + DefaultRigidBodyPropertiesCfg + CollisionPropertiesCfg + DefaultCollisionPropertiesCfg + NewtonCollisionPropertiesCfg + RigidBodyMaterialCfg + NewtonRigidBodyMaterialCfg + MeshCollisionCfg + RigidBodyPhysicsCfg + ArticulationRootPropertiesCfg LinkPhysicsOverrideCfg - SoftbodyVoxelAttributesCfg - SoftbodyPhysicalAttributesCfg - ClothPhysicalAttributesCfg + VolumeDeformableMeshingCfg + VolumeDeformablePhysicsCfg + SurfaceDeformablePhysicsCfg + SurfaceElementPropertiesCfg JointDrivePropertiesCfg + NewtonJointDrivePropertiesCfg ObjectBaseCfg LightCfg RigidObjectCfg - SoftObjectCfg - ClothObjectCfg RigidObjectGroupCfg RigidConstraintCfg URDFCfg ArticulationCfg RobotCfg + RobotPresetCfg + +Deformable physics and meshing +----------------------------- + +Like ``RigidObjectCfg.attrs``, deformable objects group physical intent under +``attrs``. Volume objects keep mesh generation under ``meshing``. Both physics +configs share the seven Newton surface-element parameters through +``attrs.surface_props``; volume coefficients default to zero, while cloth +coefficients default to ``None`` (Newton defaults). Density remains topology +specific: kg/m³ for volumes and kg/m² for surfaces. + +.. autoclass:: VolumeDeformablePhysicsCfg + :members: + :no-index: + +.. autoclass:: SurfaceDeformablePhysicsCfg + :members: + :no-index: + +.. autoclass:: SurfaceElementPropertiesCfg + :members: + :no-index: + +.. autoclass:: VolumeDeformableMeshingCfg + :members: + :no-index: + +Constructors and ``from_dict()`` accept only the current fields. Surface-element +properties are grouped under ``attrs.surface_props``. Volume elasticity uses +``youngs`` and ``poissons``. Configuration dictionaries serialize this same +schema; no legacy aliases or field migration are provided. diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst index fc0a040fd..1dab6aea9 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst @@ -11,7 +11,7 @@ derives from :class:`~embodichain.lab.sim.common.BatchEntity` and pairs a runtime class with a ``*Data`` buffer and a ``*Cfg`` config. The hierarchy covers lights (``Light``), rigid bodies (``RigidObject`` and grouped ``RigidObjectGroup``), articulated chains (``Articulation``) and their robot -specialization (``Robot``), deformables (``SoftObject``, ``ClothObject``), +specialization (``Robot``), deformables (``VolumeDeformableObject``, ``SurfaceDeformableObject``), interactive ``Gizmo`` handles, and ``RigidConstraint`` attachments between bodies. @@ -31,12 +31,6 @@ bodies. ArticulationJointKinematics ArticulationData ArticulationCfg - SoftObject - SoftBodyData - SoftObjectCfg - ClothObject - ClothBodyData - ClothObjectCfg Robot RobotCfg RobotWorkspaceCfg @@ -122,42 +116,6 @@ Articulation :show-inheritance: :exclude-members: __init__, copy, replace, to_dict, validate -Soft Object ------------ - -.. autoclass:: SoftObject - :members: - :inherited-members: - :show-inheritance: - -.. autoclass:: SoftBodyData - :members: - :show-inheritance: - -.. autoclass:: SoftObjectCfg - :members: - :inherited-members: - :show-inheritance: - :exclude-members: __init__, copy, replace, to_dict, validate - -Cloth Object ------------- - -.. autoclass:: ClothObject - :members: - :inherited-members: - :show-inheritance: - -.. autoclass:: ClothBodyData - :members: - :show-inheritance: - -.. autoclass:: ClothObjectCfg - :members: - :inherited-members: - :show-inheritance: - :exclude-members: __init__, copy, replace, to_dict, validate - Robot ----- @@ -220,3 +178,131 @@ Rigid Constraint :members: :inherited-members: :show-inheritance: + +Backend Views +------------- + +Backend views normalize tensor layouts and row selection over backend-neutral +DexSim Scene batches. The package import path exposes the common contracts, +Scene adapters, and the Newton Scene predicate. ``Scene*View.from_entities()`` +owns Scene batch creation so object facades do not depend directly on DexSim's +batch-factory signatures. + +.. currentmodule:: embodichain.lab.sim.objects.backends + +.. autosummary:: + + ArticulationViewBase + RigidBodyViewBase + is_newton_scene + SceneArticulationView + SceneRigidBodyView + +.. autoclass:: ArticulationViewBase + :members: + +.. autoclass:: RigidBodyViewBase + :members: + +.. autoclass:: SceneArticulationView + :members: + :show-inheritance: + +.. autoclass:: SceneRigidBodyView + :members: + :show-inheritance: + +.. autofunction:: is_newton_scene + +Backend implementation import paths +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. currentmodule:: embodichain.lab.sim.objects.backends.base + +.. autosummary:: + + RigidBodyViewBase + ArticulationViewBase + +.. currentmodule:: embodichain.lab.sim.objects.backends.newton + +.. autosummary:: + + is_newton_scene + +.. currentmodule:: embodichain.lab.sim.objects.backends.scene + +.. autosummary:: + + SceneArticulationView + SceneRigidBodyView + +Unified Deformable Objects +-------------------------- + +Surface and volume objects share one concrete ``DeformableObjectData`` backed +by DexSim Newton particle batches. After ``sim.prepare()``, prefer ``obj.data`` +for state reads: ``n_nodes`` gives the particle count without fetching state, +``nodal_pos_w`` and ``nodal_vel_w`` return world-frame tensors of shape +``(num_instances, n_nodes, 3)``, and ``nodal_state_w`` concatenates the two. +State reads return independent snapshots. ``default_nodal_state_w`` retains +the state captured at Spawn binding; ``root_pos_w`` is the mean node position, +not a mass-weighted center of mass. + +Use ``obj.deformable_type`` to distinguish physical topology. Read simulation +nodes through ``data``. Render vertices and triangles are available through +``get_surface_vertices()`` and ``get_surface_triangles()``; volume objects also +expose tetrahedral boundary triangles through ``get_collision_surface_triangles()``. +Use ``SimulationManager.add_deformable_object()`` and ``get_deformable_object()`` +to manage both topologies. + +.. currentmodule:: embodichain.lab.sim.objects.deformable + +.. autosummary:: + + DeformableObject + DeformableObjectData + SurfaceDeformableObject + VolumeDeformableObject + +.. autoclass:: DeformableObject + :members: + :show-inheritance: + +.. autoclass:: DeformableObjectData + :members: + +.. autoclass:: SurfaceDeformableObject + :members: + :show-inheritance: + +.. autoclass:: VolumeDeformableObject + :members: + :show-inheritance: + +Deformable implementation import paths +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. currentmodule:: embodichain.lab.sim.objects.deformable.base + +.. autosummary:: + + DeformableObject + +.. currentmodule:: embodichain.lab.sim.objects.deformable.data + +.. autosummary:: + + DeformableObjectData + +.. currentmodule:: embodichain.lab.sim.objects.deformable.surface + +.. autosummary:: + + SurfaceDeformableObject + +.. currentmodule:: embodichain.lab.sim.objects.deformable.volume + +.. autosummary:: + + VolumeDeformableObject diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst index ebe5e3170..f2cf39198 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst @@ -6,11 +6,23 @@ embodichain.lab.sim.shapes Overview -------- -Geometry configuration objects used to build the collision and visual shapes of -rigid bodies. :class:`ShapeCfg` is the common base; :class:`MeshCfg`, +Geometry configuration objects used to build collision, visual, and deformable +simulation shapes. :class:`ShapeCfg` is the common base; :class:`MeshCfg`, :class:`CubeCfg`, and :class:`SphereCfg` describe triangle-mesh, box, and -sphere primitives respectively, and :class:`LoadOption` controls how mesh -assets are loaded and decomposed. +sphere primitives respectively. :class:`MeshCollisionCfg` explicitly selects +the collision representation and its cooking settings, while +:class:`LoadOption` controls mesh loading. + +``MeshCfg`` accepts either a file path or explicit vertex/triangle arrays; for +surface deformables, the array-backed form preserves node order for particle +flags and kinematic trajectories. Volume deformables generate a separate +tetrahedral simulation mesh during voxelization. + +.. rubric:: Type aliases + +.. autosummary:: + + MeshCollisionApproximation .. rubric:: Classes @@ -18,6 +30,7 @@ assets are loaded and decomposed. CubeCfg LoadOption + MeshCollisionCfg MeshCfg ShapeCfg SphereCfg @@ -36,6 +49,12 @@ assets are loaded and decomposed. :show-inheritance: :exclude-members: __init__, copy, replace, to_dict, validate +.. autoclass:: MeshCollisionCfg + :members: + :undoc-members: + :show-inheritance: + :exclude-members: __init__, copy, replace, to_dict, validate + .. autoclass:: CubeCfg :members: :undoc-members: diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.sim_manager.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.sim_manager.rst index 1131a77f8..c1b99b2a5 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.sim_manager.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.sim_manager.rst @@ -35,6 +35,7 @@ native IK on the first update with an open window, as used by the robot tutorial SimulationManager SimulationManagerCfg + get_physics_scene .. currentmodule:: embodichain.lab.sim.sim_manager @@ -42,7 +43,23 @@ native IK on the first update with an open window, as used by the robot tutorial :members: :undoc-members: :show-inheritance: - :exclude-members: visualize_point_cloud + :exclude-members: register_contact_material_schedule, register_kinematic_joint_trajectory, register_kinematic_nodal_trajectory, register_particle_contact_material_schedule, visualize_point_cloud + +.. rubric:: Newton runtime controls + +Runtime controls must be registered after declaring their target assets and +before :meth:`SimulationManager.prepare`. The manager expands logical UIDs to +the concrete paths of every Arena, so callers do not need access to the private +Spawn scene. The particle-material schedule is host-side and disables CUDA +Graph replay; the other controls are graph-compatible. + +.. automethod:: SimulationManager.register_kinematic_joint_trajectory + +.. automethod:: SimulationManager.register_kinematic_nodal_trajectory + +.. automethod:: SimulationManager.register_contact_material_schedule + +.. automethod:: SimulationManager.register_particle_contact_material_schedule .. rubric:: Native point-cloud visualization @@ -53,3 +70,8 @@ native IK on the first update with an open window, as used by the robot tutorial :undoc-members: :show-inheritance: :exclude-members: __init__, copy, replace, to_dict, validate + +Active Physics Scene +-------------------- + +.. autofunction:: get_physics_scene diff --git a/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst b/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst index a190393b6..67ce0fc65 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst @@ -26,4 +26,3 @@ embodichain.lab.task_program.compiler CompiledTaskProgramValidator TaskProgramCompileError TaskProgramCompiler - diff --git a/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.simulation.rst b/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.simulation.rst index 06616230d..42aeaf1c7 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.simulation.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.simulation.rst @@ -21,4 +21,3 @@ embodichain.lab.task_program.integrations.simulation SimulationRobotSkillProfileBinding SimulationSceneBinding SupportSurfaceAffordanceBinding - diff --git a/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst b/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst index 03d383b6e..041e55f01 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst @@ -42,4 +42,3 @@ embodichain.lab.task_program.language parse_task_program_json render_config_path validate_task_program - diff --git a/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst b/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst index ea077faf0..aa921bd6e 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst @@ -16,4 +16,3 @@ embodichain.lab.task_program.runtime SemanticCallExecutor SemanticExecutionResult SemanticExecutionStatus - diff --git a/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst b/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst index 0db256501..8410924e8 100644 --- a/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst +++ b/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst @@ -33,6 +33,7 @@ policy, while :func:`compute_gae` provides generalized advantage estimation. build_algo get_registered_algo_names compute_gae + complete_discounted_return segmented_discounted_return .. automodule:: embodichain.learning.rl.algo diff --git a/docs/source/api_reference/embodichain/embodichain.learning.rl.models.rst b/docs/source/api_reference/embodichain/embodichain.learning.rl.models.rst index dba6979ed..a4e382278 100644 --- a/docs/source/api_reference/embodichain/embodichain.learning.rl.models.rst +++ b/docs/source/api_reference/embodichain/embodichain.learning.rl.models.rst @@ -36,4 +36,3 @@ through :func:`register_policy` / :func:`get_policy_class`. :members: :undoc-members: :show-inheritance: - \ No newline at end of file diff --git a/docs/source/api_reference/embodichain/embodichain.learning.rl.rst b/docs/source/api_reference/embodichain/embodichain.learning.rl.rst index bf1b5b01e..289e176d4 100644 --- a/docs/source/api_reference/embodichain/embodichain.learning.rl.rst +++ b/docs/source/api_reference/embodichain/embodichain.learning.rl.rst @@ -27,12 +27,18 @@ collection logic, policy/model builders, and training entry points. DifferentiableTrainer DifferentiableTrainerCfg + DifferentiableRolloutSpec DifferentiableVecEnv LearningVecEnv + ScheduledDifferentiableVecEnv + RunningObservationNormalizer + BatchedGradientNormStats build_learning_env + clip_batched_gradient_norm evaluate_episodes get_trainer_class register_learning_env + stratified_rollout_value Algorithms ---------- @@ -58,6 +64,22 @@ Evaluation :undoc-members: :show-inheritance: +Gradient Stabilization +---------------------- + +.. automodule:: embodichain.learning.rl.gradients + :members: + :undoc-members: + :show-inheritance: + +Observation Normalization +------------------------- + +.. automodule:: embodichain.learning.rl.normalization + :members: + :undoc-members: + :show-inheritance: + Routing ------- diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 253938d7a..3ce722c55 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -22,6 +22,22 @@ decoding and compiling the constrained Task Program schema surface. compile_mllm_task_program decode_mllm_task_program +embodichain.data.assets.demo_assets +----------------------------------- + +Downloadable bundles used by standalone manipulation and deformable-body +demos. Each class resolves one versioned archive into the configured +EmbodiChain data cache. + +.. currentmodule:: embodichain.data.assets.demo_assets + +.. autosummary:: + + CoordinatedPlacementAndPickment + DeformableDemoData + MultiW1Data + ScoopIceNewEnv + embodichain.data.assets.planner_assets -------------------------------------- @@ -829,6 +845,38 @@ embodichain.lab.sim.atomic_actions.transports EndpointCommandRouter EndpointCommandTransport +embodichain.lab.sim.diff +------------------------ + +Public bridge from task-defined Newton kinematics and Warp tapes into PyTorch +autograd. It does not advance the Newton dynamics solver. + +.. currentmodule:: embodichain.lab.sim.diff + +.. autosummary:: + + NewtonStepFunc + tape_context + +embodichain.lab.sim.diff.bridge +------------------------------- + +.. currentmodule:: embodichain.lab.sim.diff.bridge + +.. autosummary:: + + NewtonStepFunc + tape_context + +embodichain.lab.sim.diff.runtime +-------------------------------- + +.. currentmodule:: embodichain.lab.sim.diff.runtime + +.. autosummary:: + + NewtonDifferentiableRuntime + embodichain.lab.sim.objects.articulation ---------------------------------------- @@ -840,16 +888,6 @@ embodichain.lab.sim.objects.articulation Articulation ArticulationJointKinematics -embodichain.lab.sim.objects.cloth_object ----------------------------------------- - -.. currentmodule:: embodichain.lab.sim.objects.cloth_object - -.. autosummary:: - - ClothBodyData - ClothObject - ClothObjectCfg embodichain.lab.sim.objects.constraint -------------------------------------- @@ -907,16 +945,48 @@ embodichain.lab.sim.objects.robot ControlGroup Robot -embodichain.lab.sim.objects.soft_object ---------------------------------------- -.. currentmodule:: embodichain.lab.sim.objects.soft_object +embodichain.lab.sim.physics +--------------------------- + +Manager-level physics backend selection and lifecycle contracts for the +Default and Newton implementations integrated through DexSim. + +.. currentmodule:: embodichain.lab.sim.physics + +.. autosummary:: + + PhysicsBackend + DefaultPhysicsBackend + NewtonPhysicsBackend + make_physics_backend + +embodichain.lab.sim.physics.base +-------------------------------- + +.. currentmodule:: embodichain.lab.sim.physics.base .. autosummary:: - SoftBodyData - SoftObject - SoftObjectCfg + PhysicsBackend + +embodichain.lab.sim.physics.default +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.physics.default + +.. autosummary:: + + DefaultPhysicsBackend + +embodichain.lab.sim.physics.newton +---------------------------------- + +.. currentmodule:: embodichain.lab.sim.physics.newton + +.. autosummary:: + + NewtonPhysicsBackend embodichain.lab.sim.motion.planners.base_planner ------------------------------------------------ @@ -1092,6 +1162,17 @@ embodichain.lab.sim.sensors.camera Camera CameraCfg +embodichain.lab.sim.sensors.contact_sensor +------------------------------------------ + +.. currentmodule:: embodichain.lab.sim.sensors.contact_sensor + +.. autosummary:: + + ArticulationContactFilterCfg + ContactSensor + ContactSensorCfg + embodichain.lab.sim.sim_manager ------------------------------- @@ -1258,6 +1339,64 @@ embodichain.lab.sim.motion.solvers.srs_solver SRSSolver SRSSolverCfg +embodichain.lab.sim.spawn +------------------------- + +Translation boundary from EmbodiChain object configs and singleton USD assets +into DexSim Spawn descriptors. + +.. currentmodule:: embodichain.lab.sim.spawn + +.. autosummary:: + + articulation_desc_from_cfg + articulation_desc_from_usd + rigid_desc_from_cfg + rigid_desc_from_usd + surface_deformable_desc_from_cfg + volume_deformable_desc_from_cfg + +embodichain.lab.sim.spawn.descriptors +------------------------------------- + +.. currentmodule:: embodichain.lab.sim.spawn.descriptors + +.. autosummary:: + + articulation_desc_from_cfg + configure_articulation_desc + rigid_desc_from_cfg + surface_deformable_desc_from_cfg + volume_deformable_desc_from_cfg + +embodichain.lab.sim.spawn.scene +------------------------------- + +.. currentmodule:: embodichain.lab.sim.spawn.scene + +.. autosummary:: + + SpawnScene + +embodichain.lab.sim.spawn.source +-------------------------------- + +.. currentmodule:: embodichain.lab.sim.spawn.source + +.. autosummary:: + + resolve_articulation_source + +embodichain.lab.sim.spawn.usd +----------------------------- + +.. currentmodule:: embodichain.lab.sim.spawn.usd + +.. autosummary:: + + articulation_desc_from_usd + rigid_desc_from_usd + embodichain.lab.sim.utility.render_utils ---------------------------------------- @@ -1579,6 +1718,7 @@ embodichain.learning.rl.algo.apg APG APGCfg + complete_discounted_return segmented_discounted_return embodichain.learning.rl.algo.base @@ -1679,6 +1819,18 @@ embodichain.learning.rl.experimental.newton.train_planar_reach NewtonPlanarReachTrainingCfg train_planar_reach +embodichain.learning.rl.gradients +--------------------------------- + +Row-wise action-adjoint clipping and its rollout-level diagnostics. + +.. currentmodule:: embodichain.learning.rl.gradients + +.. autosummary:: + + BatchedGradientNormStats + clip_batched_gradient_norm + embodichain.learning.rl.models.actor_critic ------------------------------------------- @@ -1706,6 +1858,15 @@ embodichain.learning.rl.models.policy Policy +embodichain.learning.rl.normalization +------------------------------------- + +.. currentmodule:: embodichain.learning.rl.normalization + +.. autosummary:: + + RunningObservationNormalizer + embodichain.learning.rl.utils.optimizer --------------------------------------- @@ -1968,6 +2129,18 @@ embodichain_tasks.manipulation.tableware.stack_cups StackCupsEnv +embodichain_tasks.special.franka_reach_apg +------------------------------------------- + +Differentiable Franka FR3 reach environment that demonstrates the explicit +kinematics route used by analytic policy-gradient experiments. + +.. currentmodule:: embodichain_tasks.special.franka_reach_apg + +.. autosummary:: + + FrankaReachApgEnv + embodichain_tasks.special.simple_task ------------------------------------- diff --git a/docs/source/features/workspace_analyzer/workspace_analyzer.md b/docs/source/features/workspace_analyzer/workspace_analyzer.md index 025f449f0..110f20ea2 100644 --- a/docs/source/features/workspace_analyzer/workspace_analyzer.md +++ b/docs/source/features/workspace_analyzer/workspace_analyzer.md @@ -28,7 +28,7 @@ from embodichain.lab.sim.motion.workspace import ( ) # Setup simulation -sim = SimulationManager(SimulationManagerCfg(headless=False, sim_device="cpu")) +sim = SimulationManager(SimulationManagerCfg(headless=False, device="cpu")) # Add robot robot = sim.add_robot(DexforceW1Cfg.from_dict({ @@ -170,7 +170,7 @@ from embodichain.lab.sim.motion.workspace import ( from embodichain.lab.sim.motion.workspace.configs import VisualizationConfig # Setup simulation -sim = SimulationManager(SimulationManagerCfg(headless=False, sim_device="cpu")) +sim = SimulationManager(SimulationManagerCfg(headless=False, device="cpu")) # Add robot robot = sim.add_robot(DexforceW1Cfg.from_dict({ diff --git a/docs/source/guides/add_robot.rst b/docs/source/guides/add_robot.rst index 7227b81fb..230cd0eff 100644 --- a/docs/source/guides/add_robot.rst +++ b/docs/source/guides/add_robot.rst @@ -13,7 +13,7 @@ Every robot config subclasses :class:`~embodichain.lab.sim.cfg.RobotCfg` and overrides two hooks: - ``_build_defaults(self, init_dict=None)`` — populate ``urdf_cfg``, - ``control_parts``, ``solver_cfg``, ``drive_pros`` and ``attrs`` from variant + ``control_parts``, ``solver_cfg``, ``joint_drive_props`` and ``attrs`` from variant fields read out of ``init_dict``. - ``build_pk_serial_chain(self, device=...)`` — return a ``{control_part: pk.SerialChain}`` mapping, reading the PK URDF from a single @@ -34,7 +34,7 @@ Checklist 1. **Prepare the URDF** — place the URDF (+ meshes) in the assets directory. 2. **Override** ``_build_defaults(self, init_dict=None)`` — set variant fields from ``init_dict``, then populate ``urdf_cfg`` / ``control_parts`` / ``solver_cfg`` / - ``drive_pros`` / ``attrs``. + ``joint_drive_props`` / ``attrs``. 3. **Define control parts** — group joints into logical sets (e.g. ``arm``, ``gripper``). 4. **Configure the IK solver** — ``OPWSolverCfg`` (6-DOF), ``SRSSolverCfg`` (7-DOF), or a generic ``SolverCfg``. @@ -68,9 +68,9 @@ Key parameters +---------------------+----------------------------------+----------------------------------+ | ``solver_cfg`` | Dict[str, SolverCfg] | IK solver configurations | +---------------------+----------------------------------+----------------------------------+ -| ``drive_pros`` | JointDrivePropertiesCfg | Joint stiffness, damping, force | +| ``joint_drive_props`` | JointDrivePropertiesCfg | Joint drive, limits, friction | +---------------------+----------------------------------+----------------------------------+ -| ``attrs`` | RigidBodyAttributesCfg | Rigid-body physics attributes | +| ``attrs`` | RigidBodyPhysicsCfg | Grouped rigid-body physics | +---------------------+----------------------------------+----------------------------------+ | variant fields | enum / str / bool | Optional subclass fields | | | | (e.g. ``version``) | diff --git a/docs/source/guides/cli.md b/docs/source/guides/cli.md index 3586bc979..60856f525 100644 --- a/docs/source/guides/cli.md +++ b/docs/source/guides/cli.md @@ -159,9 +159,10 @@ embodichain run-env --gym_config config.yaml \ | ``--gym_config`` | *(required)* | Path to a runnable gym config with ``id`` (``.json``, ``.yaml``, or ``.yml``) | | ``--action_config`` | ``None`` | Path to action config file (``.json``, ``.yaml``, or ``.yml``) | | ``--num_envs`` | ``1`` | Number of parallel environments | -| ``--device`` | ``cpu`` | Device (``cpu`` or ``cuda``) | +| ``--device`` | *(config/backend default)* | Explicit device override (for example ``cpu`` or ``cuda:0``); omission preserves the configured or selected-backend default | +| ``--physics`` | *(from config)* | File-owned physics backend (``default`` or ``newton``); this option may confirm the same value but cannot switch a Gym config to another backend | | ``--headless`` | ``False`` | Run in headless mode | -| ``--renderer`` | ``auto`` | Renderer backend: ``auto``, ``hybrid``, ``fast-rt`` or ``rt`` | +| ``--renderer`` | *(config; ``auto`` if absent)* | Renderer backend: ``auto``, ``hybrid``, ``fast-rt`` or ``rt`` | | ``--arena_space`` | ``5.0`` | Arena space size | | ``--gpu_id`` | ``0`` | GPU ID to use | | ``--preview`` | ``False`` | Enter interactive preview mode | diff --git a/docs/source/guides/configuration.md b/docs/source/guides/configuration.md index c2cd14cc7..15f615b10 100644 --- a/docs/source/guides/configuration.md +++ b/docs/source/guides/configuration.md @@ -34,12 +34,12 @@ EmbodiChain configs form a nested hierarchy: EmbodiedEnvCfg ├── sim_cfg: SimulationManagerCfg │ ├── render_cfg: RenderCfg -│ ├── physics_config: PhysicsCfg -│ ├── gpu_memory_config: GPUMemoryCfg +│ ├── physics_cfg: DefaultPhysicsCfg | NewtonPhysicsCfg +│ │ └── gpu_memory: GPUMemoryCfg # Default backend only │ └── visualization: VisualizationCfg ├── robot: RobotCfg │ ├── urdf_cfg: URDFCfg -│ ├── drive_pros: JointDrivePropertiesCfg +│ ├── joint_drive_props: JointDrivePropertiesCfg │ └── solver_cfg: Dict[str, SolverCfg] ├── sensor: List[SensorCfg] ├── events: EventCfg @@ -152,6 +152,8 @@ When a training config references a gym config (via `trainer.gym_config`), the n "num_envs": 4, "max_episodes": 100, "max_episode_steps": 600, + "physics": "default", + "device": "cpu", "physics_config": { "gravity": [0.0, 0.0, -9.81], "bounce_threshold": 2.0, @@ -194,8 +196,8 @@ When a training config references a gym config (via `trainer.gym_config`), the n "height": 540, "width": 960 } - ], - "env": { + ], + "env": { "control_parts": ["arm"], "actions": { "delta_qpos": { @@ -247,6 +249,67 @@ When a training config references a gym config (via `trainer.gym_config`), the n } ``` +Every runnable Gym configuration explicitly owns one `physics` backend: +`"default"` or `"newton"`. Its optional flat `physics_config` mapping is +decoded strictly against that backend, so Default-only fields cannot silently +leak into a Newton configuration (and vice versa). The launcher cannot switch a +file-owned backend with `--physics`; select a different runnable configuration +when the backend should change. + +`device` is optional. If omitted, the backend config keeps its own default +(`cpu` for Default and `cuda:0` for Newton). An explicitly configured device, +or an explicit `run-env --device ...` override, is applied to both the +environment tensors and the selected physics backend. In particular, +`--device cpu` is not ignored for Newton. + +#### Paired physics configurations + +The following are **physics fragments**, not complete runnable task files. +Apply each fragment to a separate copy of the same inline deployment, retaining +its required `id`, `env`, robot/embodiment, and scene declarations. Retune any +backend-specific asset fields as described in +{doc}`/overview/sim/physics_migration`. + +Default fragment: + +```yaml +physics: default +device: cuda:0 +physics_config: + physics_dt: 0.01 + gravity: [0.0, 0.0, -9.81] + bounce_threshold: 2.0 + enable_ccd: false +``` + +Newton fragment for a rigid articulated scene: + +```yaml +physics: newton +device: cuda:0 +physics_config: + physics_dt: 0.01 + gravity: [0.0, 0.0, -9.81] + num_substeps: 10 + solver_cfg: + solver_type: mujoco_warp + use_cuda_graph: true +``` + +Omit `solver_cfg` to let Newton's AutoSolver select from the complete scene at +`prepare()`. For componentized tasks, place these backend declarations in +separate physical environment files, such as `env.default.yaml` and +`env.newton.yaml`, and point each deployment's `environment.component` at the +appropriate file. Do not repeat `physics` or `physics_config` in those +deployments. Keep the control period equal when comparing the two tasks. + +| Entry point | Backend selection | Backend parameters | +| :--- | :--- | :--- | +| Python `SimulationManagerCfg` | Concrete `physics_cfg` type | Fields of `DefaultPhysicsCfg` or `NewtonPhysicsCfg`. | +| Inline runnable Gym file | Required `physics` | Flat `physics_config`, decoded against that backend. | +| Componentized deployment | Selected environment component's `physics` | The same environment component owns `physics_config`. | +| Launcher `--physics` | Confirms the file-owned value | Cannot convert a task to a different backend. | + The `visualization` section is optional and defaults to `{"backend": "none"}`. Setting `"backend": "viser"` starts browser visualization when the environment constructs its `SimulationManager`. The @@ -279,13 +342,15 @@ layout separates a reusable environment from runnable deployment choices: └── program.yaml ``` -The pure `env.yaml` component owns physical scene entities and ordinary Gym -values. It requires `environment_id`, `simulation`, and `env`, may include run -controls such as `max_episode_steps`, and contains no runnable `id`, robot, -sensor, or Task Program selection: +The pure `env.yaml` component owns the physics backend, physical scene entities, +and ordinary Gym values. It requires `environment_id`, `physics`, `simulation`, +and `env`, may include `physics_config` and run controls such as +`max_episode_steps`, and contains no runnable `id`, robot, sensor, or Task +Program selection: ```yaml environment_id: repeated_pick_place +physics: default max_episode_steps: 1200 simulation: rigid_object: @@ -328,6 +393,11 @@ neither reusable component is coupled to an expert-authoring method. See the [Task Program tutorial](../tutorial/task_program.rst) for a complete runnable composition. +When `environment.component` is selected, that component exclusively owns +`physics` and `physics_config`; do not repeat either field in the runnable +deployment. This keeps backend choice next to the physical scene whose fields +it validates. + ### Robot Preset Configs Use `class_type` to select a `RobotCfg` subclass from diff --git a/docs/source/guides/preview_asset.md b/docs/source/guides/preview_asset.md index a83fa49bc..a0fd2ef6f 100644 --- a/docs/source/guides/preview_asset.md +++ b/docs/source/guides/preview_asset.md @@ -146,7 +146,7 @@ asset.set_local_pose(pose) | `--init_pos X Y Z` | `0 0 0.5` | Initial position of the first asset. | | `--init_rot RX RY RZ` | `0 0 0` | Initial rotation in degrees. | | `--body_type` | `kinematic` | Rigid body type: `dynamic`, `kinematic`, or `static`. | -| `--use_usd_properties` | disabled | Use physical properties stored in the USD file. | +| `--asset-physics-mode {preserve,overlay}` | `overlay` | Preserve source-authored physics or overlay explicitly configured values. | | `--fix_base` / `--no-fix_base` | fixed | Fix or unfix articulation bases. | | `--sim_device` | `cpu` | Simulation device. | | `--renderer` | `hybrid` | Renderer: `hybrid`, `fast-rt`, or `rt`. | diff --git a/docs/source/guides/run_env.md b/docs/source/guides/run_env.md index adec20e91..a605e0b32 100644 --- a/docs/source/guides/run_env.md +++ b/docs/source/guides/run_env.md @@ -63,11 +63,20 @@ At startup, `run-env`: point and executes their initialization hooks; 2. loads the runnable config, expands its selected physical components, and composes any declared Task Program components; -3. applies CLI overrides such as `--num_envs`, `--device`, `--renderer`, and - `--max_episodes`; +3. merges launcher runtime values; optional overrides such as `--device`, + `--renderer`, and `--max_episodes` apply only when supplied; 4. creates the environment selected by the gym config's `id`; and 5. enters rollout, preview, or replay mode. +The runnable config, or its selected `environment.component`, must declare +`physics: default` or `physics: newton`. See +{doc}`configuration` for paired backend configuration fragments and +{doc}`/overview/sim/physics_migration` for migration checks. That backend is file-owned: +`--physics` can confirm it but cannot switch it. Omitting `--device` preserves +an authored `device` or the selected backend's default; supplying `--device` +overrides both environment tensors and backend execution, including an explicit +CPU selection for Newton. + Use `embodichain run-env --help` for the complete option list. The {ref}`CLI Reference ` also lists defaults and visualization arguments. diff --git a/docs/source/overview/gym/action_functors.md b/docs/source/overview/gym/action_functors.md index 375c0a3c9..bd971f87f 100644 --- a/docs/source/overview/gym/action_functors.md +++ b/docs/source/overview/gym/action_functors.md @@ -59,7 +59,7 @@ This page lists all available action terms that can be used with the Action Mana * - Action Term - Description * - {class}`~actions.EefPoseTerm` - - End-effector pose (6D or 7D) -> IK -> qpos. The policy outputs target end-effector poses which are converted to joint positions via inverse kinematics. Returns ``ik_success`` in the output so reward/observation can penalize or condition on IK failures. Supports both 6D (euler angles) and 7D (quaternion) pose representations. + - End-effector pose (6D or 7D) -> IK -> qpos. The policy outputs target end-effector poses which are converted to joint positions via inverse kinematics. Returns ``ik_success`` in the output so reward/observation can penalize or condition on IK failures. Supports both 6D (euler angles) and 7D (``x, y, z, qx, qy, qz, qw``) pose representations. ```json {"func": "EefPoseTerm", "params": {"scale": 0.1, "pose_dim": 7}} @@ -129,7 +129,7 @@ actions = { func="EefPoseTerm", params={ "scale": 0.1, - "pose_dim": 7, # 7D (position + quaternion) + "pose_dim": 7, # 7D (x, y, z, qx, qy, qz, qw) }, ), } diff --git a/docs/source/overview/gym/observation_functors.md b/docs/source/overview/gym/observation_functors.md index 3b6ead061..ba6f04a7c 100644 --- a/docs/source/overview/gym/observation_functors.md +++ b/docs/source/overview/gym/observation_functors.md @@ -25,7 +25,7 @@ This page lists all available observation functors that can be used with the Obs * - Functor Name - Description * - {func}`~observations.get_object_pose` - - Get the arena poses of objects. Returns 4x4 transformation matrices of shape (num_envs, 4, 4) by default, or (num_envs, 7) as [x, y, z, qw, qx, qy, qz] when ``to_matrix=False``. Returns zero tensor if object doesn't exist. + - Get the arena poses of objects. Returns 4x4 transformation matrices of shape (num_envs, 4, 4) by default, or (num_envs, 7) as [x, y, z, qx, qy, qz, qw] when ``to_matrix=False``. Returns zero tensor if object doesn't exist. ```json {"func": "get_object_pose", "mode": "add", @@ -33,7 +33,7 @@ This page lists all available observation functors that can be used with the Obs "params": {"entity_cfg": {"uid": "bottle"}, "to_matrix": true}} ``` * - {func}`~observations.get_rigid_object_pose` - - Get the arena poses of rigid objects. Returns 4x4 transformation matrices of shape (num_envs, 4, 4) by default, or (num_envs, 7) when ``to_matrix=False``. If the object doesn't exist, returns a zero tensor. (Deprecated: use ``get_object_pose`` instead.) + - Get the arena poses of rigid objects. Returns 4x4 transformation matrices of shape (num_envs, 4, 4) by default, or (num_envs, 7) as [x, y, z, qx, qy, qz, qw] when ``to_matrix=False``. If the object doesn't exist, returns a zero tensor. (Deprecated: use ``get_object_pose`` instead.) ```json {"func": "get_rigid_object_pose", "mode": "add", diff --git a/docs/source/overview/sim/default_physics.md b/docs/source/overview/sim/default_physics.md new file mode 100644 index 000000000..bb5d27eca --- /dev/null +++ b/docs/source/overview/sim/default_physics.md @@ -0,0 +1,125 @@ +# Default Physics Backend + +```{currentmodule} embodichain.lab.sim +``` + +The Default backend provides CPU and Direct GPU simulation through DexSim. +Its established constraint-solver default is TGS (Temporal Gauss-Seidel). +Select it with {class}`~cfg.DefaultPhysicsCfg`, or `physics: default` in a +Gym configuration. See {doc}`sim_manager` for the shared capability matrix, +device selection, and time-step definitions. + +## Minimal simulation + +This example declares one falling cube, prepares the scene, and advances +100 physics steps. Set `device="cuda:0"` to use the Direct GPU path on a +compatible installation. CPU physics still uses the normal DexSim runtime; +rendering requirements are described in {doc}`/quick_start/install`. + +```python +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import DefaultPhysicsCfg, RigidObjectCfg +from embodichain.lab.sim.shapes import CubeCfg + + +def run() -> None: + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + physics_cfg=DefaultPhysicsCfg(device="cpu", physics_dt=0.01), + ) + ) + try: + cube = sim.add_rigid_object( + RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=[0.1, 0.1, 0.1]), + body_type="dynamic", + init_pos=(0.0, 0.0, 0.5), + ) + ) + sim.prepare() + for _ in range(100): + sim.update(step=1) + finally: + sim.destroy(exit_process=False) + + +try: + run() +finally: + # Flush after run() releases its local scene/object references. + SimulationManager.flush_cleanup_queue() +``` + +To run this same scene with Newton, import `NewtonPhysicsCfg` and replace the +`physics_cfg` value with the configuration in {doc}`newton_physics`. Asset +declaration, preparation, and explicit stepping retain the same structure. + +## Scene parameters + +| Parameter | Default | Meaning and tuning guidance | +| :--- | :--- | :--- | +| `physics_dt` | `0.01` | Physics-step duration in seconds; coordinate changes with the control period. | +| `device` | `"cpu"` | CPU execution; `"cuda:0"` selects Direct GPU execution. | +| `gravity` | `[0.0, 0.0, -9.81]` | World-frame acceleration in m/s². | +| `bounce_threshold` | `2.0` | Relative normal-speed threshold in m/s below which contacts do not bounce. | +| `enable_ccd` | `False` | Scene-level continuous collision detection; participating rigid bodies must also enable CCD. | +| `length_tolerance` | `0.05` | Representative scene length in metres, usually near the characteristic object size. | +| `speed_tolerance` | `0.25` | Representative scene speed in m/s; contributes to internal thresholds with the length scale. | +| `gpu_memory` | `GPUMemoryCfg()` | GPU capacities below; applies only to Default CUDA execution. | + +Set the tolerance scales before constructing the manager. These represent the +scene's scale; increasing them is not a general accuracy or performance control. + +The current integration retains PCM and TGS defaults, disables enhanced +determinism, and evaluates friction on every solver iteration. These choices +are not selectable fields of `DefaultPhysicsCfg`. The startup summary reports +the runtime's selected solver, such as TGS or PGS. Do not copy an upstream +framework's `solver_type` setting into this configuration. + +### CCD and articulation properties + +For a body that needs continuous collision detection, set both +`DefaultPhysicsCfg(enable_ccd=True)` and +`attrs.rigid_props=DefaultRigidBodyPropertiesCfg(enable_ccd=True)` on its +rigid-body physics configuration. Validate the intended CPU/GPU path with a +fast-moving-body example; the scene switch alone does not opt every body in. + +Articulation root properties are configured separately with +`ArticulationRootPropertiesCfg`. Root `min_position_iters` and +`min_velocity_iters` must be specified together; these and `sleep_threshold` +are Default-only. Root iteration settings apply before GPU buffer preparation. +They are distinct from per-link `DefaultRigidBodyPropertiesCfg` settings. +See {doc}`sim_articulation` for source preservation and joint drives. + +## GPU memory capacities + +Some Default GPU buffers have fixed capacities. Overflow can produce warnings, +missing contacts, or invalid simulation results. Size them against the largest +scene and worst-case randomized contact configuration, including all arenas. + +| `gpu_memory` field | Default | Capacity / response to overflow | +| :--- | :--- | :--- | +| `temp_buffer_capacity` | `2**24` | Bytes of temporary pinned-host storage; increase for linear allocator overflow. | +| `max_rigid_contact_count` | `2**19` | Rigid contact records; increase for contact-buffer overflow. | +| `max_rigid_patch_count` | `2**18` | Contact patches; increase for patch-buffer overflow. | +| `heap_capacity` | `2**26` | Initial GPU and pinned-host heap bytes. | +| `found_lost_pairs_capacity` | `2**25` | Broad-phase found/lost pair records. | +| `found_lost_aggregate_pairs_capacity` | `2**10` | Found/lost aggregate-pair records. | +| `total_aggregate_pairs_capacity` | `2**10` | Aggregate-pair records in the GPU pipeline. | + +These fields have no effect on Default CPU or Newton. Newton contact and +constraint budgets belong to its selected solver or collision pipeline, so +capacity values cannot be transferred directly between backends. + +## Contacts and validation + +`ContactSensor` works on Default CPU and Direct GPU. The two paths differ in +impulse and static-actor identity reporting; use the public query metadata and +`contact_capabilities` described in {doc}`sim_sensor`. Do not treat GPU and CPU +contact rows as an identical manifold representation. + +For a new task, check settling, impact response, joint tracking, and contact +sensor output before increasing the environment count. Use +{doc}`physics_migration` for symptom-based tuning and backend comparisons. diff --git a/docs/source/overview/sim/index.rst b/docs/source/overview/sim/index.rst index 8719332c7..e11ea3f9f 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -114,9 +114,10 @@ A typical robot-learning or data-generation workflow follows this sequence: 1. Create a :class:`SimulationManager` from :class:`SimulationManagerCfg`. 2. Add assets such as objects, articulations, robots, lights, and materials. 3. Add sensors for camera, stereo, or contact observations. -4. Use solvers and planners to convert task goals into robot trajectories. -5. Step the simulation with :meth:`SimulationManager.update` and collect state or sensor tensors. -6. Wrap the same simulation logic in a Gym environment when training or evaluating agents. +4. Call :meth:`SimulationManager.prepare` before reading asset state or joint/link metadata. +5. Use solvers and planners to convert task goals into robot trajectories. +6. Step the simulation with :meth:`SimulationManager.update` and collect state or sensor tensors. +7. Wrap the same simulation logic in a Gym environment when training or evaluating agents. For manipulation tasks, atomic actions can replace the lower-level solver and planner calls. An action engine receives semantic targets or poses, resolves the @@ -172,6 +173,9 @@ See Also :maxdepth: 1 sim_manager.md + default_physics.md + newton_physics.md + physics_migration.md sim_assets.md sim_sensor.md viser_visualization.md diff --git a/docs/source/overview/sim/motion/planners/motion_generator.md b/docs/source/overview/sim/motion/planners/motion_generator.md index ba7b83676..3f7856d45 100644 --- a/docs/source/overview/sim/motion/planners/motion_generator.md +++ b/docs/source/overview/sim/motion/planners/motion_generator.md @@ -67,7 +67,7 @@ sim_cfg = SimulationManagerCfg( width=1920, height=1080, physics_dt=1.0 / 100.0, - sim_device="cpu", + device="cpu", ) sim = SimulationManager(sim_cfg) @@ -93,7 +93,7 @@ robot_cfg = RobotCfg( dt=0.1, ) }, - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( stiffness={"Joint[1-6]": 1e4}, damping={"Joint[1-6]": 1e3}, ), diff --git a/docs/source/overview/sim/newton_physics.md b/docs/source/overview/sim/newton_physics.md new file mode 100644 index 000000000..7c6634d07 --- /dev/null +++ b/docs/source/overview/sim/newton_physics.md @@ -0,0 +1,222 @@ +# Newton Physics Backend + +```{currentmodule} embodichain.lab.sim +``` + +Use {class}`~cfg.NewtonPhysicsCfg` to enable the Newton backend. Its +`solver_cfg` defaults to `None` intentionally: EmbodiChain leaves the +`solver_cfg` argument unset when it creates DexSim's `NewtonCfg`, preserving +DexSim's `AutoSolverCfg` default. + +```python +from embodichain.lab.sim import SimulationManagerCfg +from embodichain.lab.sim.cfg import NewtonPhysicsCfg + +sim_config = SimulationManagerCfg( + physics_cfg=NewtonPhysicsCfg( + device="cuda:0", + physics_dt=0.01, + num_substeps=10, + ) +) +``` + +AutoSolver is resolved when DexSim finalizes the complete Spawn scene during +{meth}`SimulationManager.prepare`. Add all initial robots and objects before +calling `prepare()` so the selection sees the complete scene. An explicit +`{"solver_type": "auto"}` or `{"class_type": "AutoSolverCfg"}` mapping has +the same effect as leaving `solver_cfg` unset. + +:::{important} +This integration requires a DexSim build that exports `AutoSolverCfg`. +EmbodiChain does not fall back to a hard-coded concrete solver when that API is +unavailable. +::: + +## Configuration reference + +Scene parameters belong to `NewtonPhysicsCfg`; per-object collision, material, +and joint properties belong to asset configurations. Default-backend fields +such as `gpu_memory`, `bounce_threshold`, and `enable_ccd` do not belong here. + +| Parameter | Default | Meaning | +| :--- | :--- | :--- | +| `physics_dt` | `0.01` | One EmbodiChain physics step in seconds. | +| `device` | `"cuda:0"` | Compute device; explicit CPU execution remains subject to solver/asset support. | +| `gravity` | `[0.0, 0.0, -9.81]` | World-frame acceleration in m/s². | +| `num_substeps` | `10` | Solver substeps per physics step; solver interval is `physics_dt / num_substeps`. | +| `solver_cfg` | `None` | Preserve DexSim AutoSolver; a mapping or native solver config fixes an explicit choice. | +| `collision_cfg` | `NewtonCollisionPipelineCfg()` | External Newton collision pipeline; consumption depends on the selected solver. `None` disables this external pipeline. | +| `requires_grad` | `False` | Differentiable model; requires explicit `semi_implicit` and disables CUDA Graph capture. | +| `use_cuda_graph` | `True` | Request graph capture when supported; unavailable on CPU and disabled in gradient mode. | +| `debug_mode` | `False` | Additional runtime diagnostics. | +| `suppress_warp_kernel_logs` | `True` | Suppress Warp startup/kernel compilation messages; warnings and errors remain visible. | +| `visualizer_enabled` | `False` | DexSim Newton diagnostic visualizer; separate from the ordinary camera renderer and Viser. | +| `broad_phase` | `None` | Deprecated shortcut; use `collision_cfg.broad_phase`. | + +Use the minimal falling-cube loop in {doc}`default_physics` with this +`physics_cfg` to compare the same scene. Keep the asset, initial state, physics +duration, and environment count fixed; tune solver-specific behavior separately. + +## Automatic solver selection + +The following scene types are exposed by EmbodiChain. Independent rigid objects +and articulation links are classified separately. + +| Finalized scene contents | Selected configuration | Solver type | Active collision path | +| :--- | :--- | :--- | :--- | +| Empty scene or independent rigid bodies only | `XPBDSolverCfg` | `xpbd` | Newton collision pipeline | +| Articulations, with or without independent rigid bodies | `MJWarpSolverCfg` | `mujoco_warp` | MuJoCo Warp collision pipeline | +| Cloth or soft bodies, optionally with rigid bodies | `VBDSolverCfg` | `vbd` | Newton collision pipeline; VBD may handle deformable self-contact | +| Cloth or soft bodies with articulations, optionally with rigid bodies | `DexUniSolverCfg` | `dexuni` | Newton particle-shape soft contacts; MuJoCo rigid collision is disabled | + +The current DexUni path does not generate rigid-rigid or rigid-ground contacts. +MuJoCo Warp still advances rigid bodies and articulations, while Newton's soft +contact kernels handle deformable particle-shape contacts. + +### Additional upstream resolver rules + +DexSim also recognizes the following particle families. EmbodiChain does not +currently expose public fluid or MPM asset APIs; these rows describe the +upstream resolver only. + +| Finalized scene contents | Selected configuration | Solver type | Active collision path | +| :--- | :--- | :--- | :--- | +| Fluid particles, optionally with rigid SDF boundaries | `SPHSolverCfg` | `sph` | SPH one-way SDF boundary handling; rigid contacts are not consumed | +| MPM particles, optionally with rigid colliders | `ImplicitMPMSolverCfg` | `implicit_mpm` | Implicit-MPM collider projection; the rigid collision pipeline is not stepped | + +AutoSolver rejects multiple particle families in one scene, fluid particles +combined with articulations, and MPM particles combined with articulations. + +Selection is based on scene contents, not the configured device. DexSim reports +device incompatibility after resolution; cloth, soft-body, fluid, and MPM +solvers currently require CUDA. The selected type is also written to the +DexSim log, for example `Newton AutoSolver selected 'mujoco_warp'.` + +## Explicit solver selection + +Pass a concrete solver configuration when an algorithm or solver-specific +parameter must be fixed: + +```python +sim_config = SimulationManagerCfg( + physics_cfg=NewtonPhysicsCfg( + device="cpu", + solver_cfg={ + "solver_type": "xpbd", + "iterations": 8, + }, + ) +) +``` + +EmbodiChain mapping configs recognize `auto`, `dexuni`, `mujoco_warp` (or +`mjwarp`), `xpbd`, `semi_implicit`, `featherstone`, and `vbd`. A DexSim +`NewtonSolverCfg` object may also be assigned directly when another explicit +solver class is required. AutoSolver never selects `DFSPHSolverCfg`, +`FeatherstoneSolverCfg`, or `SemiImplicitSolverCfg`. In particular, +`requires_grad=True` requires an explicit `semi_implicit` configuration; +automatic selection is rejected for differentiable simulation. + +## Deformable support + +Volume soft bodies and surface cloth are supported through +`VolumeDeformableObjectCfg` and `SurfaceDeformableObjectCfg`. The integration +requires all of the following: + +- CUDA execution. +- `auto`, `dexuni`, `xpbd`, `semi_implicit`, or `vbd` as the configured solver. +- `requires_grad=False`. +- All deformable declarations before the first `prepare()`/scene finalization. + +This list is the integration's admission check, not a claim that every solver +has identical material, self-collision, or joint behavior. Choose a solver for +the complete scene, including any articulated robot. In particular, the DexUni +rigid-contact limitation above matters when a robot or object must also collide +with the ground or other rigid bodies. A solver accepted by upstream Newton +may still lack an EmbodiChain deformable adapter or operation. + +Read physical nodes through `data.nodal_pos_w` and `data.nodal_vel_w` after +preparation. Render vertices can have different topology and indexing. See +{doc}`sim_soft_object` and {doc}`sim_cloth` for units, configuration, and runnable +tutorials. + +## Collision pipelines and contact data + +With `mujoco_warp`, `solver_cfg.use_mujoco_contacts` selects whether the solver +uses its internal collision path or Newton-generated rigid contacts. An +external `collision_cfg` does not by itself make the solver consume those +contacts. Confirm the active path before tuning its capacities or margins. +DexUni owns collision detection and its particle-shape contacts, so set +`collision_cfg=None` when selecting it explicitly. It does not publish rigid +contacts through the current query path. + +| `collision_cfg` field | Default | Meaning | +| :--- | :--- | :--- | +| `reduce_contacts` | `True` | Reduce dense mesh contacts. | +| `rigid_contact_max` | `None` | Use model capacity or scene estimation for rigid contacts. | +| `max_triangle_pairs` | `4_000_000` | Narrow-phase triangle-pair candidate capacity. | +| `soft_contact_max` | `None` | Derive particle/soft-contact capacity from the scene. | +| `soft_contact_margin` | `0.01` | Particle/soft-contact generation margin in metres. | +| `broad_phase` | `None` | Preserve upstream default; explicit modes include `nxn`, `sap`, and `explicit`. | +| `update_interval` | `1` | Refresh at every solver substep; integer `k` refreshes at substeps `0, k, 2k, ...`. `None` refreshes only at the first substep of each physics step. | + +`shape_pairs_filtered`, `narrow_phase`, and `sdf_hydroelastic_config` accept +expert pipeline objects; their detailed contracts are in +{class}`~cfg.NewtonCollisionPipelineCfg`. Per-shape values belong to +`NewtonCollisionPropertiesCfg`. Reducing collision refresh frequency can miss +changes between updates, so assess penetration and contact continuity before +using it as a performance optimization. + +`ContactSensor` supports Newton. It creates a backend-neutral contact query and +reports the query's capabilities at runtime: + +| Execution path | Geometry | Impulse data | +| :--- | :--- | :--- | +| MuJoCo-Warp on CUDA | Available | Normal and friction impulse data available. | +| Other supported Newton rigid solvers | Available | Geometry-only paths use zero-valued impulse fields. | +| MuJoCo CPU mode | Device contact buffers unavailable | Unsupported by this sensor path. | +| DexUni | Rigid contacts not published through `ContactQuery` | Unsupported by this sensor path. | + +Check `sensor.contact_capabilities` before interpreting measurements. Zero +impulse on a geometry-only solver does not mean absence of a contact candidate. +See {doc}`sim_sensor` for frames, filtering, arena batching, and capacity limits. + +## Differentiable simulation and CUDA Graphs + +For the supported differentiable route, choose the solver explicitly: + +```python +physics_cfg = NewtonPhysicsCfg( + device="cuda:0", + solver_cfg={"solver_type": "semi_implicit"}, + requires_grad=True, + use_cuda_graph=False, +) +``` + +This enables differentiable model state; it does not turn an ordinary Gym +rollout, arbitrary state mutation, or every asset into a differentiable +computation. Use the learning integration described in +{doc}`/overview/rl/algorithm`. AutoSolver and deformable state mutation are +rejected in this mode. Upstream support for other differentiable solvers does +not expand the current EmbodiChain contract. + +For ordinary CUDA simulation, `use_cuda_graph=True` is a request, not evidence +that capture has completed. The startup summary distinguishes pending, +captured, and disabled states. Preparation and initial camera rendering do not +advance the simulation merely to capture a graph. Model changes can invalidate +captured execution; author fixed-root placements and initial topology before +the first update. Particle contact-material schedules disable graph replay. + +Measure compilation and first-step capture separately from steady-state +stepping. Compare runs at the same simulated duration and control period, and +record the resolved solver, substeps, graph status, device, and rendering load. + +## Upstream references + +The [Newton solver guide](https://newton-physics.github.io/newton/stable/solvers/index.html) +provides native solver feature tables. DexSim's AutoSolver and coupled solver +configuration names describe this integration and should not be confused with +native Newton Python class names. See {doc}`physics_migration` for the reference +versions and a practical migration workflow. diff --git a/docs/source/overview/sim/sim_articulation.md b/docs/source/overview/sim/sim_articulation.md index 6e5118654..698254414 100644 --- a/docs/source/overview/sim/sim_articulation.md +++ b/docs/source/overview/sim/sim_articulation.md @@ -8,20 +8,19 @@ The {class}`~objects.Articulation` class represents the fundamental physics enti ## Configuration Articulations are configured using the {class}`~cfg.ArticulationCfg` dataclass. + | Parameter | Type | Default | Description | | :--- | :--- | :--- | :--- | | `fpath` | `str` | `None` | Path to the asset file (URDF/USD). | | `init_pos` | `tuple` | `(0,0,0)` | Initial root position `(x, y, z)`. | | `init_rot` | `tuple` | `(0,0,0)` | Initial root rotation `(r, p, y)` in degrees. | -| `fix_base` | `bool` | `True` | Whether to fix the base of the articulation. | -| `use_usd_properties` | `bool` | `False` | If True, use physical properties from USD file; if False, override with config values. Only effective for usd files. | +| `root_props` | `ArticulationRootPropertiesCfg` | `fixed_base=True`, `self_collision_enabled=False`; other fields `None` | Fixed-base/self-collision are portable; root sleep and paired solver iterations are Default-only and ignored by Newton. Explicit `None` preserves source/backend values. | +| `asset_physics_mode` | `"preserve" \| "overlay"` | `"preserve"` | Preserve source link/joint physics, or apply explicitly configured overlays after source resolution. | | `init_qpos` | `List[float]` | `None` | Initial joint positions. | -| `qpos_limits` | `Tensor` / `Dict[str, List[float]]` | `None` | Override joint position limits. Replaces asset limits and may either tighten or expand the range. | +| `qpos_limits` | `Tensor` / `Dict[str, List[float]]` | `None` | Override limits by flattened source-resolved DOF order or joint-name/regex rules before backend build. | | `body_scale` | `List[float]` | `[1.0, 1.0, 1.0]` | Scaling factors for the articulation links. | -| `disable_self_collision` | `bool` | `True` | Whether to disable self-collisions. | -| `enable_gravity` | `bool` | `True` | Whether gravity affects the articulation. This runtime flag also applies when `use_usd_properties=True`. | -| `drive_pros` | `JointDrivePropertiesCfg` | `drive_type="none"` | Default drive properties. | -| `attrs` | `RigidBodyAttributesCfg` | `...` | Default rigid body attributes applied to all links. | +| `joint_drive_props` | `JointDrivePropertiesCfg` | `None` | Optional sparse joint drive, limit, friction, and armature overlay. | +| `attrs` | `RigidBodyPhysicsCfg` | empty groups | Grouped rigid-body physics applied to all links. | | `link_attrs` | `dict[str, LinkPhysicsOverrideCfg]` | `None` | Optional per-link overrides keyed by group name; each group matches link names via regex. | At runtime, call `articulation.set_gravity(...)` to change gravity for every @@ -35,20 +34,24 @@ override specific links (matched by regex, same rules as joint drive dict keys): ```python from embodichain.lab.sim.cfg import ( ArticulationCfg, + CollisionPropertiesCfg, LinkPhysicsOverrideCfg, - RigidBodyAttributesCfg, - RigidBodyAttributesOverrideCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, ) art_cfg = ArticulationCfg( fpath="path/to/robot.urdf", - attrs=RigidBodyAttributesCfg(static_friction=0.5), + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + material_props=RigidBodyMaterialCfg(static_friction=0.5), + ), link_attrs={ "eef": LinkPhysicsOverrideCfg( link_names_expr=[".*(hand|finger|ee).*"], - attrs=RigidBodyAttributesOverrideCfg( - static_friction=0.95, - contact_offset=0.001, + attrs=RigidBodyPhysicsCfg( + material_props=RigidBodyMaterialCfg(static_friction=0.95), + collision_props=CollisionPropertiesCfg(contact_offset=0.001), ), ), }, @@ -58,19 +61,55 @@ art_cfg = ArticulationCfg( At runtime, use `articulation.set_link_physical_attr(...)` and `get_link_physical_attr(...)` for the same partial-override behavior. +### Source mass properties + +For URDF-backed articulations, `MassPropertiesCfg.recompute_inertia` is the +only switch that permits geometry-derived mass properties to replace the +asset's authored inertia. It defaults to `False` (or `None`, which resolves to +the same behavior), so an `overlay` that only configures joint drives or other +unrelated attributes retains the source mass, inertia, and center of mass in +both backends. Set it to `True` only when collision geometry should be used to +derive a new tensor. A positive `mass` can be overridden while retaining the +source tensor; `density` requires `recompute_inertia=True` when the source +already provides a valid tensor. An all-zero or otherwise invalid source +tensor is not preserved: when the link has collision geometry, both backends +derive a fallback tensor from that geometry. + ### Drive Configuration -The `drive_pros` parameter controls the joint physics behavior. It is defined using the `JointDrivePropertiesCfg` class. Generic articulations default to `drive_type="none"`, so passive assets such as cabinets and drawers do not receive internal drive forces unless explicitly configured. +The `joint_drive_props` parameter uses `JointDrivePropertiesCfg` for sparse +joint-property overlays. Each field defaults to `None`, preserving its source +or backend value. Generic articulations default to +`asset_physics_mode="preserve"`; explicit drive changes require `"overlay"`. +Robot configurations default to `"overlay"`. An unspecified drive is not an +instruction to remove an existing source-authored actuator. | Parameter | Type | Default | Description | | :--- | :--- | :--- | :--- | -| `stiffness` | `float` / `Dict` | `1.0e4` | Stiffness (P-gain) of the joint drive. Unit: $N/m$ or $Nm/rad$. | -| `damping` | `float` / `Dict` | `1.0e3` | Damping (D-gain) of the joint drive. Unit: $Ns/m$ or $Nms/rad$. | -| `max_effort` | `float` / `Dict` | `1.0e10` | Maximum effort (force/torque) the joint can exert. | -| `max_velocity` | `float` / `Dict` | `1.0e10` | Maximum velocity allowed for the joint ($m/s$ or $rad/s$). | -| `friction` | `float` / `Dict` | `0.0` | Joint friction coefficient. | -| `armature` | `float` / `Dict` | `0.0` | Joint armature added to joint-space inertia ($kg$ for prismatic, $kg \cdot m^2$ for revolute). | -| `drive_type` | `str` | `"none"` | Drive mode: `"force"`(driven by a force), `"acceleration"`(driven by an acceleration) or `none`(no force). | +| `stiffness` | `float` / `Dict` | `None` | Position gain; force-drive units are N/m or N·m/rad. | +| `damping` | `float` / `Dict` | `None` | Velocity gain; force-drive units are N·s/m or N·m·s/rad. | +| `max_effort` | `float` / `Dict` | `None` | Authored effort limit in N or N·m; solver enforcement varies. | +| `max_velocity` | `float` / `Dict` | `None` | Authored speed limit in m/s or rad/s; solver enforcement varies. | +| `friction` | `float` / `Dict` | `None` | Passive joint friction; interpretation depends on backend/solver. | +| `armature` | `float` / `Dict` | `None` | Added joint-space inertia in kg for prismatic joints or kg·m² for revolute joints. | +| `drive_type` | `str` | `None` | `"force"`, `"acceleration"` (Default only), or `"none"`; omission preserves the source value. | +| `target_mode` | `str` / `int` / `Dict` | `None` | Target components: `none`/0, `position`/1, `velocity`/2, `position_velocity`/3, or `effort`/4. | + +`None` is accepted for every field in this table. Gain units shown apply to +force/torque drives; Default acceleration drives use a mass-independent +response. Newton rejects active acceleration drives. Unless `target_mode` is +explicit, `drive_type="force"` or `"acceleration"` selects +`position_velocity`, while `drive_type="none"` selects `none`. + +Default implements target components through effective gains. Newton authors +its target mode and uses gain-based fallbacks for solvers without native mode +support: `none` and `effort` clear both gains, and `velocity` clears position +gain. Non-MuJoCo Newton position mode assumes a zero velocity target. + +Newton support for effort/velocity limits, passive friction, and armature is +solver-dependent; storing a value is not proof that the solver enforces it. +Inspect the resolved properties and test the response after changing solvers. +See {doc}`physics_migration` for portability and drive-calibration guidance. ### Joint Position Limits @@ -120,18 +159,19 @@ articulation layer. ```python import torch from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.objects import Articulation, ArticulationCfg +from embodichain.lab.sim.cfg import ArticulationCfg, ArticulationRootPropertiesCfg +from embodichain.lab.sim.objects import Articulation # 1. Initialize Simulation device = "cuda" if torch.cuda.is_available() else "cpu" -sim_cfg = SimulationManagerCfg(sim_device=device) +sim_cfg = SimulationManagerCfg(device=device) sim = SimulationManager(sim_config=sim_cfg) # 2. Configure Articulation art_cfg = ArticulationCfg( fpath="assets/robots/franka/franka.urdf", init_pos=(0, 0, 0.5), - fix_base=True + root_props=ArticulationRootPropertiesCfg(fixed_base=True), ) # 3. Spawn Articulation @@ -154,7 +194,7 @@ from embodichain.data import get_data_path usd_art_cfg = ArticulationCfg( fpath=get_data_path("path/to/robot.usd"), init_pos=(0, 0, 0.5), - use_usd_properties=True # Keep USD drive/physics properties + asset_physics_mode="preserve", ) usd_robot = sim.add_articulation(cfg=usd_art_cfg) @@ -162,8 +202,8 @@ usd_robot = sim.add_articulation(cfg=usd_art_cfg) usd_art_cfg_override = ArticulationCfg( fpath=get_data_path("path/to/robot.usd"), init_pos=(0, 0, 0.5), - use_usd_properties=False, # Use config instead - drive_pros=JointDrivePropertiesCfg(stiffness=5000, damping=500) + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(stiffness=5000, damping=500), ) robot = sim.add_articulation(cfg=usd_art_cfg_override) ``` @@ -182,8 +222,8 @@ State data is accessed via getter methods that return batched tensors (`N` envir | Method | Shape / Return Type | Description | | :--- | :--- | :--- | -| `get_local_pose(to_matrix=False)` | `(N, 7)` or `(N, 4, 4)` | Root link pose `[x, y, z, qw, qx, qy, qz]` or a 4x4 matrix. | -| `get_link_pose(link_name, to_matrix=False)` | `(N, 7)` or `(N, 4, 4)` | Specific link pose `[x, y, z, qw, qx, qy, qz]` or a 4x4 matrix. | +| `get_local_pose(to_matrix=False)` | `(N, 7)` or `(N, 4, 4)` | Root link pose `[x, y, z, qx, qy, qz, qw]` or a 4x4 matrix. | +| `get_link_pose(link_name, to_matrix=False)` | `(N, 7)` or `(N, 4, 4)` | Specific link pose `[x, y, z, qx, qy, qz, qw]` or a 4x4 matrix. | | `get_qpos(target=False)` | `(N, dof)` | Current joint positions (or joint targets if `target=True`). | | `get_qvel(target=False)` | `(N, dof)` | Current joint velocities (or velocity targets if `target=True`). | | `get_joint_drive()` | `Tuple[Tensor, ...]` | Returns `(stiffness, damping, max_effort, max_velocity, friction, armature)`, each shaped `(N, dof)`. | @@ -260,8 +300,8 @@ sim.update() ### Pose Control ```python # Teleport the articulation root to a new pose -# shape: (N, 7) formatted as [x, y, z, qw, qx, qy, qz] -new_root_pose = torch.tensor([[0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0]], device=device).repeat(sim.num_envs, 1) +# shape: (N, 7) formatted as [x, y, z, qx, qy, qz, qw] +new_root_pose = torch.tensor([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]], device=device).repeat(sim.num_envs, 1) articulation.set_local_pose(new_root_pose) ``` diff --git a/docs/source/overview/sim/sim_assets.md b/docs/source/overview/sim/sim_assets.md index 1ac7e1c1b..bd21ef75f 100644 --- a/docs/source/overview/sim/sim_assets.md +++ b/docs/source/overview/sim/sim_assets.md @@ -98,35 +98,31 @@ Configured via {class}`~cfg.RigidObjectCfg`. | Parameter | Type | Default | Description | | :--- | :--- | :--- | :--- | | `shape` | `ShapeCfg` | `ShapeCfg()` | Shape configuration (e.g., Mesh, Box). | -| `attrs` | `RigidBodyAttributesCfg` | `RigidBodyAttributesCfg()` | Physical attributes. | +| `attrs` | `RigidBodyPhysicsCfg` | `RigidBodyPhysicsCfg()` | Grouped physical attributes. | | `body_type` | `Literal` | `"dynamic"` | "dynamic", "kinematic", or "static". | -| `max_convex_hull_num` | `int` | `1` | Maximum hull count for approximate convex decomposition. | -| `shape.acd_method` | `str` | `"visacd"` | Decomposition method. `visacd` is the default; `coacd` and `vhacd` remain available. | -| `sdf_resolution` | `int` | `0` | Resolution for signed distance field. In most cases, a resolution of around 250 produces good results; resolutions exceeding 1000 are rarely necessary.| +| `shape.collision` | `MeshCollisionCfg \| None` | `None` | Explicit mesh collision geometry: convex hull, convex decomposition, triangle mesh, or SDF. `None` uses one convex hull. | | `body_scale` | `tuple` | `(1.0, 1.0, 1.0)` | Scale of the rigid body. | -`visacd` requires CUDA support. Select `coacd` or `vhacd` explicitly when that -requirement is unavailable. +### Rigid Body Physics -### Rigid Body Attributes +{class}`~cfg.RigidBodyPhysicsCfg` keeps physical settings in optional groups. +An unset field leaves the source asset or backend default intact, which makes +the same configuration usable as either a complete procedural definition or a +sparse USD/URDF overlay. -The {class}`~cfg.RigidBodyAttributesCfg` class defines physical properties for rigid bodies. +| Group | Type | Contents | +| :--- | :--- | :--- | +| `mass_props` | `MassPropertiesCfg` | Mass, density, inertia, and COM pose. | +| `rigid_props` | `DefaultRigidBodyPropertiesCfg` | Rigid-body behavior such as damping, CCD, and solver iterations. | +| `collision_props` | `CollisionPropertiesCfg` | Collision enablement, contact/rest offsets, and concrete-backend contact properties. | +| `material_props` | `RigidBodyMaterialCfg` | Restitution, friction, and concrete-backend material properties. | -| Parameter | Type | Default | Description | -| :--- | :--- | :--- | :--- | -| `mass` | `float` | `1.0` | Mass in kg. Set to 0 to use density. | -| `density` | `float` | `1000.0` | Density in kg/m^3. | -| `angular_damping` | `float` | `0.7` | Angular damping coefficient. | -| `linear_damping` | `float` | `0.7` | Linear damping coefficient. | -| `max_depenetration_velocity` | `float` | `10.0` | Maximum depenetration velocity. | -| `sleep_threshold` | `float` | `0.001` | Threshold below which the body can go to sleep. | -| `enable_ccd` | `bool` | `False` | Enable continuous collision detection. | -| `contact_offset` | `float` | `0.002` | Contact offset for collision detection. | -| `rest_offset` | `float` | `0.001` | Rest offset for collision detection. | -| `enable_collision` | `bool` | `True` | Enable collision for the rigid body. | -| `restitution` | `float` | `0.0` | Restitution (bounciness) coefficient. | -| `dynamic_friction` | `float` | `0.5` | Dynamic friction coefficient. | -| `static_friction` | `float` | `0.5` | Static friction coefficient. | +COM quaternions in configuration use `xyzw`. The Spawn adapter converts to the +native backend order only when it writes an engine descriptor. + +Mesh cooking is owned by `MeshCfg.collision`, not by rigid-body physics. Its +`approximation` field selects the representation explicitly; strategy-specific +fields such as `max_hulls` and `sdf_resolution` are validated against it. For a runnable rigid-object example, see the {doc}`Create Scene ` tutorial. diff --git a/docs/source/overview/sim/sim_cloth.md b/docs/source/overview/sim/sim_cloth.md index dfc19caad..280b59806 100644 --- a/docs/source/overview/sim/sim_cloth.md +++ b/docs/source/overview/sim/sim_cloth.md @@ -3,150 +3,80 @@ ```{currentmodule} embodichain.lab.sim ``` -The {class}`~objects.Cloth` class represents deformable surface entities in EmbodiChain. Unlike rigid bodies, cloth objects are defined by vertices and meshes rather than a single rigid pose. +{class}`~objects.SurfaceDeformableObject` represents batched Newton cloth +particle sets. Cloth requires the +Newton backend on CUDA and a particle-capable solver. + +See {doc}`newton_physics` for solver selection, mixed rigid/deformable scene +limitations, and the requirement to declare deformables before preparation. ## Configuration -Configured via {class}`~cfg.ClothObjectCfg`. - -| Parameter | Type | Default | Description | -| :--- | :--- | :--- | :--- | -| `physical_attr` | `ClothPhysicalAttributesCfg` | `...` | Physical attributes. | -| `shape` | `MeshCfg` | `MeshCfg()` | Mesh configuration. | - -### CLoth Body Attributes - -Cloth bodies require both voxelization and physical attributes. - -**Physical Attributes ({class}`~cfg.ClothPhysicalAttributesCfg`)** - -| Parameter | Type | Default | Description | -| :--- | :--- | :--- | :--- | -| `youngs` | `float` | `1e10` | Young's modulus (higher = stiffer). | -| `poissons` | `float` | `0.3` | Poisson's ratio. | -| `dynamic_friction` | `float` | `0.5` | Dynamic friction coefficient. | -| `elasticity_damping` | `float` | `0.0` | Elasticity damping factor. | -| `thickness` | `float` | `0.01` | Cloth thickness (m). | -| `bending_stiffness` | `float` | `0.001` | Bending stiffness. | -| `bending_damping` | `float` | `0.0` | Bending damping. | -| `enable_kinematic` | `bool` | `False` | If True, (partially) kinematic behavior is enabled. | -| `enable_ccd` | `bool` | `True` | Enable continuous collision detection (CCD). | -| `enable_self_collision` | `bool` | `False` | Enable self-collision handling. | -| `has_gravity` | `bool` | `True` | Whether the cloth is affected by gravity. | -| `self_collision_stress_tolerance` | `float` | `0.9` | Stress tolerance threshold for self-collision constraints. | -| `collision_mesh_simplification` | `bool` | `True` | Whether to simplify the collision mesh for self-collision. | -| `vertex_velocity_damping` | `float` | `0.005` | Per-vertex velocity damping. | -| `mass` | `float` | `-1.0` | Total mass of the cloth. If negative, density is used to compute mass. | -| `density` | `float` | `1.0` | Material density in kg/m^3. | -| `max_depenetration_velocity` | `float` | `1e6` | Maximum velocity used to resolve penetrations. | -| `max_velocity` | `float` | `100.0` | Clamp for linear (or vertex) velocity. | -| `self_collision_filter_distance` | `float` | `0.1` | Distance threshold for filtering self-collision vertex pairs. | -| `linear_damping` | `float` | `0.05` | Global linear damping applied to the cloth. | -| `sleep_threshold` | `float` | `0.05` | Velocity/energy threshold below which the cloth can go to sleep. | -| `settling_threshold` | `float` | `0.1` | Threshold used to decide convergence/settling state. | -| `settling_damping` | `float` | `10.0` | Additional damping applied during settling phase. | -| `min_position_iters` | `int` | `4` | Minimum solver iterations for position correction. | -| `min_velocity_iters` | `int` | `1` | Minimum solver iterations for velocity updates. | - -For a runnable example, see the {doc}`Cloth Body Simulation ` tutorial. - - -### Setup & Initialization +Use {class}`~cfg.SurfaceDeformableObjectCfg` with physical parameters grouped +in `attrs`, following the rigid-object configuration convention. + +| Field | Default | Meaning | +| :--- | :--- | :--- | +| `attrs.density` | `1.0` | Surface density in kg/m². | +| `attrs.surface_props.tri_ke/tri_ka/tri_kd` | `None` | Triangle elastic stiffness, area stiffness, and damping. | +| `attrs.surface_props.tri_drag/tri_lift` | `None` | Aerodynamic drag and lift. | +| `attrs.surface_props.edge_ke/edge_kd` | `None` | Bending stiffness and damping. | +| `attrs.add_springs` | `False` | Create explicit mesh-edge springs. | +| `attrs.spring_ke/spring_kd` | `None` | Spring stiffness and damping. | +| `particle_radius` | `None` | Particle radius; inherit solver default when omitted. | +| `shape` | `MeshCfg()` | Simulation source mesh and default render mesh. | +| `visual_shape` | `None` | Optional independently indexed render mesh. | + +The seven `surface_props` coefficients use +{class}`~cfg.SurfaceElementPropertiesCfg`. `None` preserves Newton's +cloth defaults. Cloth uses its source triangle mesh directly and does not +require volume voxelization. ```python -import torch -from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.objects import ClothObject, ClothObjectCfg - - -def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): - """Create a flat rectangle in the XY plane centered at `origin`. - - The rectangle is subdivided into an `nx` by `ny` grid (cells) and - triangulated. `nx=1, ny=1` yields the simple two-triangle rectangle. - - Returns an vertices and triangles. - """ - w = float(width) - h = float(height) - if nx < 1 or ny < 1: - raise ValueError("nx and ny must be >= 1") - - # Vectorized vertex positions using PyTorch - x_lin = torch.linspace(-w / 2.0, w / 2.0, steps=nx + 1, dtype=torch.float64) - y_lin = torch.linspace(-h / 2.0, h / 2.0, steps=ny + 1, dtype=torch.float64) - yy, xx = torch.meshgrid(y_lin, x_lin) # shapes: (ny+1, nx+1) - xx_flat = xx.reshape(-1) - yy_flat = yy.reshape(-1) - zz_flat = torch.full_like(xx_flat, 0, dtype=torch.float64) - verts = torch.stack([xx_flat, yy_flat, zz_flat], dim=1) # (Nverts, 3) - - # Vectorized triangle indices - idx = torch.arange((nx + 1) * (ny + 1), dtype=torch.int64).reshape(ny + 1, nx + 1) - v0 = idx[:-1, :-1].reshape(-1) - v1 = idx[:-1, 1:].reshape(-1) - v2 = idx[1:, :-1].reshape(-1) - v3 = idx[1:, 1:].reshape(-1) - tri1 = torch.stack([v0, v1, v3], dim=1) - tri2 = torch.stack([v0, v3, v2], dim=1) - faces = torch.cat([tri1, tri2], dim=0).to(torch.int32) - return verts, faces - -# 1. Initialize Simulation -device = "cuda" if torch.cuda.is_available() else "cpu" -sim_cfg = SimulationManagerCfg(sim_device=device) -sim = SimulationManager(sim_config=sim_cfg) - -cloth_verts, cloth_faces = create_2d_grid_mesh(width=0.3, height=0.3, nx=12, ny=12) -cloth_mesh = o3d.geometry.TriangleMesh( - vertices=o3d.utility.Vector3dVector(cloth_verts.to("cpu").numpy()), - triangles=o3d.utility.Vector3iVector(cloth_faces.to("cpu").numpy()), +from embodichain.lab.sim.cfg import ( + SurfaceElementPropertiesCfg, + SurfaceDeformableObjectCfg, + SurfaceDeformablePhysicsCfg, ) -cloth_save_path = os.path.join(tempfile.gettempdir(), "cloth_mesh.ply") -o3d.io.write_triangle_mesh(cloth_save_path, cloth_mesh) -# 2. Configure Cloth Object -cfg=ClothObjectCfg( - uid="cloth_demo", - shape=MeshCfg(fpath=cloth_save_path), - init_pos=[0.5, 0.0, 0.3], - init_rot=[0, 0, 0], - physical_attr=ClothPhysicalAttributesCfg( - mass=0.01, - youngs=1e10, - poissons=0.4, - thickness=0.04, - bending_stiffness=0.01, - bending_damping=0.1, - dynamic_friction=0.95, - min_position_iters=30, +from embodichain.lab.sim.shapes import MeshCfg + +cfg = SurfaceDeformableObjectCfg( + uid="cloth", + shape=MeshCfg(fpath="cloth.obj"), + attrs=SurfaceDeformablePhysicsCfg( + density=0.2, + surface_props=SurfaceElementPropertiesCfg( + tri_ke=1000.0, tri_ka=1000.0, edge_ke=0.001, + ), ), ) +``` -# 3. Spawn Cloth Object -# Note: Assuming the method in SimulationManager is 'add_cloth_object' -cloth_object: ClothObject = sim.add_cloth_object(cfg=cfg) +Add this configuration through `sim.add_deformable_object(cfg)` and call +`sim.prepare()` before accessing `cloth.data`. For a runnable example, see +{doc}`Cloth Body Simulation `. -# 4. Initialize Physics -sim.reset_objects_state() -``` ### Cloth Object Class -#### Vertex Data (Observation) -For cloth objects, the state is represented by the positions and velocities of its vertices, rather than a single root pose. +#### Nodal State + +After `sim.prepare()`, read simulation nodes through `object.data`. Each state +property returns an independent tensor snapshot. -| Method | Return Shape | Description | +| Property | Shape | Description | | :--- | :--- | :--- | -| `get_current_vertex_position()` | `(num_envs, n_vert, 3)` | Current positions of mesh vertices. | -| `get_current_vertex_velocity()` | `(num_envs, n_vert, 3)` | Current positions of mesh vertices. | -| `get_rest_vertex_position()` | `(num_envs, n_vert, 3` | Rest (initial) positions of collision vertices. | +| `data.nodal_pos_w` | `(N, V, 3)` | Current simulation-node positions in world coordinates. | +| `data.nodal_vel_w` | `(N, V, 3)` | Current simulation-node velocities in world coordinates. | +| `data.default_nodal_state_w` | `(N, V, 6)` | Positions and velocities captured at Spawn binding. | -> Note: N is the number of environments/instances, V_col is the number of collision vertices, and V_sim is the number of simulation vertices. +`N` is the number of instances; `V` is `data.n_nodes`. Render vertices are +separate and are read through `get_surface_vertices()`. ```python # Example: Accessing vertex data -vert_position = cloth_object.get_current_vertex_position() +vert_position = cloth_object.data.nodal_pos_w print(f"vertices positions: {vert_position}") -vert_velocity = cloth_object.get_current_vertex_velocity() +vert_velocity = cloth_object.data.nodal_vel_w print(f"Vertex Velocities: {vert_velocity}") ``` @@ -173,7 +103,7 @@ You can set the global pose of a cloth object (which transforms all its vertices ```python # Reset or Move the Cloth Object -target_pose = torch.tensor([[0, 0, 1.0, 1, 0, 0, 0]], device=device) # (x, y, z, qw, qx, qy, qz) +target_pose = torch.tensor([[0, 0, 1.0, 0, 0, 0, 1]], device=device) # (x, y, z, qx, qy, qz, qw) cloth_object.set_local_pose(target_pose) # Important: Step simulation to apply changes diff --git a/docs/source/overview/sim/sim_manager.md b/docs/source/overview/sim/sim_manager.md index 02399a5a4..8d3a7be98 100644 --- a/docs/source/overview/sim/sim_manager.md +++ b/docs/source/overview/sim/sim_manager.md @@ -15,13 +15,16 @@ The simulation is configured using the {class}`SimulationManagerCfg` class. ```python from embodichain.lab.sim import SimulationManagerCfg +from embodichain.lab.sim.cfg import DefaultPhysicsCfg sim_config = SimulationManagerCfg( width=1920, # Window width height=1080, # Window height num_envs=10, # Number of parallel environments - physics_dt=0.01, # Physics time step - sim_device="cpu", # Simulation device ("cpu" or "cuda:0", etc.) + device="cpu", # Simulation device ("cpu" or "cuda:0", etc.) + physics_cfg=DefaultPhysicsCfg( + physics_dt=0.01, # Physics time step + ), arena_space=5.0 # Spacing between environments ) ``` @@ -34,33 +37,99 @@ sim_config = SimulationManagerCfg( | `height` | `int` | `1080` | The height of the simulation window. | | `headless` | `bool` | `False` | Whether to run the simulation in headless mode (no Window). | | `render_cfg` | `RenderCfg` | `RenderCfg()` | The rendering configuration parameters. | -| `gpu_id` | `int` | `0` | The gpu index that the simulation engine will be used. Affects gpu physics device. | +| `gpu_id` | `int` | `0` | Rendering GPU index; also resolves an unindexed CUDA compute device. | | `thread_mode` | `ThreadMode` | `RENDER_SHARE_ENGINE` | The threading mode for the simulation engine. | | `cpu_num` | `int` | `1` | The number of CPU threads to use for the simulation engine. | | `num_envs` | `int` | `1` | The number of parallel environments (arenas) to simulate. | | `arena_space` | `float` | `5.0` | The distance between each arena when building multiple arenas. | -| `physics_dt` | `float` | `0.01` | The time step for the physics simulation. | +| `device` | `str` \| `torch.device` \| `None` | `None` | Optional explicit compute-device override. When omitted, the selected physics config keeps its backend default. | +| `physics_cfg` | `DefaultPhysicsCfg` \| `NewtonPhysicsCfg` | `DefaultPhysicsCfg()` | Physics backend configuration (class selects default vs Newton). | | `profiler` | `ProfilerCfg` \| `None` | `None` | Optional hierarchical wall-time profiler for simulation updates. | -| `sim_device` | `str` \| `torch.device` | `"cpu"` | The device for the physics simulation. | -| `physics_config` | `PhysicsCfg` | `PhysicsCfg()` | The physics configuration parameters. | -| `gpu_memory_config` | `GPUMemoryCfg` | `GPUMemoryCfg()` | The GPU memory configuration parameters. | | `visualization` | `VisualizationCfg` | `VisualizationCfg()` | Browser visualization, opt-in Gizmo commands, and Viser server settings. | ### Physics Configuration -The {class}`~cfg.PhysicsCfg` class controls the global physics simulation parameters. +EmbodiChain exposes two physics backends, selected by the configuration type: -| Parameter | Type | Default | Description | +| Backend | Python configuration | Execution and solver model | Start here | | :--- | :--- | :--- | :--- | -| `gravity` | `np.ndarray` | `[0, 0, -9.81]` | Gravity vector for the simulation environment. | -| `bounce_threshold` | `float` | `2.0` | The speed threshold below which collisions will not produce bounce effects. | -| `enable_ccd` | `bool` | `False` | Enable continuous collision detection (CCD) for fast-moving objects. | -| `length_tolerance` | `float` | `0.05` | The length tolerance for the simulation. Larger values increase speed. | -| `speed_tolerance` | `float` | `0.25` | The speed tolerance for the simulation. Larger values increase speed. | - -PCM and TGS remain enabled, enhanced determinism remains disabled, and friction -is evaluated on every solver iteration. These solver implementation details use -fixed defaults and are not exposed by `PhysicsCfg`. +| `default` | `DefaultPhysicsCfg` | CPU or Direct GPU execution; TGS is the established constraint-solver default. | {doc}`default_physics` | +| `newton` | `NewtonPhysicsCfg` | Newton through DexSim; scene-aware automatic selection or an explicit solver. | {doc}`newton_physics` | + +Both are integrated through DexSim's runtime and Spawn SDK. `default` and +`newton` are the only public backend identifiers. The renderer is selected +separately by `render_cfg`; native-window and browser visualization settings +control how the scene is displayed. Selecting Newton does not select a different +camera renderer. `headless=True` closes the native window while permitting +configured offscreen camera rendering. + +### Supported capabilities + +This table describes the current EmbodiChain integration. A feature in an +upstream solver does not imply that every asset or operation exposes it here. + +| Capability | Default | Newton | +| :--- | :--- | :--- | +| Rigid objects, rigid-object groups, articulations, robots | Supported | Supported; joint features depend on the solver. | +| Volume and surface deformables | Unsupported | Supported on CUDA with a particle-capable solver; see {doc}`newton_physics`. | +| Native rigid constraints through the manager | Supported | Unsupported; this is distinct from solver-internal articulation constraints. | +| Camera and stereo camera | Supported | Supported through the shared rendering integration. | +| `ContactSensor` | Supported on CPU and Direct GPU | Supported; geometry and impulse availability depend on solver/device. See {doc}`sim_sensor`. | +| Differentiable simulation | Unsupported | Explicit `semi_implicit` solver only; see {doc}`newton_physics`. | + +Use Default for workflows needing its native rigid constraints or established +TGS behavior. Use Newton for particle-based cloth/soft bodies, solver-specific +experiments, or the supported differentiable path. For rigid robot tasks that +can run on either backend, validate the target task with each configuration; +shared APIs do not guarantee identical trajectories or contact responses. + +### Common configuration and devices + +All backends inherit these parameters from {class}`~cfg.PhysicsBackendCfg`: + +| Parameter | Default | Meaning | +| :--- | :--- | :--- | +| `physics_dt` | `0.01` | Duration of one EmbodiChain physics step, in seconds. | +| `device` | `"cpu"` for Default; `"cuda:0"` for Newton | Compute device for physics and environment tensors. Solver/device restrictions still apply. | +| `gravity` | `[0.0, 0.0, -9.81]` | World-frame acceleration in m/s². | + +`physics_cfg.device` owns the backend default. An explicit +`SimulationManagerCfg(device=...)` overrides it; the legacy `sim_device` +argument is an alias. `gpu_id` selects the rendering GPU and supplies the index +for an unindexed `"cuda"` device. Keep explicit compute and render GPU indices +consistent with the intended deployment. Omitting Gym's `--device` preserves +the configured value or backend default; an explicit `--device cpu` also applies +to Newton, subject to the selected solver and asset restrictions. + +Python chooses a backend through `physics_cfg`. Gym files declare `physics` +and a matching `physics_config`; an environment component owns both fields and +its deployment cannot override either. `--physics` can confirm the file's +backend but cannot switch it. See {doc}`/guides/configuration` for paired +configuration examples and {doc}`physics_migration` for migration checks. + +### Physics, control, and solver time + +| Interval | Definition | Example | +| :--- | :--- | :--- | +| Physics step | `physics_dt` | `0.01 s` (100 Hz) | +| Gym control step | `physics_dt * sim_steps_per_control` | `0.01 * 4 = 0.04 s` (25 Hz) | +| Newton solver substep | `physics_dt / num_substeps` | `0.01 / 10 = 0.001 s` (1 kHz) | + +In this example, one control action spans four physics steps and forty Newton +solver substeps. Increasing Newton's `num_substeps` refines integration without +changing the control period. Reducing `physics_dt` also changes the control +period unless `sim_steps_per_control` is adjusted. Rendering and visualization +publication have their own cadence and do not define the control frequency. + +### Default Backend + +See {doc}`default_physics` for scale parameters, CCD, CPU/GPU behavior, and GPU +buffer capacities. + +### Newton Backend and Automatic Solver Selection + +See {doc}`newton_physics` for solver selection, collision scheduling, supported +deformables, contact data, and gradient/CUDA Graph restrictions. ### Rendering @@ -89,6 +158,29 @@ sim_config = SimulationManagerCfg() sim = SimulationManager(sim_config) ``` +### Declare, prepare, then step + +Use the same readiness boundary with both physics backends: + +1. Construct the manager and declare the initial assets and sensors. +2. Register any Newton trajectory or contact-material schedules. +3. Call `sim.prepare()` before reading asset state, joint/link metadata, or + native handles. +4. Apply controls and advance time with `sim.update(step=1)`. +5. Release resources when the simulation finishes. + +Newton defers physical model construction until preparation; Default may +materialize assets earlier, but its CUDA buffers also require preparation. +`prepare()` is idempotent for an unchanged scene and binds declared facades in +place. It publishes initial render state without advancing simulation time. +Declare all deformables before the first preparation. After supported topology +changes, prepare again before consuming state and reacquire native views. + +The compatibility methods `init_gpu_physics()` and +`finalize_newton_physics()` delegate to `prepare()`; new examples should use +the shared method. See {doc}`default_physics` for a complete minimal loop and +{doc}`newton_physics` for the matching backend configuration. + ## Profiling simulation updates Configure {class}`ProfilerCfg` directly on the simulation manager when using @@ -202,14 +294,14 @@ EmbodiChain supports importing USD files (`.usd`, `.usda`, `.usdc`) for both rig # Import rigid object with USD properties rigid_cfg = RigidObjectCfg( shape=MeshCfg(fpath=get_data_path("path/to/object.usd")), - use_usd_properties=True # Use properties from USD file + asset_physics_mode="preserve", ) obj = sim.add_rigid_object(cfg=rigid_cfg) # Import articulation with USD properties robot_cfg = ArticulationCfg( fpath=get_data_path("path/to/robot.usd"), - use_usd_properties=True # Use joint drive properties from USD + asset_physics_mode="preserve", ) robot = sim.add_articulation(cfg=robot_cfg) ``` diff --git a/docs/source/overview/sim/sim_rigid_object.md b/docs/source/overview/sim/sim_rigid_object.md index b80f267ba..694cce0d8 100644 --- a/docs/source/overview/sim/sim_rigid_object.md +++ b/docs/source/overview/sim/sim_rigid_object.md @@ -13,30 +13,27 @@ Configured via the {class}`~cfg.RigidObjectCfg` class. | :--- | :--- | :--- | :--- | | `shape` | {class}`~shapes.ShapeCfg` | `ShapeCfg()` | Geometry configuration for visual and collision shapes. Use `MeshCfg` for mesh files or primitive cfgs (e.g., `CubeCfg`). | | `body_type` | `Literal["dynamic","kinematic","static"]` | `"dynamic"` | Actor type for the rigid body. See `{class}`~cfg.RigidObjectCfg.to_dexsim_body_type` for conversion. | -| `attrs` | {class}`~cfg.RigidBodyAttributesCfg` | defaults in code | Physical attributes (mass, damping, friction, restitution, collision offsets, CCD, etc.). | +| `attrs` | {class}`~cfg.RigidBodyPhysicsCfg` | empty groups | Grouped physical attributes (mass, damping, friction, restitution, collision offsets, CCD, etc.). | | `init_pos` | `Sequence[float]` | `(0,0,0)` | Initial root position (x, y, z). | | `init_rot` | `Sequence[float]` | `(0,0,0)` (Euler degrees) | Initial root orientation (Euler angles in degrees) or provide `init_local_pose`. | -| `use_usd_properties` | `bool` | `False` | If True, use physical properties from USD file; if False, override with config values. Only effective for usd files. | +| `asset_physics_mode` | {class}`~cfg.AssetPhysicsMode` | `"preserve"` | Preserve source-authored physics or overlay explicitly configured values. | | `uid` | `str` | `None` | Optional unique identifier for the object; manager will assign one if omitted. | -### Rigid Body Attributes ({class}`~cfg.RigidBodyAttributesCfg`) +### Rigid Body Physics ({class}`~cfg.RigidBodyPhysicsCfg`) -The full attribute set lives in `{class}`~cfg.RigidBodyAttributesCfg`. Common fields shown in code include: +Physical properties are grouped by intent. Every field is optional: `None` +means that a source asset or the active backend keeps ownership of that value. -| Parameter | Type | Default (from code) | Description | -| :--- | :--- | :---: | :--- | -| `mass` | `float` | `1.0` | Mass of the rigid body in kilograms (set to 0 to use density). | -| `density` | `float` | `1000.0` | Density used when mass is negative/zero. | -| `linear_damping` | `float` | `0.7` | Linear damping coefficient. | -| `angular_damping` | `float` | `0.7` | Angular damping coefficient. | -| `dynamic_friction` | `float` | `0.5` | Dynamic friction coefficient. | -| `static_friction` | `float` | `0.5` | Static friction coefficient. | -| `restitution` | `float` | `0.0` | Restitution (bounciness). | -| `contact_offset` | `float` | `0.002` | Contact offset for collision detection. | -| `rest_offset` | `float` | `0.001` | Rest offset for collision detection. | -| `enable_ccd` | `bool` | `False` | Enable continuous collision detection. | +| Group | Example fields | +| :--- | :--- | +| `mass_props` | `mass`, `density`, `inertia`, `com_position`, `com_quaternion` | +| `rigid_props` | `linear_damping`, `angular_damping`, `enable_ccd` | +| `collision_props` | `collision_enabled`, `contact_offset`, `rest_offset` | +| `material_props` | `dynamic_friction`, `static_friction`, `restitution` | -Use the `.attr()` helper to convert to `dexsim.PhysicalAttr` when interfacing with the engine. +COM quaternions are always authored in `xyzw` order. Native engine attributes +are an internal adapter detail. Backend-specific values use the concrete type in +the corresponding property slot rather than a second backend block. ## Setup & Initialization @@ -45,15 +42,26 @@ import torch from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg from embodichain.lab.sim.shapes import CubeCfg -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg +from embodichain.lab.sim.cfg import ( + MassPropertiesCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, +) # 1. Initialize Simulation device = "cuda" if torch.cuda.is_available() else "cpu" -sim_cfg = SimulationManagerCfg(sim_device=device) +sim_cfg = SimulationManagerCfg(device=device) sim = SimulationManager(sim_cfg) # 2. Configure a rigid object (cube) -physics_attrs = RigidBodyAttributesCfg(mass=1.0, dynamic_friction=0.5, static_friction=0.5, restitution=0.1) +physics_attrs = RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), + material_props=RigidBodyMaterialCfg( + dynamic_friction=0.5, + static_friction=0.5, + restitution=0.1, + ), +) cfg = RigidObjectCfg( uid="cube", @@ -66,7 +74,8 @@ cfg = RigidObjectCfg( # 3. Spawn Rigid Object cube: RigidObject = sim.add_rigid_object(cfg=cfg) -# 4. (Optional) Open window and run +# 4. Commit the complete scene, then optionally open a window and run +sim.prepare() if not sim.sim_config.headless: sim.open_window() sim.update() @@ -85,7 +94,7 @@ from embodichain.data import get_data_path usd_cfg = RigidObjectCfg( shape=MeshCfg(fpath=get_data_path("path/to/object.usd")), body_type="dynamic", - use_usd_properties=True # Keep USD properties + asset_physics_mode="preserve", # Keep USD properties ) obj = sim.add_rigid_object(cfg=usd_cfg) @@ -93,8 +102,8 @@ obj = sim.add_rigid_object(cfg=usd_cfg) usd_cfg_override = RigidObjectCfg( shape=MeshCfg(fpath=get_data_path("path/to/object.usd")), body_type="dynamic", - use_usd_properties=False, # Use config instead - attrs=RigidBodyAttributesCfg(mass=2.0) + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=2.0)), ) obj2 = sim.add_rigid_object(cfg=usd_cfg_override) ``` @@ -107,18 +116,18 @@ Rigid objects are observed and controlled via single poses and linear/angular ve | Method / Property | Return / Args | Description | | :--- | :--- | :--- | -| `get_local_pose(to_matrix=False)` | `(N, 7)` or `(N, 4, 4)` | Get object local pose as (x, y, z, qw, qx, qy, qz) or 4x4 matrix per environment. | +| `get_local_pose(to_matrix=False)` | `(N, 7)` or `(N, 4, 4)` | Get object local pose as (x, y, z, qx, qy, qz, qw) or 4x4 matrix per environment. | | `set_local_pose(pose, env_ids=None)` | `pose: (N, 7)` or `(N, 4, 4)` | Teleport object to given pose (requires calling `sim.update()` to apply). | -| `body_data.pose` | `(N, 7)` | Access object pose directly (for dynamic/kinematic bodies). | +| `body_data.pose` | `(N, 7)` | Access object pose as `[x, y, z, qx, qy, qz, qw]` (for dynamic/kinematic bodies). | | `body_data.lin_vel` | `(N, 3)` | Access linear velocity of object root (for dynamic bodies). | | `body_data.ang_vel` | `(N, 3)` | Access angular velocity of object root (for dynamic bodies). | | `body_data.vel` | `(N, 6)` | Concatenated linear and angular velocities. | | `body_data.lin_acc` | `(N, 3)` | Access linear acceleration of object root (for dynamic bodies). | | `body_data.ang_acc` | `(N, 3)` | Access angular acceleration of object root (for dynamic bodies). | | `body_data.acc` | `(N, 6)` | Concatenated linear and angular accelerations. | -| `body_data.com_pose` | `(N, 7)` | Get center of mass pose of rigid bodies. | -| `body_data.default_com_pose` | `(N, 7)` | Default center of mass pose. | -| `body_state` | `(N, 13)` | Get full body state: [x, y, z, qw, qx, qy, qz, lin_x, lin_y, lin_z, ang_x, ang_y, ang_z]. | +| `body_data.com_pose` | `(N, 7)` | Get center of mass pose as `[x, y, z, qx, qy, qz, qw]`. | +| `body_data.default_com_pose` | `(N, 7)` | Default center of mass pose as `[x, y, z, qx, qy, qz, qw]`. | +| `body_state` | `(N, 13)` | Get full body state: [x, y, z, qx, qy, qz, qw, lin_x, lin_y, lin_z, ang_x, ang_y, ang_z]. | ### Dynamics Control @@ -132,7 +141,7 @@ Rigid objects are observed and controlled via single poses and linear/angular ve | Method / Property | Return / Args | Description | | :--- | :--- | :--- | -| `set_attrs(attrs, env_ids=None)` | `attrs: RigidBodyAttributesCfg` | Set physical attributes (mass, friction, damping, etc.). | +| `set_attrs(attrs, env_ids=None)` | `attrs: RigidBodyPhysicsCfg` | Set grouped physical attributes (mass, friction, damping, etc.). | | `set_mass(mass, env_ids=None)` | `mass: (N,)` | Set mass for rigid object. | | `get_mass(env_ids=None)` | `(N,)` | Get mass for rigid object. | | `set_friction(friction, env_ids=None)` | `friction: (N,)` | Set dynamic and static friction. | @@ -185,7 +194,7 @@ When a rigid object is loaded, its material assignment is captured without repla ### Observation Shapes -- Pose: `(N, 7)` per-object pose (position + quaternion). +- Pose: `(N, 7)` per-object pose `[x, y, z, qx, qy, qz, qw]`. - Velocities: `(N, 3)` for linear and angular velocities respectively. N denotes the number of parallel environments when using vectorized simulation (`SimulationManagerCfg.num_envs`). @@ -195,8 +204,9 @@ N denotes the number of parallel environments when using vectorized simulation ( - When moving objects programmatically via `set_local_pose`, call `sim.update()` (or step the sim) to ensure transforms and collision state are synchronized. - Use `static` body type for fixed obstacles or environment pieces (they do not consume dynamic simulation resources). - Use `kinematic` for objects whose pose is driven by code (teleporting or animation) but still interact with dynamic objects. -- For complex meshes, enabling convex decomposition (`RigidObjectCfg.max_convex_hull_num`) or providing a simplified collision mesh improves stability and performance. -- To use GPU physics, ensure `SimulationManagerCfg.sim_device` is set to `cuda` and call `sim.init_gpu_physics()` before large-batch simulations. +- For complex meshes, configure `MeshCfg.collision` with `approximation="convex_decomposition"` and a bounded `max_hulls`, or provide a simplified collision mesh. +- For large GPU batches, select an indexed CUDA device and call the same + backend-neutral `sim.prepare()` boundary after all initial assets are added. ## Example: Applying Force and Torque diff --git a/docs/source/overview/sim/sim_rigid_object_group.md b/docs/source/overview/sim/sim_rigid_object_group.md index e6d61f82f..1effc671f 100644 --- a/docs/source/overview/sim/sim_rigid_object_group.md +++ b/docs/source/overview/sim/sim_rigid_object_group.md @@ -19,7 +19,7 @@ Configured via the {class}`~cfg.RigidObjectGroupCfg` class. | `ext` | `str` | `".obj"` | File extension filter when loading assets from `folder_path`. | | `init_pos` / `init_rot` | `Sequence` (optional) | group-level transform | Optional transform to apply as a base offset to all members. | -Refer to {class}`~cfg.RigidObjectCfg` and {class}`~cfg.RigidBodyAttributesCfg` for per-member configuration options (mass, friction, restitution, collision options, shapes, etc.). +Refer to {class}`~cfg.RigidObjectCfg` and {class}`~cfg.RigidBodyPhysicsCfg` for per-member configuration options (mass, friction, restitution, collision options, shapes, etc.). ### Folder-based initialization @@ -40,19 +40,25 @@ from embodichain.lab.sim.shapes import CubeCfg from embodichain.lab.sim.objects import ( RigidObjectGroup, RigidObjectGroupCfg, RigidObjectCfg ) -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg +from embodichain.lab.sim.cfg import ( + MassPropertiesCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, +) # 1. Initialize Simulation device = "cuda" if torch.cuda.is_available() else "cpu" -sim_cfg = SimulationManagerCfg(sim_device=device) +sim_cfg = SimulationManagerCfg(device=device) sim = SimulationManager(sim_cfg) # 2. Define shared physics attributes -physics_attrs = RigidBodyAttributesCfg( - mass=1.0, - dynamic_friction=0.5, - static_friction=0.5, - restitution=0.1, +physics_attrs = RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), + material_props=RigidBodyMaterialCfg( + dynamic_friction=0.5, + static_friction=0.5, + restitution=0.1, + ), ) # 3. Create group config with multiple members @@ -68,7 +74,8 @@ group_cfg = RigidObjectGroupCfg( # 4. Spawn the rigid object group obj_group: RigidObjectGroup = sim.add_rigid_object_group(cfg=group_cfg) -# 5. Run or step simulation +# 5. Commit the complete scene, then run or step simulation +sim.prepare() sim.update() ``` @@ -83,9 +90,9 @@ A group provides batch operations on multiple rigid objects. Key APIs include: | :--- | :--- | :--- | | `num_objects` | `int` | Number of objects in each group instance. | | `body_data` | `RigidBodyGroupData` | Data manager providing `pose`, `lin_vel`, `ang_vel` properties. | -| `body_state` | `(N, M, 13)` | Full body state of all members: [x, y, z, qw, qx, qy, qz, lin_x, lin_y, lin_z, ang_x, ang_y, ang_z]. | -| `get_local_pose(to_matrix=False)` | `(N, M, 7)` or `(N, M, 4, 4)` | Poses of all members across N envs; M = number of members. | -| `set_local_pose(pose, env_ids=None, obj_ids=None)` | `pose: (N, M, 7)` or `(N, M, 4, 4)` | Set poses for specific environments and/or objects; requires `sim.update()` to apply. | +| `body_state` | `(N, M, 13)` | Full body state of all members: [x, y, z, qx, qy, qz, qw, lin_x, lin_y, lin_z, ang_x, ang_y, ang_z]. | +| `get_local_pose(to_matrix=False)` | `(N, M, 7)` or `(N, M, 4, 4)` | Poses of all members as `[x, y, z, qx, qy, qz, qw]` or matrices; M = number of members. | +| `set_local_pose(pose, env_ids=None, obj_ids=None)` | `pose: (N, M, 7)` or `(N, M, 4, 4)` | Set poses in `[x, y, z, qx, qy, qz, qw]` or matrix form; requires `sim.update()` to apply. | | `get_user_ids()` | `(N, M)` | Get user IDs tensor for all members in the group. | | `clear_dynamics(env_ids=None)` | - | Reset velocities and clear all forces/torques for the group. | | `set_visual_material(mat, env_ids=None)` | `mat: VisualMaterial` | Change visual appearance for all members. | @@ -106,10 +113,12 @@ Use these shapes when collecting vectorized observations for multi-environment t - Groups are convenient for batch operations: resetting, setting visibility, and applying transforms to multiple objects together. - Use `obj_ids` parameter in `set_local_pose()` to control specific objects within the group rather than all members. -- Prefer providing simplified collision meshes or enabling convex decomposition (`max_convex_hull_num` > 1) for complex visual meshes to improve physics stability. +- Prefer simplified collision meshes or an explicit `MeshCfg.collision` convex-decomposition strategy with a bounded `max_hulls` for complex visual meshes. - `RigidObjectGroup` only supports `dynamic` and `kinematic` body types (not `static`). - When teleporting many members, batch pose updates and call `sim.update()` once to avoid synchronization overhead. -- For GPU physics, set `SimulationManagerCfg.sim_device` to `cuda` and call `sim.init_gpu_physics()` before running simulations. +- Add every initial scene asset, then call the backend-neutral `sim.prepare()` + before reading state or stepping. `BaseEnv` performs this boundary + automatically. - Use `clear_dynamics()` to reset velocities without changing poses. ## Example: Working with Group Poses @@ -137,7 +146,11 @@ sim.update() ## Integration with Sensors -Members in a group behave like normal `RigidObject`s: they can be observed by cameras, attached to contact sensors. You can operate on individual members or treat the group as a single unit depending on your scenario. +Members in a group behave like normal `RigidObject`s and can be observed by +cameras. They can also be included in a `ContactSensor` on the Default backend; +Newton currently rejects that sensor through its capability boundary. You can +operate on individual members or treat the group as a single unit depending on +your scenario. ## Related Topics diff --git a/docs/source/overview/sim/sim_robot.md b/docs/source/overview/sim/sim_robot.md index 80c9f842b..ef7e4f43d 100644 --- a/docs/source/overview/sim/sim_robot.md +++ b/docs/source/overview/sim/sim_robot.md @@ -25,9 +25,9 @@ from embodichain.lab.sim.objects import Robot, RobotCfg from embodichain.lab.sim.motion.solvers import SolverCfg # 1. Initialize Simulation Environment -# Note: Use 'sim_device' to specify device (e.g., "cuda:0" or "cpu") +# Note: Use 'device' to specify device (e.g., "cuda:0" or "cpu") device = "cuda" if torch.cuda.is_available() else "cpu" -sim_cfg = SimulationManagerCfg(sim_device=device, physics_dt=0.01) +sim_cfg = SimulationManagerCfg(device=device, physics_dt=0.01) sim = SimulationManager(sim_config=sim_cfg) # 2. Configure Robot @@ -79,7 +79,7 @@ print(f"EE Pose: {ee_pose}") Compute the required joint positions to reach a target pose. ```python # Compute IK -# pose: Target pose (N, 7) or (N, 4, 4) +# pose: Target pose (N, 7) as [x, y, z, qx, qy, qz, qw], or (N, 4, 4) target_pose = ee_pose.clone() # Example target target_pose[:, 2] += 0.1 # Move up 10cm diff --git a/docs/source/overview/sim/sim_sensor.md b/docs/source/overview/sim/sim_sensor.md index 1a8388f5f..632b2b0f0 100644 --- a/docs/source/overview/sim/sim_sensor.md +++ b/docs/source/overview/sim/sim_sensor.md @@ -33,7 +33,7 @@ The `ExtrinsicsCfg` class defines the position and orientation of the camera. | :--- | :--- | :--- | :--- | | `parent` | `str` | `None` | Name of the link to attach to (e.g., `"ee_link"`). If `None`, camera is fixed in world. | | `pos` | `list` | `[0.0, 0.0, 0.0]` | Position offset `[x, y, z]`. | -| `quat` | `list` | `[1.0, 0.0, 0.0, 0.0]` | Orientation quaternion `[w, x, y, z]`. | +| `quat` | `list` | `[0.0, 0.0, 0.0, 1.0]` | Orientation quaternion `[x, y, z, w]`. | | `eye` | `tuple` | `None` | (Optional) Camera eye position for look-at mode. | | `target` | `tuple` | `None` | (Optional) Target position for look-at mode. | | `up` | `tuple` | `None` | (Optional) Up vector for look-at mode. | @@ -55,7 +55,7 @@ camera_cfg = CameraCfg( extrinsics=CameraCfg.ExtrinsicsCfg( parent="ee_link", # Attach to robot end-effector pos=[0.09, 0.05, 0.04], # Relative position - quat=[0, 1, 0, 0], # Relative rotation [w, x, y, z] + quat=[1, 0, 0, 0], # Relative rotation [x, y, z, w] ), enable_color=True, enable_depth=True, @@ -128,7 +128,7 @@ stereo_camera: StereoCamera = sim.add_sensor(sensor_cfg=stereo_cfg) ### Configuration -The {class}`ContactSensorCfg` class defines the configuration for contact sensors. It inherits from {class}`~SensorCfg` and enables filtering and monitoring of contact events between specific rigid bodies and articulation links in the simulation. +The {class}`ContactSensorCfg` class defines the configuration for contact sensors. It inherits from {class}`~SensorCfg` and enables filtering and monitoring of contact events between specific rigid bodies and articulation links in the simulation. The same API works with the Default (DexSim adapter) and Newton physics backends. | Parameter | Type | Default | Description | | :--- | :--- | :--- | :--- | @@ -137,6 +137,10 @@ The {class}`ContactSensorCfg` class defines the configuration for contact sensor | `filter_need_both_actor` | `bool` | `True` | Whether to filter contact only when both actors are in the filter list. If `False`, contact is reported if either actor is in the filter. | | `max_contacts_per_env` | `int` | `64` | Maximum number of contacts per environment that the sensor can handle. | +The sensor forwards `max_contacts_per_env` to DexSim as a per-Arena query +quota. If the global contact buffer is also full, DexSim distributes retained +rows across Arenas before filling later rows from the same Arena. + ### Articulation Contact Filter Configuration The `ArticulationContactFilterCfg` class specifies which articulation links to monitor for contacts. @@ -191,10 +195,17 @@ env_contact_positions = contact_report["position"][env_id][env_valid_mask] valid_mask = contact_report["is_valid"] all_valid_positions = contact_report["position"][valid_mask] # Shape: (total_valid_contacts, 3) -# 4. Filter contacts by specific user IDs -cube2_user_ids = sim.get_rigid_object("cube2").get_user_ids() -finger1_user_ids = sim.get_robot("UR10_PGI").get_user_ids("finger1_link").reshape(-1) -filter_user_ids = torch.cat([cube2_user_ids, finger1_user_ids]) +# 4. Filter contacts by backend-neutral contact actor IDs +filter_user_ids = torch.as_tensor( + [ + actor_id + for actor_id in contact_sensor.item_user_ids.tolist() + if contact_sensor.get_actor_info(actor_id).path.endswith("/cube2") + or contact_sensor.get_actor_info(actor_id).link_name == "finger1_link" + ], + dtype=torch.int32, + device=sim.device, +) # Filter for specific environments filter_contact_report = contact_sensor.filter_by_user_ids(filter_user_ids, env_ids=[env_id]) @@ -214,11 +225,11 @@ Retrieve contact data using `contact_sensor.get_data()`. The data is returned as | Key | Data Type | Shape | Description | | :--- | :--- | :--- | :--- | | `position` | `torch.float32` | `(num_envs, max_contacts_per_env, 3)` | Contact positions in arena frame (world coordinates minus arena offset). | -| `normal` | `torch.float32` | `(num_envs, max_contacts_per_env, 3)` | Contact normal vectors. | -| `friction` | `torch.float32` | `(num_envs, max_contacts_per_env, 3)` | Contact friction forces. *Note: Currently this value may not be accurate.* | -| `impulse` | `torch.float32` | `(num_envs, max_contacts_per_env)` | Contact impulse magnitudes. | -| `distance` | `torch.float32` | `(num_envs, max_contacts_per_env)` | Contact penetration distances. | -| `user_ids` | `torch.int32` | `(num_envs, max_contacts_per_env, 2)` | Pair of user IDs for the two actors in contact. Use with `rigid_object.get_user_ids()` to identify objects. | +| `normal` | `torch.float32` | `(num_envs, max_contacts_per_env, 3)` | Unit normal vectors pointing from actor 0 toward actor 1. | +| `friction` | `torch.float32` | `(num_envs, max_contacts_per_env, 3)` | Tangential contact impulse applied to actor 0. Availability is reported by `contact_capabilities.friction`. | +| `impulse` | `torch.float32` | `(num_envs, max_contacts_per_env)` | Backend contact impulse magnitudes. Default CPU reports the total impulse norm; Direct GPU and force-reporting Newton solvers report normal impulse magnitude. | +| `distance` | `torch.float32` | `(num_envs, max_contacts_per_env)` | Signed contact separation (negative means penetration). | +| `user_ids` | `torch.int32` | `(num_envs, max_contacts_per_env, 2)` | Pair of query-local, backend-neutral contact actor IDs. The legacy field name is retained; resolve IDs with `get_actor_info()`. | | `is_valid` | `torch.bool` | `(num_envs, max_contacts_per_env)` | Boolean mask indicating which contact slots contain valid data. Use this mask to filter out unused slots. | **Note**: Use the `is_valid` mask to access only valid contacts: @@ -233,7 +244,24 @@ num_valid = contact_report["is_valid"][env_id].sum().item() env_positions = contact_report["position"][env_id, :num_valid] ``` +Force-capable backends preserve every backend-emitted contact row that passes +the positive-impulse filter. Default CPU uses a total-impulse norm threshold of +`1e-7`; Default Direct GPU and force-reporting Newton solvers use the same +threshold on normal impulse. Geometry-only Newton solvers retain all candidate +rows with zero impulse. The sensor does not synthesize a common contact +manifold: solver options such as MuJoCo-Warp's `enable_multiccd` control how +many points the backend emits for a geometry pair. Multiple contact points and +shape pairs can map to the same actor pair. The fixed-size numeric buffers are +not cleared every update; values where `is_valid=False` are unspecified and +may be left over from an earlier update. + ### Additional Methods -- **`filter_by_user_ids(item_user_ids, env_ids=None)`**: Filter contact report to include only contacts involving specific user IDs. Optionally filter by specific environment IDs. -- **`set_contact_point_visibility(visible, rgba, point_size, env_ids=None)`**: Enable/disable visualization of contact points with customizable color and size. Optionally visualize only specific environments. \ No newline at end of file +- **`get_actor_info(actor_id)`**: Resolve a contact actor ID to its Spawn path, articulation link, Arena, and environment ID. +- **`contact_capabilities`**: Report whether geometry, normal impulse, and friction impulse are available for the active backend/solver. +- **`filter_by_user_ids(item_user_ids, env_ids=None)`**: Filter contact report by contact actor IDs. The method name is retained for compatibility. Optionally filter by specific environment IDs. +- **`set_contact_point_visibility(visible, rgba, point_size, env_ids=None)`**: Enable/disable visualization of contact points with customizable color and size. Optionally visualize only specific environments. + +Newton MuJoCo-Warp exposes contact forces, so both impulse fields are available. Other supported Newton rigid solvers currently expose contact geometry with zero-valued impulse fields. MuJoCo CPU mode does not expose device contact buffers, and DexUni does not currently publish rigid contacts through `ContactQuery`; those modes are therefore unsupported by this sensor. + +Default Direct GPU reports static counterparts with actor ID `-1` because its raw contact buffer does not expose their object identity. To monitor a dynamic body or articulation link against arbitrary static geometry, select the dynamic/link object and set `filter_need_both_actor=False`. Default CPU and Newton can identify registered static shapes. diff --git a/docs/source/overview/sim/sim_soft_object.md b/docs/source/overview/sim/sim_soft_object.md index 5936321d4..4126e18ae 100644 --- a/docs/source/overview/sim/sim_soft_object.md +++ b/docs/source/overview/sim/sim_soft_object.md @@ -3,95 +3,82 @@ ```{currentmodule} embodichain.lab.sim ``` -The {class}`~objects.SoftObject` class represents deformable entities (e.g. sponges, soft robotics) in EmbodiChain. Unlike rigid bodies, soft objects are defined by vertices and meshes rather than a single rigid pose. +{class}`~objects.VolumeDeformableObject` represents batched Newton volumetric +particle sets. Volume deformables +require the Newton backend on CUDA and a particle-capable solver. -## Configuration - -Configured via {class}`~cfg.SoftObjectCfg`. - -| Parameter | Type | Default | Description | -| :--- | :--- | :--- | :--- | -| `voxel_attr` | `SoftbodyVoxelAttributesCfg` | `...` | Voxelization attributes. | -| `physical_attr` | `SoftbodyPhysicalAttributesCfg` | `...` | Physical attributes. | -| `shape` | `MeshCfg` | `MeshCfg()` | Mesh configuration. | - -### Soft Body Attributes - -Soft bodies require both voxelization and physical attributes. - -**Voxel Attributes ({class}`~cfg.SoftbodyVoxelAttributesCfg`)** - -| Parameter | Type | Default | Description | -| :--- | :--- | :--- | :--- | -| `triangle_remesh_resolution` | `int` | `8` | Resolution to remesh the softbody mesh before building physics collision mesh. | -| `triangle_simplify_target` | `int` | `0` | Simplify mesh faces to target value. | -| `simulation_mesh_resolution` | `int` | `8` | Resolution to build simulation voxelize textra mesh. | -| `simulation_mesh_output_obj` | `bool` | `False` | Whether to output the simulation mesh as an obj file for debugging. | - -**Physical Attributes ({class}`~cfg.SoftbodyPhysicalAttributesCfg`)** +See {doc}`newton_physics` for solver selection, mixed rigid/deformable scene +limitations, and the requirement to declare deformables before preparation. -| Parameter | Type | Default | Description | -| :--- | :--- | :--- | :--- | -| `youngs` | `float` | `1e6` | Young's modulus (higher = stiffer). | -| `poissons` | `float` | `0.45` | Poisson's ratio (higher = closer to incompressible). | -| `dynamic_friction` | `float` | `0.0` | Dynamic friction coefficient. | -| `elasticity_damping` | `float` | `0.0` | Elasticity damping factor. | -| `material_model` | `SoftBodyMaterialModel` | `CO_ROTATIONAL` | Material constitutive model. | -| `enable_kinematic` | `bool` | `False` | If True, (partially) kinematic behavior is enabled. | -| `enable_ccd` | `bool` | `False` | Enable continuous collision detection. | -| `enable_self_collision` | `bool` | `False` | Enable self-collision handling. | -| `mass` | `float` | `-1.0` | Total mass. If negative, density is used. | -| `density` | `float` | `1000.0` | Material density in kg/m^3. | - -For a runnable example, see the {doc}`Soft Body Simulation ` tutorial. +## Configuration +{class}`~cfg.VolumeDeformableObjectCfg` separates physical parameters under +`attrs` from mesh generation under `meshing`. -### Setup & Initialization +| Field | Default | Meaning | +| :--- | :--- | :--- | +| `attrs.density` | `1000.0` | Volume density in kg/m³. | +| `attrs.youngs` | `1e6` | Young's modulus in Pa. | +| `attrs.poissons` | `0.45` | Poisson's ratio in (-1, 0.5). | +| `attrs.elasticity_damping` | `0.0` | Volumetric damping. | +| `attrs.surface_props` | Seven zero coefficients | Optional Newton surface forces. | +| `attrs.add_surface_edges` | `True` | Create surface bending-edge constraints. | +| `meshing.triangle_remesh_resolution` | `8` | Source surface remeshing resolution. | +| `meshing.triangle_simplify_target` | `0` | Target proxy face count; zero disables simplification. | +| `meshing.simulation_mesh_resolution` | `8` | Voxel resolution for the tetrahedral mesh. | +| `meshing.voxel_num_relaxation_iters` | `5` | Tetrahedral-mesh relaxation iterations. | +| `meshing.voxel_rel_min_tet_volume` | `0.05` | Relative minimum tetrahedral volume. | +| `meshing.voxel_surface_dist_ratio` | `0.2` | Surface distance as a voxel-size ratio. | +| `meshing.embedding_impl` | `dexsim_exact_cpu` | Render-to-volume binding implementation. | + +`attrs.surface_props` uses the same +{class}`~cfg.SurfaceElementPropertiesCfg` as cloth. Volume defaults are +zero rather than cloth's `None`; explicitly omitted coefficients in a partial +surface group also resolve to zero for volume descriptors. ```python -import torch -from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.objects import SoftObject, SoftObjectCfg - -# 1. Initialize Simulation -device = "cuda" if torch.cuda.is_available() else "cpu" -sim_cfg = SimulationManagerCfg(sim_device=device) -sim = SimulationManager(sim_config=sim_cfg) - -# 2. Configure Soft Object -soft_cfg = SoftObjectCfg( - fpath="assets/objects/sponge.msh", # Example asset path - init_pos=(0, 0, 0.5), - init_rot=(0, 0, 0) +from embodichain.lab.sim.cfg import ( + VolumeDeformableObjectCfg, + VolumeDeformablePhysicsCfg, + VolumeDeformableMeshingCfg, +) +from embodichain.lab.sim.shapes import MeshCfg + +cfg = VolumeDeformableObjectCfg( + uid="soft_body", + shape=MeshCfg(fpath="body.obj"), + attrs=VolumeDeformablePhysicsCfg( + youngs=1e5, poissons=0.4, density=75.0, + ), + meshing=VolumeDeformableMeshingCfg(simulation_mesh_resolution=12), ) +``` -# 3. Spawn Soft Object -# Note: Assuming the method in SimulationManager is 'add_soft_object' -soft_object: SoftObject = sim.add_soft_object(cfg=soft_cfg) +Add this configuration through `sim.add_deformable_object(cfg)` and call +`sim.prepare()` before reading object data. For a runnable example, see +{doc}`Soft Body Simulation `. -# 4. Initialize Physics -sim.reset_objects_state() -``` ### Soft Object Class -#### Vertex Data (Observation) -For soft objects, the state is represented by the positions and velocities of its vertices, rather than a single root pose. +#### Nodal State + +After `sim.prepare()`, read simulation nodes through `object.data`. Each state +property returns an independent tensor snapshot. -| Method | Return Shape | Description | +| Property | Shape | Description | | :--- | :--- | :--- | -| `get_current_collision_vertices()` | `(N, V_col, 3)` | Current positions of collision mesh vertices. | -| `get_current_sim_vertices()` | `(N, V_sim, 3)` | Current positions of simulation mesh vertices (nodes). | -| `get_current_sim_vertex_velocities()` | `(N, V_sim, 3)` | Current velocities of simulation vertices. | -| `get_rest_collision_vertices()` | `(N, V_col, 3)` | Rest (initial) positions of collision vertices. | -| `get_rest_sim_vertices()` | `(N, V_sim, 3)` | Rest (initial) positions of simulation vertices. | +| `data.nodal_pos_w` | `(N, V, 3)` | Current simulation-node positions in world coordinates. | +| `data.nodal_vel_w` | `(N, V, 3)` | Current simulation-node velocities in world coordinates. | +| `data.default_nodal_state_w` | `(N, V, 6)` | Positions and velocities captured at Spawn binding. | -> Note: N is the number of environments/instances, V_col is the number of collision vertices, and V_sim is the number of simulation vertices. +`N` is the number of instances; `V` is `data.n_nodes`. Render vertices are +separate and are read through `get_surface_vertices()`. ```python # Example: Accessing vertex data -sim_verts = soft_object.get_current_sim_vertices() +sim_verts = soft_object.data.nodal_pos_w print(f"Simulation Vertices Shape: {sim_verts.shape}") -velocities = soft_object.get_current_sim_vertex_velocities() +velocities = soft_object.data.nodal_vel_w print(f"Vertex Velocities: {velocities}") ``` @@ -118,7 +105,7 @@ You can set the global pose of a soft object (which transforms all its vertices) ```python # Reset or Move the Soft Object -target_pose = torch.tensor([[0, 0, 1.0, 1, 0, 0, 0]], device=device) # (x, y, z, qw, qx, qy, qz) +target_pose = torch.tensor([[0, 0, 1.0, 0, 0, 0, 1]], device=device) # (x, y, z, qx, qy, qz, qw) soft_object.set_local_pose(target_pose) # Important: Step simulation to apply changes diff --git a/docs/source/overview/sim/viser_visualization.md b/docs/source/overview/sim/viser_visualization.md index 407054e6c..441b87a7e 100644 --- a/docs/source/overview/sim/viser_visualization.md +++ b/docs/source/overview/sim/viser_visualization.md @@ -101,7 +101,7 @@ The browser scene currently includes: - `RigidObject`, including multi-segment render meshes; - each constituent object in a `RigidObjectGroup`; - every visible link of `Robot` and `Articulation`; -- dynamic `SoftObject` and `ClothObject` geometry; +- dynamic `VolumeDeformableObject` and `SurfaceDeformableObject` geometry; - camera frustums and low-frequency RGB previews, including the primary (left) RGB view of stereo sensors; - read-only Gizmo frames, or interactive transform controls when commands are @@ -179,7 +179,7 @@ sampled independently from rigid-body poses: - **Cloth** uses the physical cloth vertices and a welded mapping of the source render triangles. Its browser topology matches the simulated surface. -- **Soft bodies** expose live PhysX collision vertices through DexSim, but +- **Soft bodies** expose live DexSim collision vertices, but DexSim does not expose the collision triangle connectivity. EmbodiChain therefore visualizes a stable convex-hull surface over those vertices. The preview follows deformation but omits concave render-mesh details. diff --git a/docs/source/overview/task_program/index.md b/docs/source/overview/task_program/index.md index f7117c244..544189f66 100644 --- a/docs/source/overview/task_program/index.md +++ b/docs/source/overview/task_program/index.md @@ -96,9 +96,9 @@ targets: kind: cyclic_pose values: - position: [-0.40, 0.48, 0.10] - quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + quaternion_xyzw: [0.0, 0.0, 0.0, 1.0] - position: [-0.42, -0.08, 0.10] - quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + quaternion_xyzw: [0.0, 0.0, 0.0, 1.0] program: kind: repeat count: 3 diff --git a/docs/source/resources/robot/cobotmagic.md b/docs/source/resources/robot/cobotmagic.md index 4ec5cd8d4..c1cf4f94b 100644 --- a/docs/source/resources/robot/cobotmagic.md +++ b/docs/source/resources/robot/cobotmagic.md @@ -39,7 +39,7 @@ CobotMagic is a versatile dual-arm collaborative robot developed by AgileX Robot from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.robots import CobotMagicCfg -config = SimulationManagerCfg(headless=False, sim_device="cpu", num_envs=2) +config = SimulationManagerCfg(headless=False, device="cpu", num_envs=2) sim = SimulationManager(config) robot = sim.add_robot(cfg=CobotMagicCfg().from_dict({})) @@ -58,7 +58,7 @@ sim.update(step=1) - **urdf_cfg**: URDF configuration, supports multi-component assembly (e.g., dual arms) - **control_parts**: Control groups for independent control of each arm and gripper - **solver_cfg**: Inverse kinematics solver configuration, customizable end-effector and base -- **drive_pros**: Joint drive properties (stiffness, damping, max effort, etc.) +- **joint_drive_props**: Joint drive properties (stiffness, damping, max effort, etc.) - **attrs**: Rigid body physical attributes (mass, friction, damping, etc.) ### 2. Custom Usage Example diff --git a/docs/source/tutorial/articulation.rst b/docs/source/tutorial/articulation.rst index c5477c064..50645a39b 100644 --- a/docs/source/tutorial/articulation.rst +++ b/docs/source/tutorial/articulation.rst @@ -44,7 +44,7 @@ Loading the URDF Resolve the bundled drawer asset, then pass its path to :class:`cfg.ArticulationCfg`. The example intentionally does not set -``drive_pros``. Therefore the configuration uses the Articulation default, +``joint_drive_props``. Therefore the configuration uses the Articulation default, ``drive_type="none"``. ``SimulationManager.add_articulation`` loads one drawer into each configured environment and returns a batched :class:`objects.Articulation` handle. @@ -62,7 +62,7 @@ effective physics limit used by both the backend and the force-control loop. Verifying the constructed drive type ------------------------------------ -Checking ``articulation.cfg.drive_pros.drive_type`` confirms the requested +Checking ``articulation.cfg.joint_drive_props.drive_type`` confirms the requested configuration, but it does not prove what the physics backend received. The example therefore calls :meth:`objects.Articulation.get_joint_drive_type`, which reads the drive type from every constructed DexSim entity. It raises an @@ -158,7 +158,7 @@ articulation needs an actuator, opt in with articulation_cfg = ArticulationCfg( fpath="path/to/articulation.urdf", - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( drive_type="force", stiffness=1.0e4, damping=1.0e3, @@ -170,8 +170,8 @@ For a controllable robot, prefer :class:`cfg.RobotCfg` and .. attention:: - For USD assets, ``use_usd_properties=True`` preserves the drive types stored - in the USD file instead of applying the Articulation configuration default. + For file-backed assets, ``asset_physics_mode="preserve"`` keeps source + physics, while ``"overlay"`` applies explicitly configured values. Next Steps ~~~~~~~~~~ diff --git a/docs/source/tutorial/create_cloth.rst b/docs/source/tutorial/create_cloth.rst index 181a1eaf4..6508904d1 100644 --- a/docs/source/tutorial/create_cloth.rst +++ b/docs/source/tutorial/create_cloth.rst @@ -48,17 +48,19 @@ The simulation environment is configured with :class:`SimulationManagerCfg`. For Adding a cloth object to the scene ------------------------------------ -The grid mesh generated earlier is saved to disk and then passed to :meth:`SimulationManager.add_cloth_object`. The physical properties of the cloth are controlled through :class:`cfg.ClothObjectCfg` together with :class:`cfg.ClothPhysicalAttributesCfg`: +The grid mesh generated earlier is saved to disk and then passed to :meth:`SimulationManager.add_deformable_object`. The physical properties of the cloth are controlled through :class:`cfg.SurfaceDeformableObjectCfg` together with :class:`cfg.SurfaceDeformablePhysicsCfg`: - :class:`cfg.MeshCfg` — references the ``.ply`` file written to the system temp directory -- :class:`cfg.ClothPhysicalAttributesCfg` — material parameters: +- :class:`cfg.SurfaceDeformablePhysicsCfg` — material parameters: - - ``mass`` — total mass of the cloth panel (kg) - - ``youngs`` / ``poissons`` — elastic stiffness and compressibility - - ``thickness`` — collision thickness of the cloth surface - - ``bending_stiffness`` / ``bending_damping`` — resistance to and dissipation of bending motion - - ``dynamic_friction`` — friction between the cloth and other objects - - ``min_position_iters`` — solver iteration count for position constraints + - ``density`` — surface density (kg/m²) + - ``surface_props.tri_ke`` / ``tri_ka`` — triangle elastic and area stiffness + - ``surface_props.tri_kd`` — triangle damping + - ``surface_props.edge_ke`` / ``edge_kd`` — bending stiffness and damping + - ``add_springs`` / ``spring_ke`` / ``spring_kd`` — optional mesh springs + +The object stores these properties in ``attrs``. Set ``particle_radius`` on +``SurfaceDeformableObjectCfg`` to control the particle contact radius. .. literalinclude:: ../../../scripts/tutorials/sim/create_cloth.py :language: python @@ -68,7 +70,7 @@ The grid mesh generated earlier is saved to disk and then passed to :meth:`Simul Adding a rigid body for interaction ------------------------------------- -A small cubic rigid body (``padding_box``) is placed beneath the cloth so the cloth drapes over it. It is added with :meth:`SimulationManager.add_rigid_object` using :class:`cfg.RigidObjectCfg` and :class:`cfg.RigidBodyAttributesCfg`: +A small cubic rigid body (``padding_box``) is placed beneath the cloth so the cloth drapes over it. It is added with :meth:`SimulationManager.add_rigid_object` using :class:`cfg.RigidObjectCfg` and :class:`cfg.RigidBodyPhysicsCfg`: - :class:`cfg.CubeCfg` — defines the box dimensions - ``body_type="dynamic"`` — the box responds to physics; change to ``"static"`` for a fixed obstacle diff --git a/docs/source/tutorial/create_softbody.rst b/docs/source/tutorial/create_softbody.rst index 41077a89f..82a373965 100644 --- a/docs/source/tutorial/create_softbody.rst +++ b/docs/source/tutorial/create_softbody.rst @@ -35,11 +35,11 @@ The first step is to configure the simulation environment. This is done using th Adding a soft body to the scene ------------------------------- -With the simulation context created, we can add a soft (deformable) object. This tutorial demonstrates adding a soft-body cow mesh to the scene using the :meth:`SimulationManager.add_soft_object` method. The object's geometry and physical parameters are defined through configuration objects: +With the simulation context created, we can add a soft (deformable) object. This tutorial demonstrates adding a soft-body cow mesh to the scene using the :meth:`SimulationManager.add_deformable_object` method. The object's geometry and physical parameters are defined through configuration objects: - :class:`cfg.MeshCfg` for the mesh shape (``cow.obj``) -- :class:`cfg.SoftbodyVoxelAttributesCfg` for voxelization and simulation mesh resolution -- :class:`cfg.SoftbodyPhysicalAttributesCfg` for material properties (Young's modulus, Poisson's ratio, density, frictions, solver iterations) +- :class:`cfg.VolumeDeformableMeshingCfg` for voxelization and simulation mesh resolution +- :class:`cfg.VolumeDeformablePhysicsCfg` for ``attrs`` (``youngs``, ``poissons``, volume density, and ``surface_props``) .. literalinclude:: ../../../scripts/tutorials/sim/create_softbody.py :language: python diff --git a/docs/source/tutorial/rigid_constraint.rst b/docs/source/tutorial/rigid_constraint.rst index 500558a6c..15afc78d2 100644 --- a/docs/source/tutorial/rigid_constraint.rst +++ b/docs/source/tutorial/rigid_constraint.rst @@ -47,7 +47,7 @@ Adding two cubes Two dynamic cubes are added with :meth:`SimulationManager.add_rigid_object`. Each uses a :class:`CubeCfg` shape (a primitive cube, so no mesh asset file is -needed) and a :class:`RigidBodyAttributesCfg` for mass and friction. ``cube_a`` +needed) and a :class:`RigidBodyPhysicsCfg` for mass and friction. ``cube_a`` is placed slightly higher than ``cube_b`` so that, once detached, the lower cube lands first and the relative pose visibly changes. diff --git a/docs/source/tutorial/robot.rst b/docs/source/tutorial/robot.rst index cd3f277ac..5875f9b3a 100644 --- a/docs/source/tutorial/robot.rst +++ b/docs/source/tutorial/robot.rst @@ -63,7 +63,7 @@ Drive properties control how the robot's joints behave during simulation, includ .. literalinclude:: ../../../scripts/tutorials/sim/create_robot.py :language: python - :start-at: drive_pros=JointDrivePropertiesCfg( + :start-at: joint_drive_props=JointDrivePropertiesCfg( :end-at: ) You can set different stiffness values for different joint groups using regex patterns. More details on drive properties can be found in :class:`cfg.JointDrivePropertiesCfg`. diff --git a/docs/source/tutorial/task_program.rst b/docs/source/tutorial/task_program.rst index d64fbfb0d..a13b5eb96 100644 --- a/docs/source/tutorial/task_program.rst +++ b/docs/source/tutorial/task_program.rst @@ -66,9 +66,9 @@ callables: kind: cyclic_pose values: - position: [-0.40, 0.48, 0.10] - quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + quaternion_xyzw: [0.0, 0.0, 0.0, 1.0] - position: [-0.42, -0.08, 0.10] - quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + quaternion_xyzw: [0.0, 0.0, 0.0, 1.0] program: kind: repeat count: 3 diff --git a/embodichain/data/assets/demo_assets.py b/embodichain/data/assets/demo_assets.py index 255002ab6..75d3637bb 100644 --- a/embodichain/data/assets/demo_assets.py +++ b/embodichain/data/assets/demo_assets.py @@ -14,6 +14,8 @@ # limitations under the License. # ---------------------------------------------------------------------------- +"""Registered downloadable asset bundles for standalone demos.""" + from __future__ import annotations import open3d as o3d @@ -27,9 +29,23 @@ demo_assets = "demo" +__all__ = [ + "CoordinatedPlacementAndPickment", + "DeformableDemoData", + "MultiW1Data", + "ScoopIceNewEnv", +] + class ScoopIceNewEnv(EmbodiChainDataset): - def __init__(self, data_root: str = None): + """Downloadable meshes and robot assets for the scoop-ice demo.""" + + def __init__(self, data_root: str | None = None) -> None: + """Initialize the scoop-ice asset bundle. + + Args: + data_root: Optional cache root overriding the EmbodiChain default. + """ data_descriptor = o3d.data.DataDescriptor( os.path.join( EMBODICHAIN_DOWNLOAD_PREFIX, demo_assets, "ScoopIceNewEnv.zip" @@ -43,7 +59,14 @@ def __init__(self, data_root: str = None): class MultiW1Data(EmbodiChainDataset): - def __init__(self, data_root: str = None): + """Downloadable scene assets for multi-W1 manipulation demos.""" + + def __init__(self, data_root: str | None = None) -> None: + """Initialize the multi-W1 demo asset bundle. + + Args: + data_root: Optional cache root overriding the EmbodiChain default. + """ data_descriptor = o3d.data.DataDescriptor( os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, demo_assets, "multi_w1_demo.zip"), "984e8fa3aa05cb36a1fd973a475183ed", @@ -53,10 +76,37 @@ def __init__(self, data_root: str = None): super().__init__(prefix, data_descriptor, path) +class DeformableDemoData(EmbodiChainDataset): + """Shared cloth-twist and W1 T-shirt-folding demo assets.""" + + def __init__(self, data_root: str | None = None) -> None: + """Initialize the downloadable deformable-demo bundle. + + Args: + data_root: Optional cache root overriding the EmbodiChain default. + """ + data_descriptor = o3d.data.DataDescriptor( + os.path.join( + EMBODICHAIN_DOWNLOAD_PREFIX, + demo_assets, + "deformable_demo_assets.zip", + ), + "cdb1d1b105f0e96f46945052296da4d3", + ) + prefix = type(self).__name__ + path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root + super().__init__(prefix, data_descriptor, path) + + class CoordinatedPlacementAndPickment(EmbodiChainDataset): - """Dataset class for coordinated placement and pickment tutorial meshes.""" + """Downloadable meshes for coordinated placement and pickment tutorials.""" + + def __init__(self, data_root: str | None = None) -> None: + """Initialize the coordinated manipulation asset bundle. - def __init__(self, data_root: str = None): + Args: + data_root: Optional cache root overriding the EmbodiChain default. + """ data_descriptor = o3d.data.DataDescriptor( os.path.join( EMBODICHAIN_DOWNLOAD_PREFIX, diff --git a/embodichain/data_pipeline/engine/data.py b/embodichain/data_pipeline/engine/data.py index fb73a94d6..3b4e21f18 100644 --- a/embodichain/data_pipeline/engine/data.py +++ b/embodichain/data_pipeline/engine/data.py @@ -27,7 +27,7 @@ from dataclasses import dataclass from enum import Enum from types import TracebackType -from typing import Literal +from typing import TYPE_CHECKING, Literal from multiprocessing.sharedctypes import Synchronized, SynchronizedArray from multiprocessing.synchronize import Event as MpEvent @@ -45,6 +45,9 @@ "OnlineDataWorkerError", ] +if TYPE_CHECKING: + from embodichain.lab.sim import SimulationManagerCfg + _ERROR_BUFFER_SIZE = 64 * 1024 @@ -66,6 +69,19 @@ class OnlineDataWorkerError(RuntimeError): """Fallback error for a worker exception that cannot be reconstructed.""" +def _add_exception_note(error: BaseException, note: str) -> None: + """Attach a PEP 678-style note on every supported Python version.""" + add_note = getattr(error, "add_note", None) + if add_note is not None: + add_note(note) + return + notes = getattr(error, "__notes__", None) + if notes is None: + notes = [] + error.__notes__ = notes + notes.append(note) + + def _forced_shutdown_error() -> OnlineDataWorkerError: """Build the error used when graceful worker durability is unknown.""" return OnlineDataWorkerError( @@ -226,6 +242,29 @@ class OnlineDataEngineCfg: """Maximum seconds to wait for the worker's initial buffer fill.""" +def _apply_worker_simulation_overrides( + sim_cfg: "SimulationManagerCfg", + gym_config: dict[str, object], +) -> None: + """Apply worker-only overrides without replacing the typed physics config. + + ``config_to_cfg`` has already selected and decoded the backend before the + worker starts. Mutating that instance keeps backend-specific defaults + (notably Newton's ``cuda:0`` device) and physics settings intact while + still applying the worker's headless/render/GPU options. + """ + sim_cfg.headless = bool(gym_config.get("headless", True)) + sim_cfg.render_cfg.renderer = str(gym_config.get("renderer", "hybrid")) + sim_cfg.gpu_id = int(gym_config.get("gpu_id", 0)) + + # ``None`` means that no runtime override was authored. Leaving the + # value untouched is what allows NewtonPhysicsCfg's CUDA default to win + # over PhysicsBackendCfg's generic CPU default. + device = gym_config.get("device") + if device is not None: + sim_cfg.device = device + + # --------------------------------------------------------------------------- # Subprocess entry point (module-level so it can be pickled by multiprocessing) # --------------------------------------------------------------------------- @@ -270,8 +309,6 @@ def _run_sim_worker( get_manager_modules, ) from embodichain.lab.gym.envs.demo import execute_demo_episode - from embodichain.lab.sim import SimulationManagerCfg - from embodichain.lab.sim.cfg import RenderCfg from embodichain.utils.logger import log_info, log_warning gym_config: dict = cfg.gym_config @@ -285,12 +322,7 @@ def _run_sim_worker( # Otherwise a longer successful plan is silently clipped by the writer and # published as if the complete episode had been stored. env_cfg.max_episode_steps = int(shared_buffer.batch_size[1]) - env_cfg.sim_cfg = SimulationManagerCfg( - headless=gym_config.get("headless", True), - sim_device=gym_config.get("device", "cpu"), - render_cfg=RenderCfg(renderer=gym_config.get("renderer", "hybrid")), - gpu_id=gym_config.get("gpu_id", 0), - ) + _apply_worker_simulation_overrides(env_cfg.sim_cfg, gym_config) num_envs: int = env_cfg.num_envs buffer_size: int = shared_buffer.batch_size[0] @@ -700,8 +732,8 @@ def start(self) -> None: forced_shutdown = self._shutdown_worker() except BaseException as caught_cleanup_error: cleanup_error = caught_cleanup_error - error.add_note( - f"Worker cleanup also failed: {caught_cleanup_error}" + _add_exception_note( + error, f"Worker cleanup also failed: {caught_cleanup_error}" ) else: self._cleanup_complete = True @@ -713,9 +745,10 @@ def start(self) -> None: # primary, but never lose that late durability error. channel_error = self._receive_worker_error() if channel_error is not None and channel_error is not error: - error.add_note( + _add_exception_note( + error, "Worker also failed during cleanup: " - f"{type(channel_error).__name__}: {channel_error}" + f"{type(channel_error).__name__}: {channel_error}", ) if forced_shutdown: @@ -723,7 +756,7 @@ def start(self) -> None: if channel_error is None: self._record_worker_error(durability_error) channel_error = durability_error - error.add_note(str(durability_error)) + _add_exception_note(error, str(durability_error)) if ( stop_requested @@ -1248,12 +1281,12 @@ def stop(self) -> None: self._record_worker_error(durability_error) worker_error = durability_error else: - worker_error.add_note(str(durability_error)) + _add_exception_note(worker_error, str(durability_error)) self._set_state(OnlineDataEngineState.FAILED) self._lifecycle_condition.notify_all() if worker_error is not None: - worker_error.add_note( - f"Worker cleanup also failed: {cleanup_error}" + _add_exception_note( + worker_error, f"Worker cleanup also failed: {cleanup_error}" ) raise worker_error self._worker_error = cleanup_error @@ -1269,7 +1302,7 @@ def stop(self) -> None: self._record_worker_error(durability_error) worker_error = durability_error else: - worker_error.add_note(str(durability_error)) + _add_exception_note(worker_error, str(durability_error)) self._cleanup_complete = True if worker_error is not None: @@ -1298,9 +1331,10 @@ def __exit__( except BaseException as cleanup_error: if exc_value is None: raise - exc_value.add_note( + _add_exception_note( + exc_value, "OnlineDataEngine cleanup also failed: " - f"{type(cleanup_error).__name__}: {cleanup_error}" + f"{type(cleanup_error).__name__}: {cleanup_error}", ) return None diff --git a/embodichain/gen_sim/gradio_ui/app_articraft.py b/embodichain/gen_sim/gradio_ui/app_articraft.py index f39653df5..dfa6b92e2 100644 --- a/embodichain/gen_sim/gradio_ui/app_articraft.py +++ b/embodichain/gen_sim/gradio_ui/app_articraft.py @@ -742,7 +742,8 @@ def _start_remote_viser_preview(session_id: str, artifact: Path) -> str: str(artifact.resolve()), "--asset_type", "articulation", - "--use_usd_properties", + "--asset-physics-mode", + "preserve", "--viser", "--viser-host", "0.0.0.0", diff --git a/embodichain/gen_sim/gradio_ui/gradio_visualization_architecture.md b/embodichain/gen_sim/gradio_ui/gradio_visualization_architecture.md index 81700e2db..8671993d3 100644 --- a/embodichain/gen_sim/gradio_ui/gradio_visualization_architecture.md +++ b/embodichain/gen_sim/gradio_ui/gradio_visualization_architecture.md @@ -134,7 +134,7 @@ description + optional image → articraft view → Gradio iframe ``` -两种方式的本地产物均在 `ARTICRAFT_OUTPUT_ROOT` 下。Remote server 的 USDC sidecar 将单个 assembly 设为 `defaultPrim` 和 articulation root，Gradio 使用当前 Python 环境启动 `embodichain preview-asset --asset_type articulation --use_usd_properties --viser`，并把动态 Viser 端口嵌入页面；Viser 启动失败时仍保留成功 USDC，并回退到结果摘要。Local Codex 继续使用隔离的 Articraft Conda 环境和原生 USDZ Viewer。 +两种方式的本地产物均在 `ARTICRAFT_OUTPUT_ROOT` 下。Remote server 的 USDC sidecar 将单个 assembly 设为 `defaultPrim` 和 articulation root，Gradio 使用当前 Python 环境启动 `embodichain preview-asset --asset_type articulation --asset-physics-mode preserve --viser`，并把动态 Viser 端口嵌入页面；Viser 启动失败时仍保留成功 USDC，并回退到结果摘要。Local Codex 继续使用隔离的 Articraft Conda 环境和原生 USDZ Viewer。 `Reset Articulation` 会清空当前会话的描述、参考图、记录与下载结果，终止该会话的 Articraft 生成、Articraft/Viser Viewer 进程组，并请求取消仍在运行的远程任务。新请求替换旧请求时也执行相同的会话级取消。 diff --git a/embodichain/gen_sim/scene_engine/cli/preview.py b/embodichain/gen_sim/scene_engine/cli/preview.py index 9804293ab..ad433ade5 100644 --- a/embodichain/gen_sim/scene_engine/cli/preview.py +++ b/embodichain/gen_sim/scene_engine/cli/preview.py @@ -26,7 +26,14 @@ from typing import TYPE_CHECKING, Any from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.cfg import ArticulationCfg, LightCfg, MeshCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + ArticulationRootPropertiesCfg, + LightCfg, + MeshCfg, + MeshCollisionCfg, + RigidObjectCfg, +) from embodichain.lab.visualization import ( VisualizationCfg, add_viser_args_to_parser, @@ -87,8 +94,6 @@ def preview_scene_export( ) ) try: - if sim.is_use_gpu_physics: - sim.init_gpu_physics() _add_lights(sim) _add_objects( sim=sim, @@ -107,6 +112,7 @@ def preview_scene_export( entries=_config_entries(scene_config, "articulation"), config_dir=config_path.parent, ) + sim.prepare() is_viser = sim.sim_config.visualization.backend == "viser" joint_controller = _setup_viser_joint_control( @@ -207,19 +213,29 @@ def _add_objects( field_name=f"{uid}.body_scale", ) max_convex_hull_num = max(1, int(entry.get("max_convex_hull_num", 32))) + mesh_collision = MeshCollisionCfg(approximation="convex_hull") + if max_convex_hull_num > 1: + # The exported preview schema still carries the legacy hull budget; + # normalize it at this input boundary into the explicit Lab schema. + mesh_collision = MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=max_convex_hull_num, + acd_method="coacd", + ) sim.add_rigid_object( RigidObjectCfg( uid=uid, - shape=MeshCfg(fpath=str(mesh_path)), + shape=MeshCfg( + fpath=str(mesh_path), + collision=mesh_collision, + ), # Keep every preview body static: exported poses are already the # final gravity-settled poses and should not be simulated again. body_type="static", init_pos=tuple(init_pos), init_rot=tuple(init_rot), body_scale=tuple(body_scale), - max_convex_hull_num=max_convex_hull_num, - acd_method="visacd", # Use visacd by default. ) ) print(f"[{label}] {uid}: pos={init_pos} rot={init_rot} scale={body_scale}") @@ -273,7 +289,7 @@ def _add_articulations( init_pos=tuple(init_pos), init_rot=tuple(init_rot), body_scale=tuple(body_scale), - fix_base=True, + root_props=ArticulationRootPropertiesCfg(fixed_base=True), # Generated USDC is not URDF, so it cannot build a PK chain. build_pk_chain=False, ) diff --git a/embodichain/gen_sim/scene_engine/core/scene_object.py b/embodichain/gen_sim/scene_engine/core/scene_object.py index c8279118e..00fac54c8 100644 --- a/embodichain/gen_sim/scene_engine/core/scene_object.py +++ b/embodichain/gen_sim/scene_engine/core/scene_object.py @@ -26,7 +26,7 @@ class ObjectPhysics: """Physics and collision settings shared by settling and scene export.""" body_type: Literal["dynamic", "kinematic"] # Runtime behaviour in simulation. - attrs: dict[str, float | int] # Rigid-body material and contact attributes. + attrs: dict[str, object] # Grouped rigid-body physics configuration. max_convex_hull_num: int # Collision-decomposition hull budget. def __post_init__(self) -> None: @@ -37,11 +37,8 @@ def __post_init__(self) -> None: raise ValueError("max_convex_hull_num must be positive.") if not self.attrs: raise ValueError("attrs must contain at least one physics attribute.") - if not all( - isinstance(name, str) and isinstance(value, (float, int)) - for name, value in self.attrs.items() - ): - raise ValueError("attrs must map strings to numeric physics values.") + if not all(isinstance(name, str) for name in self.attrs): + raise ValueError("attrs must use string configuration keys.") def to_dict(self) -> dict[str, object]: """Serialize the physics settings for scene debugging artifacts.""" diff --git a/embodichain/gen_sim/scene_engine/pipeline/utils/gravity_settler.py b/embodichain/gen_sim/scene_engine/pipeline/utils/gravity_settler.py index 22c821e1e..2799cf170 100644 --- a/embodichain/gen_sim/scene_engine/pipeline/utils/gravity_settler.py +++ b/embodichain/gen_sim/scene_engine/pipeline/utils/gravity_settler.py @@ -32,8 +32,8 @@ transform_matrix_to_layout_object, ) from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RigidObjectCfg -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.cfg import RigidBodyPhysicsCfg, RigidObjectCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.utils.logger import log_info @@ -181,6 +181,7 @@ def settle(self) -> dict[str, dict[str, list[float]]]: "dynamic" if asset_id in self.dynamic_asset_ids else "kinematic" ), ) + sim.prepare() sim.update(step=self.config.settle_steps) settled_pose_by_id: dict[str, dict[str, list[float]]] = {} @@ -273,7 +274,10 @@ def _add_sim_body( return sim.add_rigid_object( RigidObjectCfg( uid=object_id, - shape=MeshCfg(fpath=str(body_info["mesh_path"])), + shape=MeshCfg( + fpath=str(body_info["mesh_path"]), + collision=self._mesh_collision_cfg(physics), + ), init_pos=tuple( self._three_floats(rigid_layout.get("pos"), field_name="pos") ), @@ -283,24 +287,28 @@ def _add_sim_body( ), attrs=self._rigid_body_attrs(physics), body_type=body_type, - max_convex_hull_num=self._max_convex_hull_num(physics), - acd_method="visacd", ) ) @staticmethod - def _rigid_body_attrs(physics: ObjectPhysics | None) -> RigidBodyAttributesCfg: + def _rigid_body_attrs(physics: ObjectPhysics | None) -> RigidBodyPhysicsCfg: """Convert persisted collision material data into one Lab config.""" if physics is None: raise ValueError("Gravity settling requires SimReady physics settings.") - return RigidBodyAttributesCfg(**physics.attrs) + return RigidBodyPhysicsCfg.from_dict(physics.attrs) @staticmethod - def _max_convex_hull_num(physics: ObjectPhysics | None) -> int: - """Read the persisted collision-hull budget after validating physics.""" + def _mesh_collision_cfg(physics: ObjectPhysics | None) -> MeshCollisionCfg: + """Normalize the persisted legacy hull budget into the Lab schema.""" if physics is None: raise ValueError("Gravity settling requires SimReady physics settings.") - return physics.max_convex_hull_num + if physics.max_convex_hull_num == 1: + return MeshCollisionCfg(approximation="convex_hull") + return MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=physics.max_convex_hull_num, + acd_method="coacd", + ) @staticmethod def _require_body_layout_id(body: GravitySettleBody, *, name: str) -> str: diff --git a/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py b/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py index 978143dda..636cf6519 100644 --- a/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py +++ b/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py @@ -327,7 +327,9 @@ def _scene_object_from_export_entry( support_optimization_rect_xy=support_optimization_rect_xy, physics=ObjectPhysics( body_type=str(entry.get("body_type", "dynamic")), # type: ignore[arg-type] - attrs=self._physics_attrs(entry.get("attrs", {"mass": 1.0})), + attrs=self._physics_attrs( + entry.get("attrs", {"mass_props": {"mass": 1.0}}) + ), max_convex_hull_num=max(1, int(entry.get("max_convex_hull_num", 32))), ), ) @@ -485,16 +487,14 @@ def _points2(cls, value: object, *, field_name: str) -> list[list[float]] | None ] @staticmethod - def _physics_attrs(value: object) -> dict[str, float | int]: + def _physics_attrs(value: object) -> dict[str, object]: """Validate exported physics attributes.""" if not isinstance(value, dict) or not value: raise ValueError("Scene object attrs must be a non-empty object.") - attrs: dict[str, float | int] = {} - for key, item in value.items(): - if not isinstance(key, str) or not isinstance(item, (float, int)): - raise ValueError("Scene object attrs must map strings to numbers.") - attrs[key] = item - return attrs + from embodichain.lab.sim.cfg.rigid import _rigid_body_physics_from_dict + + _rigid_body_physics_from_dict(value) + return dict(value) def import_scene_from_output_root(output_root: str | Path) -> Scene: diff --git a/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py b/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py index fdf59b711..da51c11a1 100644 --- a/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py +++ b/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py @@ -47,21 +47,34 @@ from embodichain.utils.logger import log_info _TABLE_PHYSICS_ATTRS = { - "mass": 10.0, # Keep the table heavy if a simulator treats it as movable. - "static_friction": 0.95, # Resist lateral sliding at table contacts. - "dynamic_friction": 0.9, # Maintain high friction during sliding contacts. - "restitution": 0.01, # Prevent a table contact from producing visible bounce. + "mass_props": { + "mass": 10.0, # Keep the table heavy if a simulator treats it as movable. + }, + "material_props": { + "static_friction": 0.95, # Resist lateral sliding at table contacts. + "dynamic_friction": 0.9, # Maintain high friction during sliding contacts. + "restitution": 0.01, # Prevent a table contact from producing visible bounce. + }, } _ASSET_PHYSICS_ATTRS = { - "mass": 0.01, # Use a lightweight default for unconstrained generated assets. - "contact_offset": 0.003, # Start contact detection slightly before mesh contact. - "rest_offset": 0.001, # Keep a small stable separation after contact resolution. - "restitution": 0.01, # Prevent generated assets from bouncing on the table. - "max_depenetration_velocity": 10.0, # Cap corrective separation speed. - "min_position_iters": 32, # Use extra position iterations for stable contacts. - "min_velocity_iters": 8, # Use extra velocity iterations for stable contacts. + "mass_props": { + "mass": 0.01, # Use a lightweight default for unconstrained generated assets. + }, + "collision_props": { + "contact_offset": 0.003, # Start contact detection slightly before mesh contact. + "rest_offset": 0.001, # Keep a small stable separation after contact resolution. + }, + "material_props": { + "restitution": 0.01, # Prevent generated assets from bouncing on the table. + }, + "rigid_props": { + "backend": "default", + "max_depenetration_velocity": 10.0, # Cap corrective separation speed. + "min_position_iters": 32, # Use extra position iterations for stable contacts. + "min_velocity_iters": 8, # Use extra velocity iterations for stable contacts. + }, } -_FIXED_MAX_CONVEX_HULL_NUM = 16 # Shared VisACD hull budget for settling and export. +_FIXED_MAX_CONVEX_HULL_NUM = 16 # Shared decomposition hull budget for settling/export. @dataclass(frozen=True) diff --git a/embodichain/lab/gym/envs/_startup_summary.py b/embodichain/lab/gym/envs/_startup_summary.py new file mode 100644 index 000000000..c3fae5261 --- /dev/null +++ b/embodichain/lab/gym/envs/_startup_summary.py @@ -0,0 +1,184 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Read-only functor details for the environment startup summary.""" + +from __future__ import annotations + +import inspect +import os +import shutil +import sys +from collections.abc import Mapping, Sequence +from dataclasses import MISSING +from functools import partial + +import numpy as np +import torch +from prettytable import PrettyTable, TableStyle + +from .managers.cfg import SceneEntityCfg + + +def _describe(value: object, depth: int = 0) -> str: + """Bound configuration output without reading tensor data or arbitrary repr.""" + if value is MISSING: + return "not set" + if value is None or isinstance(value, (bool, int, float)): + return repr(value) + if isinstance(value, str): + return repr(value[:97] + "..." if len(value) > 100 else value) + if isinstance(value, (torch.Tensor, np.ndarray)): + device = f", device={value.device}" if isinstance(value, torch.Tensor) else "" + return f"{type(value).__name__}(shape={tuple(value.shape)}, dtype={value.dtype}{device})" + if isinstance(value, SceneEntityCfg): + fields = [f"uid={_describe(value.uid)}"] + for prefix in ("joint", "body"): + selected = getattr(value, f"{prefix}_names") + if selected is not None: + fields.append(f"{prefix}_names={_describe(selected, depth + 1)}") + else: + indices = getattr(value, f"{prefix}_ids") + if not (isinstance(indices, slice) and indices == slice(None)): + fields.append(f"{prefix}_ids={_describe(indices, depth + 1)}") + return "SceneEntityCfg(" + ", ".join(fields) + ")" + if isinstance(value, (Mapping, list, tuple)): + if depth >= 2 or len(value) > 6: + return f"{type(value).__name__}(len={len(value)})" + if isinstance(value, Mapping): + return ( + "{" + + ", ".join( + f"{_describe(k, depth + 1)}: {_describe(v, depth + 1)}" + for k, v in value.items() + ) + + "}" + ) + contents = ", ".join(_describe(v, depth + 1) for v in value) + return f"[{contents}]" if isinstance(value, list) else f"({contents})" + if isinstance(value, slice): + return f"slice({value.start}, {value.stop}, {value.step})" + return type(value).__name__ + + +def _callable_name(func: object, full: bool) -> str: + if isinstance(func, partial): + return _callable_name(func.func, full) + if isinstance(func, str): + path = func.replace(":", ".") + return path if full else ".".join(path.split(".")[-2:]) + target = func if inspect.isroutine(func) or inspect.isclass(func) else type(func) + module = target.__module__ or type(func).__module__ + name = target.__qualname__ + if full: + return f"{module}.{name}" + if inspect.isroutine(target): + return f"{module.rsplit('.', 1)[-1]}.{target.__name__}" + return target.__name__ + + +def _functor_cells( + manager_name: str, manager: object, mode: str, name: str, full: bool +) -> list[str]: + details = [] + if manager_name == "ActionManager": + func = manager.get_term(name) + cfg = func.cfg + details.append(f"input={func.input_key} · dim={func.action_dim}") + else: + cfg = manager.get_functor_cfg(name) + func = cfg.func + if manager_name == "EventManager" and mode == "interval": + details.append(f"every {cfg.interval_step} control steps") + elif manager_name == "ObservationManager": + output = getattr(cfg, "name", MISSING) + if output is not MISSING: + details.append(f"output={output}") + elif manager_name == "RewardManager": + details.append(f"weight={cfg.weight:g}") + elif manager_name == "DatasetManager": + setting = "ON" if manager.save_failed_episodes else "OFF" + details.append(f"save failed episodes={setting}") + if full: + if isinstance(func, partial): + bound = [f"args={_describe(func.args)}"] if func.args else [] + bound.extend( + f"{k}={_describe(v)}" for k, v in (func.keywords or {}).items() + ) + if bound: + details.append("Bound: " + ", ".join(bound)) + params = getattr(cfg, "params", {}) + details.append( + "Params:\n" + + "\n".join(f"{key}={_describe(value)}" for key, value in params.items()) + if params + else "Params: {}" + ) + return [name, _callable_name(func, full), mode, "\n".join(details) or "—"] + + +def format_functor_summary( + managers: Sequence[tuple[str, object, list[tuple[str, list[str]]]]], + *, + full: bool, + color: bool | None = None, + width: int | None = None, +) -> str: + """Render initialized functors in execution groups without invoking them.""" + total = sum(len(names) for _, _, groups in managers for _, names in groups) + if not total: + return "" + if color is None: + color = "NO_COLOR" not in os.environ and sys.stderr.isatty() + width = max(64, min(width or shutil.get_terminal_size((100, 24)).columns, 120)) + name_width = min(24, 18 + (width - 64) // 8) + callable_width = 14 + (width - 64) // 3 + table = PrettyTable(["Name", "Callable", "Mode", "Details"]) + table.set_style(TableStyle.SINGLE_BORDER) + table.align = "l" + table.title = f"EmbodiChain · Functor Details · {total} active" + table.max_width = { + "Name": name_width, + "Callable": callable_width, + "Mode": 8, + "Details": width - 13 - name_width - callable_width - 8, + } + for manager_name, manager, groups in managers: + count = sum(len(names) for _, names in groups) + if not count: + continue + table.add_row([f"{manager_name} · {count}", "", "", ""], divider=True) + entries = [(mode, name) for mode, names in groups for name in names] + for index, (mode, name) in enumerate(entries): + table.add_row( + _functor_cells(manager_name, manager, mode, name, full), + divider=index == len(entries) - 1, + ) + lines = table.get_string().splitlines() + lines[0] = "╭" + lines[0][1:-1] + "╮" + lines[-1] = "╰" + lines[-1][1:-1] + "╯" + if color: + for index, line in enumerate(lines): + cells = line.split("│") + if len(cells) == 6: + group = not any(cell.strip() for cell in cells[2:5]) + for column, code in ((1, "1;36" if group else "1;32"), (3, "1;33")): + if cells[column].strip(): + cells[column] = f"\033[{code}m{cells[column]}\033[0m" + lines[index] = "│".join(cells) + elif "EmbodiChain ·" in line: + lines[index] = f"\033[1;36m{line}\033[0m" + return "\n".join(lines) diff --git a/embodichain/lab/gym/envs/base_env.py b/embodichain/lab/gym/envs/base_env.py index 536a50521..f0730e488 100644 --- a/embodichain/lab/gym/envs/base_env.py +++ b/embodichain/lab/gym/envs/base_env.py @@ -30,6 +30,11 @@ from embodichain.lab.sim.types import EnvObs, EnvAction from embodichain.lab.sim import SimulationManagerCfg, SimulationManager +from embodichain.lab.sim._startup_summary import ( + format_summary, + scene_rows, + simulation_rows, +) from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.sensors import BaseSensor, Camera from embodichain.lab.gym.utils import gym_utils @@ -128,7 +133,6 @@ class BaseEnv(gym.Env): # EmbodiedEnv defers the summary until all managers and recording buffers # have been initialized. _defer_initialization_summary: bool = False - _initialization_summary_label_width: int = 22 def __init__( self, @@ -136,6 +140,7 @@ def __init__( **kwargs, ): self.cfg = cfg + self._initialization_summary_logged = False # the number of envs to be simulated in parallel. self._num_envs = self.cfg.num_envs @@ -161,21 +166,48 @@ def __init__( self._configure_timing() + # Phase 1 only declares scene topology. Spawn-backed assets intentionally + # remain metadata-light until the single prepare boundary below. self._setup_scene(**kwargs) # Keep the established env._profiler API while sharing the single # profiler instance owned by SimulationManager. self._profiler = self.sim.profiler - # TODO: To be removed. - if self.device.type == "cuda": - self.sim.init_gpu_physics() + # Materialize every physical declaration in one transaction. DexSim's + # articulation adapter parses each source while finalizing, then the + # resulting handles bind the existing EmbodiChain facades in place. + self.sim.prepare() + + # Phase 2 may now consume link/joint metadata, construct action spaces, + # and create render-only resources such as CameraGroup instances. + configured_robot = self._setup_robot(**kwargs) + if configured_robot is not None: + self.robot = configured_robot + + if self.robot is None: + logger.log_error( + f"The robot instance must be initialized in :meth:`_setup_robot` function." + ) + if len(self.active_joint_ids) == 0: + self.active_joint_ids = self.robot.active_joint_ids + if self.single_action_space is None: + logger.log_error( + f":attr:`single_action_space` must be defined in the :meth:`_setup_robot` function." + ) + + self.sensors = self._setup_sensors(**kwargs) + self._camera_group_ids = [ + sensor.group_id + for sensor in self.sensors.values() + if isinstance(sensor, Camera) + ] if not self.sim_cfg.headless: self.sim.open_window() self._elapsed_steps = torch.zeros( - self._num_envs, dtype=torch.int32, device=self.sim_cfg.sim_device + self._num_envs, dtype=torch.int32, device=self.sim_cfg.device ) # -1 means no limit on episode length, and the episode will only end when the task is successfully completed or failed. @@ -199,11 +231,20 @@ def __init__( self._log_initialization_summary() def _log_initialization_summary(self) -> None: - """Log the environment initialization summary without log prefixes.""" + """Log the complete startup table once after the environment is ready.""" + if self.sim_cfg.startup_summary == "off" or getattr( + self, "_initialization_summary_logged", False + ): + return logger.log_info("\n".join(self._initialization_summary_lines()), prefix=False) + self._initialization_summary_logged = True + self.sim._startup_summary_logged = True + self.sim._scene_summary_logged = True def _initialization_summary_lines(self) -> list[str]: - """Build a compact, structured summary of the initialized environment.""" + """Combine the shared simulation snapshot with environment details.""" + if self.sim_cfg.startup_summary == "off": + return [] robot_description = type(self.robot).__name__ robot_uid = getattr(self.robot, "uid", None) if robot_uid: @@ -220,66 +261,48 @@ def _initialization_summary_lines(self) -> list[str]: if self.cfg.max_episode_steps > 0 else "unlimited" ) - - lines = [ - f"╭─ Environment initialized: {type(self).__name__}", - "├─ Runtime", - self._format_initialization_summary_row("Config", type(self.cfg).__name__), - self._format_initialization_summary_row("Device", self.device), - self._format_initialization_summary_row( - "Parallel environments", self.num_envs - ), - self._format_initialization_summary_row( - "Seed", self.cfg.seed if self.cfg.seed is not None else "not set" - ), - self._format_initialization_summary_row( - "Headless", str(bool(self.sim_cfg.headless)).lower() - ), - self._format_initialization_summary_row("Robot", robot_description), - self._format_initialization_summary_row("Sensors", sensor_description), - "├─ Timing", - self._format_initialization_summary_row( - "Physics", - f"{self.physics_dt:g} s ({self.physics_frequency:g} Hz)", - ), - self._format_initialization_summary_row( - "Control", - f"{self.step_dt:g} s ({self.control_frequency:g} Hz, " - f"{self.cfg.sim_steps_per_control} physics steps)", - ), - self._format_initialization_summary_row("Episode limit", episode_limit), - ] - + rows = simulation_rows(self.sim) + scene_rows(self.sim) + rows.extend( + [ + ("Environment", "Config", type(self.cfg).__name__), + ( + "Environment", + "Seed", + str(self.cfg.seed) if self.cfg.seed is not None else "not set", + ), + ("Environment", "Robot", robot_description), + ("Environment", "Sensors", sensor_description), + ( + "Environment", + "Control timestep", + f"{self.step_dt:g} s ({self.control_frequency:g} Hz, " + f"{self.cfg.sim_steps_per_control} physics steps)", + ), + ("Environment", "Episode limit", episode_limit), + ] + ) summary_metadata = [ (name, value) for name, value in self.metadata.items() if name != "render_fps" ] - if summary_metadata: - lines.append("├─ Metadata") - for name, value in sorted(summary_metadata, key=lambda item: str(item[0])): - lines.append( - self._format_initialization_summary_row( - str(name), self._format_initialization_metadata_value(value) - ) + for name, value in sorted(summary_metadata, key=lambda item: str(item[0])): + rows.append( + ( + "Metadata", + str(name), + self._format_initialization_metadata_value(value), ) + ) + rows.extend(self._extra_initialization_summary_rows()) + return format_summary( + f"Environment initialized: {type(self).__name__}", rows + ).splitlines() - lines.extend(self._extra_initialization_summary_lines()) - lines.append("╰─ Ready") - return lines - - def _extra_initialization_summary_lines(self) -> list[str]: - """Return subclass-specific initialization summary lines.""" + def _extra_initialization_summary_rows(self) -> list[tuple[str, str, str]]: + """Return subclass-specific startup table rows.""" return [] - @classmethod - def _format_initialization_summary_row( - cls, label: str, value: object, indent: int = 0 - ) -> str: - """Format an aligned key-value row inside the initialization tree.""" - label_width = max(1, cls._initialization_summary_label_width - 2 * indent) - return f"│ {' ' * indent}{label:<{label_width}} {value}" - @staticmethod def _format_initialization_metadata_value(value: object) -> str: """Format metadata without expanding large nested structures.""" @@ -483,46 +506,47 @@ def add_camera_group_id(self, group_id: int) -> None: self._camera_group_ids.append(group_id) def _setup_scene(self, **kwargs): - # Init sim manager. - # we want to open gui window when the scene is setup, so init sim manager in headless mode first. + """Declare physical scene topology without consuming runtime metadata.""" + # Init sim manager. We want to open the GUI window after the scene is + # materialized, so construct the manager in headless mode first. headless = self.sim_cfg.headless self.sim_cfg.headless = True - self.sim = SimulationManager(self.sim_cfg) + self.sim = SimulationManager(self.sim_cfg, defer_startup_summary=True) self.sim_cfg.headless = headless logger.log_info( - f"Initializing {self.num_envs} environments on {self.sim_cfg.sim_device}." + f"Initializing {self.num_envs} environments on {self.sim_cfg.device}." ) - self.robot = self._setup_robot(**kwargs) - if len(self.active_joint_ids) == 0: - self.active_joint_ids = self.robot.active_joint_ids - - if self.robot is None: - logger.log_error( - f"The robot instance must be initialized in :meth:`_setup_robot` function." - ) - if self.single_action_space is None: - logger.log_error( - f":attr:`single_action_space` must be defined in the :meth:`_setup_robot` function." - ) + # Config-driven environments can declare their robot here while + # deferring all link/joint queries until the post-prepare phase. Generic + # BaseEnv subclasses may keep returning None and add a runtime robot in + # _setup_robot() for backwards compatibility. + self.robot = self._declare_robot(**kwargs) self._prepare_scene(**kwargs) - self.sensors = self._setup_sensors(**kwargs) + def _declare_robot(self, **kwargs) -> Robot | None: + """Optionally declare a robot before the scene prepare boundary. - # Setup camera groups for rendering. - self._camera_group_ids: List[int] = [] - for sensor in self.sensors.values(): - if isinstance(sensor, Camera): - self._camera_group_ids.append(sensor.group_id) + Config-driven environments should override this hook and call + :meth:`SimulationManager.add_robot` without querying link/joint data. + The returned facade is bound in place by :meth:`SimulationManager.prepare`. + + Generic subclasses that only implement the historical + :meth:`_setup_robot` hook remain supported: their robot is added after + the initial prepare boundary and is prepared immediately by the manager. + """ + del kwargs + return None def _setup_robot(self, **kwargs) -> Robot: - """Load the robot agent, setup the controller and action space. + """Configure the bound robot, controller, and action space. Note: - 1. The fuction must return the robot instance. - 2. The self.single_action_space should be defined. + This hook runs after :meth:`SimulationManager.prepare`, so link, + joint, and limit metadata are available. It must return the robot + instance and define ``self.single_action_space``. """ # TODO: single_action_space may be configured in config? diff --git a/embodichain/lab/gym/envs/differentiable_env.py b/embodichain/lab/gym/envs/differentiable_env.py new file mode 100644 index 000000000..13103e140 --- /dev/null +++ b/embodichain/lab/gym/envs/differentiable_env.py @@ -0,0 +1,169 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Newton-backed kinematic environment for analytic policy gradient. + +Wraps a task-defined Newton kinematics callback in a Warp tape and bridges +autograd into PyTorch via :class:`embodichain.lab.sim.diff.NewtonStepFunc`. +The environment deliberately does not advance the configured Newton solver; +differentiable dynamics are outside the current public contract. + +Usage: + + class MyTask(DifferentiableEnv): + def _apply_action_kernel(self, action_wp, tape): ... + def _make_kinematic_step_fn(self): ... + def _read_outputs(self, final_state) -> dict: ... +""" + +from __future__ import annotations + +from typing import Any, Callable + +import torch + +from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.sim.cfg import NewtonPhysicsCfg +from embodichain.lab.sim.diff import NewtonStepFunc +from embodichain.utils import logger + +__all__ = ["DifferentiableEnv"] + + +class DifferentiableEnv(EmbodiedEnv): + """Newton-only environment with an APG-ready kinematic :meth:`step`. + + Subclasses implement :meth:`_apply_action_kernel`, + :meth:`_make_kinematic_step_fn`, and :meth:`_read_outputs`. The action, + kinematics, observation, and reward kernels execute inside one Warp tape. + No Newton solver or collision step is invoked by this environment. + """ + + def __init__( + self, + cfg: EmbodiedEnvCfg, + *args: Any, + **kwargs: Any, + ) -> None: + self._validate_diff_cfg(cfg) + super().__init__(cfg, *args, **kwargs) + self._truncate_backward_at: int | None = getattr( + cfg, "truncate_backward_at", None + ) + + @staticmethod + def _validate_diff_cfg(cfg: EmbodiedEnvCfg) -> None: + physics_cfg = cfg.sim_cfg.physics_cfg + if not isinstance(physics_cfg, NewtonPhysicsCfg): + logger.log_error( + "DifferentiableEnv requires NewtonPhysicsCfg, " + f"got {type(physics_cfg).__name__}." + ) + if not physics_cfg.requires_grad: + logger.log_error( + "DifferentiableEnv requires requires_grad=True on " + "the NewtonPhysicsCfg." + ) + + # -- subclass contract ------------------------------------------------ # + + def _apply_action_kernel(self, action_wp: Any, tape: Any) -> None: + """Write an action for the task-defined kinematics callback. + + The hook receives no solver control because :class:`DifferentiableEnv` + never advances Newton dynamics. + """ + raise NotImplementedError( + "DifferentiableEnv subclasses must implement " + "_apply_action_kernel(action_wp, tape)." + ) + + def _read_outputs(self, final_state: Any) -> dict: + """Read the post-step observation and reward as torch tensors. + + Must return a dict with keys ``"obs"``, ``"reward"``, + ``"terminated"``, ``"truncated"``, plus the ``_order`` and + ``_grad_track`` metadata expected by + :class:`NewtonStepFunc`. ``obs`` and ``reward`` should be torch + tensors backed by ``wp.to_torch`` of grad-tracked Warp arrays. + """ + raise NotImplementedError( + "DifferentiableEnv subclasses must implement _read_outputs(final_state)." + ) + + def _make_kinematic_step_fn(self) -> Callable[[], Any]: + """Return the task-defined kinematics callback. + + Raises: + NotImplementedError: If the subclass has no kinematics hook. + """ + raise NotImplementedError( + "DifferentiableEnv requires _make_kinematic_step_fn()." + ) + + # -- gym surface ------------------------------------------------------ # + + def step(self, action: torch.Tensor): + """Advance one differentiable control step. + + Terminal environments are auto-reset only when the call cannot retain + a Warp tape for backward. A grad-tracked step returns terminal + observations unchanged and records ``deferred_reset_ids`` in ``info``; + callers must run backward before resetting those environments. + """ + if not isinstance(action, torch.Tensor): + action = torch.as_tensor(action, dtype=torch.float32) + retains_tape_for_backward = bool( + torch.is_grad_enabled() and action.requires_grad + ) + sim_state = self._build_sim_state_dict(action) + outputs = NewtonStepFunc.apply(action, sim_state) + obs, reward, terminated, truncated = outputs[:4] + info = sim_state["last_info"] + + done_mask = terminated | truncated + if done_mask.any(): + reset_ids = done_mask.nonzero(as_tuple=False).squeeze(-1) + if retains_tape_for_backward: + info["requires_reset_after_backward"] = True + info["deferred_reset_ids"] = reset_ids.detach().clone() + else: + fresh_obs, _ = self.reset(options={"reset_ids": reset_ids}) + obs = torch.where( + done_mask.unsqueeze(-1).expand_as(obs), + fresh_obs.detach(), + obs, + ) + return obs, reward, terminated, truncated, info + + def _build_sim_state_dict(self, action: torch.Tensor) -> dict: + del action + return { + "manager": self.sim, + "action_kernel": self._wrap_action_kernel(), + "kernel_args": (), + "obs_reward_fn": self._read_outputs, + "last_info": {}, + "step_fn": self._make_kinematic_step_fn(), + } + + def _wrap_action_kernel(self) -> Callable[..., None]: + """Adapt the task action hook to :class:`NewtonStepFunc`.""" + env = self + + def _inner(action_wp: Any, tape: Any, *_: Any) -> None: + env._apply_action_kernel(action_wp, tape=tape) + + return _inner diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index b8de4176e..77a9f6cd9 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -42,6 +42,7 @@ from embodichain.lab.sim.cfg import ( RobotCfg, + RobotPresetCfg, RigidObjectCfg, RigidObjectGroupCfg, ArticulationCfg, @@ -57,6 +58,7 @@ from embodichain.lab.sim.sensors import BaseSensor, SensorCfg from embodichain.lab.sim.types import EnvObs, EnvAction from embodichain.lab.gym.envs import BaseEnv, EnvCfg +from embodichain.lab.gym.envs._startup_summary import format_functor_summary from embodichain.lab.gym.envs.demo import ( DEMO_SCHEMA_VERSION, DemoExecutionCfg, @@ -109,8 +111,9 @@ class EmbodiedEnvCfg(EnvCfg): instance as attributes during initialization. Key fields - - **robot**: `RobotCfg` (required) — the agent definition (URDF/MJCF, initial - state, control mode, etc.). + - **robot**: `RobotCfg | RobotPresetCfg` (required) — one portable robot + definition or replace-only complete alternatives selected by the active + physics backend. - **control_parts**: Optional[List[str]] — named robot parts to control. If `None`, all controllable joints are used. - **active_joint_ids**: List[int] — explicit joint indices to use for @@ -148,7 +151,7 @@ class EnvLightCfg: # TODO: support more types of indirect light in the future. indirect: dict[str, Any] | None = None - robot: RobotCfg = MISSING + robot: RobotCfg | RobotPresetCfg = MISSING control_parts: list[str] | None = None """List of robot parts to control. If None, all controllable joints will be used. @@ -298,8 +301,8 @@ class EmbodiedEnv(BaseEnv): _manager_summary_fields: tuple[tuple[str, str], ...] = ( ("EventManager", "event_manager"), ("ObservationManager", "observation_manager"), - ("RewardManager", "reward_manager"), ("ActionManager", "action_manager"), + ("RewardManager", "reward_manager"), ("DatasetManager", "dataset_manager"), ) @@ -466,8 +469,27 @@ def __init__( self._log_initialization_summary() - def _extra_initialization_summary_lines(self) -> list[str]: - """Build manager and functor details for the initialization summary.""" + def _initialization_summary_lines(self) -> list[str]: + """Append a separate functor table after the environment summary.""" + lines = super()._initialization_summary_lines() + if not lines: + return lines + managers = [] + for name, attribute in self._manager_summary_fields: + manager = getattr(self, attribute, None) + if manager is not None: + managers.append( + (name, manager, self._manager_functor_groups(name, manager)) + ) + details = format_functor_summary( + managers, full=self.sim_cfg.startup_summary == "full" + ) + if details: + lines.extend(["", *details.splitlines()]) + return lines + + def _extra_initialization_summary_rows(self) -> list[tuple[str, str, str]]: + """Keep manager status and counts in the main initialization table.""" manager_summaries: list[tuple[str, list[tuple[str, list[str]]] | None, int]] = ( [] ) @@ -487,30 +509,24 @@ def _extra_initialization_summary_lines(self) -> list[str]: total_functor_count += functor_count functor_noun = "functor" if total_functor_count == 1 else "functors" - lines = [ - f"├─ Managers ({active_manager_count}/{len(manager_summaries)} active, " - f"{total_functor_count} {functor_noun})" + rows = [ + ( + "Managers", + "Total", + f"{active_manager_count}/{len(manager_summaries)} active, " + f"{total_functor_count} {functor_noun}", + ) ] for manager_name, groups, functor_count in manager_summaries: if groups is None: - lines.append( - self._format_initialization_summary_row(manager_name, "disabled") - ) + rows.append(("Managers", manager_name, "disabled")) continue manager_functor_noun = "functor" if functor_count == 1 else "functors" - lines.append( - self._format_initialization_summary_row( - manager_name, f"{functor_count} {manager_functor_noun}" - ) + rows.append( + ("Managers", manager_name, f"{functor_count} {manager_functor_noun}") ) - for mode, names in groups: - lines.append( - self._format_initialization_summary_row( - mode, ", ".join(names), indent=1 - ) - ) - return lines + return rows @staticmethod def _manager_functor_groups( @@ -857,9 +873,9 @@ def _extend_reward( return rewards def _prepare_scene(self, **kwargs) -> None: - self._setup_lights() self._setup_background() self._setup_interactive_objects() + self._setup_lights() def _update_sim_state(self, **kwargs) -> None: """Perform the simulation step and apply events if configured. @@ -1915,8 +1931,15 @@ def _postprocess_action(self, action): return self.action_manager.process_action(action, mode="post") return super()._postprocess_action(action) + def _declare_robot(self, **kwargs) -> Robot: + """Declare the configured robot without reading articulation metadata.""" + del kwargs + if self.cfg.robot is None: + logger.log_error("Robot configuration is not provided.") + return self.sim.add_robot(self.cfg.robot) + def _setup_robot(self, **kwargs) -> Robot: - """Setup the robot in the environment. + """Configure the finalized robot interface for the environment. Currently, only joint position control is supported. Would be extended to support joint velocity and torque control in the future. @@ -1924,11 +1947,10 @@ def _setup_robot(self, **kwargs) -> Robot: Returns: Robot: The robot instance added to the scene. """ - if self.cfg.robot is None: - logger.log_error("Robot configuration is not provided.") - - # Initialize the robot based on the configuration. - robot: Robot = self.sim.add_robot(self.cfg.robot) + del kwargs + robot = self.robot + if robot is None: + logger.log_error("Robot was not declared before simulation prepare.") # Setup active joints for robot to control. if self.cfg.control_parts: diff --git a/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py b/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py index 458401b6c..48ff76c4b 100644 --- a/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py +++ b/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py @@ -124,8 +124,9 @@ def _get_dynamic_entity_catalog( def _is_dynamic_entity(kind: str, entity: _DynamicEntity) -> bool: """Return whether an entity participates in dynamic physics. - Articulation links are physics-backed even when ``fix_base`` constrains the - root link, so every non-robot articulation is a valid settle target. + Articulation links are physics-backed even when + ``root_props.fixed_base`` constrains the root link, so every + non-robot articulation is a valid settle target. """ if kind == "articulation": return True diff --git a/embodichain/lab/gym/envs/managers/actions.py b/embodichain/lab/gym/envs/managers/actions.py index 4f9430f9b..6882f998d 100644 --- a/embodichain/lab/gym/envs/managers/actions.py +++ b/embodichain/lab/gym/envs/managers/actions.py @@ -232,7 +232,7 @@ class EefPoseTerm(ActionTerm): Supports two pose representations: - 6D: position (3) + Euler angles (3) - - 7D: position (3) + quaternion (4) + - 7D: position (3) + quaternion in ``xyzw`` order (4) On IK failure, falls back to current_qpos for that env. Returns ``ik_success`` in the TensorDict so reward/observation @@ -248,7 +248,7 @@ class EefPoseTerm(ActionTerm): >>> # 7D: position (3) + quaternion (4) >>> action = torch.zeros(num_envs, 7) >>> action[:, :3] = 0.1 # target position - >>> action[:, 3] = 1.0 # quaternion w + >>> action[:, 6] = 1.0 # quaternion w (xyzw identity) >>> result = term.process_action(action) >>> # result["qpos"] = IK solution >>> # result["ik_success"] = bool tensor indicating IK success diff --git a/embodichain/lab/gym/envs/managers/events.py b/embodichain/lab/gym/envs/managers/events.py index 907effd70..3407c3e8f 100644 --- a/embodichain/lab/gym/envs/managers/events.py +++ b/embodichain/lab/gym/envs/managers/events.py @@ -609,7 +609,7 @@ def drop_rigid_object_group_sequentially( .repeat(num_instance, 1) ) drop_pose = torch.zeros((num_instance, 7), device=env.device) - drop_pose[:, 3] = 1.0 # w component of quaternion + drop_pose[:, 6] = 1.0 # w component of xyzw quaternion drop_pose[:, :3] = drop_pos for i in range(num_objects): random_offset = sample_uniform( diff --git a/embodichain/lab/gym/envs/managers/observations.py b/embodichain/lab/gym/envs/managers/observations.py index 3729ec8d0..45374c9ef 100644 --- a/embodichain/lab/gym/envs/managers/observations.py +++ b/embodichain/lab/gym/envs/managers/observations.py @@ -50,7 +50,8 @@ def get_object_pose( env: The environment instance. obs: The observation dictionary. entity_cfg: The configuration of the scene entity. - to_matrix: Whether to return the pose as a 4x4 transformation matrix. If False, returns as (position, quaternion). + to_matrix: Whether to return the pose as a 4x4 transformation matrix. If + False, returns ``(x, y, z, qx, qy, qz, qw)``. Returns: A tensor of shape (num_envs, 7) or (num_envs, 4, 4) representing the world poses of the objects. @@ -90,7 +91,8 @@ def get_rigid_object_pose( env: The environment instance. obs: The observation dictionary. entity_cfg: The configuration of the scene entity. - to_matrix: Whether to return the pose as a 4x4 transformation matrix. If False, returns as (position, quaternion). + to_matrix: Whether to return the pose as a 4x4 transformation matrix. If + False, returns ``(x, y, z, qx, qy, qz, qw)``. Returns: A tensor of shape (num_envs, 7) or (num_envs, 4, 4) representing the world poses of the rigid objects. @@ -1157,14 +1159,9 @@ def __call__( device=env.device, ) else: - ( - stiffness, - damping, - max_effort, - max_velocity, - friction, - armature, - ) = art.get_joint_drive() + stiffness, damping, max_effort, max_velocity, friction, armature = ( + art.get_joint_drive() + ) result = TensorDict( { "stiffness": stiffness, diff --git a/embodichain/lab/gym/envs/managers/randomization/physics.py b/embodichain/lab/gym/envs/managers/randomization/physics.py index 1eea74e04..a0a7da9a7 100644 --- a/embodichain/lab/gym/envs/managers/randomization/physics.py +++ b/embodichain/lab/gym/envs/managers/randomization/physics.py @@ -35,6 +35,8 @@ def randomize_rigid_object_mass( entity_cfg: SceneEntityCfg, mass_range: tuple[float, float], relative: bool = False, + recompute_inertia: bool = True, + min_mass: float = 1e-6, ) -> None: """Randomize the mass of rigid objects in the environment. @@ -44,25 +46,54 @@ def randomize_rigid_object_mass( entity_cfg (SceneEntityCfg): The configuration for the scene entity. mass_range (tuple[float, float]): The range (min, max) to sample the mass from. relative (bool): Whether to apply the mass change relative to the initial mass. Defaults to False. + recompute_inertia (bool): Whether to scale the initial inertia by the sampled + mass ratio. Defaults to True. + min_mass (float): Minimum allowed sampled mass. Defaults to 1e-6. + + Raises: + ValueError: If ``min_mass`` is not positive or an initial mass is not positive. """ if entity_cfg.uid not in env.sim.get_rigid_object_uid_list(): return rigid_object: RigidObject = env.sim.get_rigid_object(entity_cfg.uid) + if rigid_object.is_non_dynamic: + logger.log_warning( + f"Cannot randomize mass for non-dynamic rigid object '{entity_cfg.uid}'." + ) + return + if min_mass <= 0.0: + raise ValueError(f"min_mass must be positive, got {min_mass}.") + num_instance = len(env_ids) + index = torch.as_tensor(env_ids, dtype=torch.long, device=rigid_object.device) + body_data = rigid_object.body_data + if body_data is None: + return + default_masses = body_data.default_mass[index] + if torch.any(default_masses <= 0.0): + raise ValueError("Initial rigid-body masses must be positive.") sampled_masses = sample_uniform( - lower=mass_range[0], upper=mass_range[1], size=(num_instance,) + lower=mass_range[0], + upper=mass_range[1], + size=(num_instance,), + device=rigid_object.device, ) if relative: - init_mass = rigid_object.cfg.attrs.mass - init_mass = torch.full((sampled_masses.shape), init_mass, device=env.device) - sampled_masses = init_mass + sampled_masses + sampled_masses = default_masses + sampled_masses + + sampled_masses = sampled_masses.clamp_min(min_mass) rigid_object.set_mass(sampled_masses, env_ids=env_ids) + if recompute_inertia: + mass_ratios = sampled_masses / default_masses + sampled_inertia = body_data.default_inertia[index] * mass_ratios.unsqueeze(-1) + rigid_object.set_inertia(sampled_inertia, env_ids=env_ids) + def randomize_rigid_object_center_of_mass( env: EmbodiedEnv, @@ -111,6 +142,8 @@ def randomize_articulation_mass( mass_range: tuple[float, float] | dict[str, tuple[float, float]], link_names: str | list[str] | None = None, relative: bool = False, + recompute_inertia: bool = True, + min_mass: float = 1e-6, ) -> None: """Randomize the mass of articulation links in the environment. @@ -127,14 +160,23 @@ def randomize_articulation_mass( link_names (str | list[str] | None): A regex pattern or list of regex patterns to match link names. If None, all links are randomized. Ignored when ``mass_range`` is a dict. Defaults to None. - relative (bool): Whether to apply the mass change relative to the current mass. + relative (bool): Whether to apply the mass change relative to the initial mass. Defaults to False. + recompute_inertia (bool): Whether to scale initialization-time inertia by + the sampled mass ratio. Defaults to True. + min_mass (float): Minimum allowed sampled mass. Defaults to 1e-6. + + Raises: + ValueError: If ``min_mass`` or an initialization-time link mass is not + positive. """ if entity_cfg.uid not in env.sim.get_articulation_uid_list(): return articulation: Articulation = env.sim.get_articulation(entity_cfg.uid) + if min_mass <= 0.0: + raise ValueError(f"min_mass must be positive, got {min_mass}.") num_instance = len(env_ids) if isinstance(mass_range, dict): @@ -149,18 +191,18 @@ def randomize_articulation_mass( matched_link_names = list(mass_range.keys()) link_lower = torch.tensor( [mass_range[name][0] for name in matched_link_names], - device=env.device, + device=articulation.device, dtype=torch.float32, ) link_upper = torch.tensor( [mass_range[name][1] for name in matched_link_names], - device=env.device, + device=articulation.device, dtype=torch.float32, ) # Broadcast: (num_instance, num_links) sampled_masses = torch.rand( (num_instance, len(matched_link_names)), - device=env.device, + device=articulation.device, dtype=torch.float32, ) sampled_masses = link_lower + sampled_masses * (link_upper - link_lower) @@ -179,17 +221,39 @@ def randomize_articulation_mass( lower=mass_range[0], upper=mass_range[1], size=(num_instance, len(matched_link_names)), + device=articulation.device, + ) + + env_index = torch.as_tensor(env_ids, dtype=torch.long, device=articulation.device) + link_indices = torch.as_tensor( + [articulation.link_names.index(name) for name in matched_link_names], + dtype=torch.long, + device=articulation.device, + ) + default_masses = articulation.body_data.default_mass[ + env_index[:, None], link_indices[None, :] + ] + if torch.any(default_masses <= 0.0): + raise ValueError( + "Initialization-time articulation link masses must be positive." ) if relative: - link_indices = [ - articulation.link_names.index(name) for name in matched_link_names - ] - current_masses = articulation.default_link_masses.clone()[env_ids][ - :, link_indices - ] - sampled_masses = current_masses + sampled_masses + sampled_masses = default_masses + sampled_masses + + sampled_masses = sampled_masses.clamp_min(min_mass) articulation.set_mass( sampled_masses, link_names=matched_link_names, env_ids=env_ids ) + + if recompute_inertia: + default_inertia = articulation.body_data.default_inertia[ + env_index[:, None], link_indices[None, :] + ] + mass_ratios = sampled_masses / default_masses + articulation.set_inertia( + default_inertia * mass_ratios.unsqueeze(-1), + link_names=matched_link_names, + env_ids=env_ids, + ) diff --git a/embodichain/lab/gym/envs/managers/randomization/spatial.py b/embodichain/lab/gym/envs/managers/randomization/spatial.py index 384490d51..65cd83ea0 100644 --- a/embodichain/lab/gym/envs/managers/randomization/spatial.py +++ b/embodichain/lab/gym/envs/managers/randomization/spatial.py @@ -860,7 +860,7 @@ def _move_object_z( return # Both RigidObject and Articulation return (N, 7) by default: - # (x, y, z, qw, qx, qy, qz) + # (x, y, z, qx, qy, qz, qw) pose = obj.get_local_pose() # (N, 7) current_z = pose[env_ids, 2] if absolute: diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index 390955c2d..8a551c13e 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -35,6 +35,7 @@ "max_episode_steps", "num_envs", "arena_space", + "physics", "physics_config", "render_cfg", "visualization", @@ -145,7 +146,7 @@ def _resolve_environment_component( *, base_dir: Path, ) -> dict[str, object]: - """Expand one reusable physical environment component.""" + """Expand one backend-specific reusable physical environment component.""" resolved = _owned_mapping(config, path="Gym deployment") declaration = _mapping( resolved.pop("environment"), @@ -160,8 +161,8 @@ def _resolve_environment_component( component = _mapping( _load_yaml_component(component_path, field_name="environment component"), path="environment component", - required=frozenset({"environment_id", "simulation", "env"}), - optional=_ENVIRONMENT_COMPONENT_FIELDS - {"simulation", "env"}, + required=frozenset({"environment_id", "physics", "simulation", "env"}), + optional=_ENVIRONMENT_COMPONENT_FIELDS - {"physics", "simulation", "env"}, ) _identifier( component["environment_id"], @@ -180,6 +181,15 @@ def _resolve_environment_component( } environment_values.update(deepcopy(dict(simulation))) + deployment_physics_fields = sorted( + {"physics", "physics_config"}.intersection(resolved) + ) + if deployment_physics_fields: + raise ValueError( + "environment.component owns physics and physics_config; remove " + f"{deployment_physics_fields} from the Gym deployment." + ) + duplicate_fields = sorted( set(environment_values).intersection(resolved) - _ENVIRONMENT_DEPLOYMENT_OVERRIDE_FIELDS diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index f6365201d..b75766e18 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -25,6 +25,7 @@ import gymnasium import gymnasium as gym +from collections.abc import Mapping from typing import Callable, Dict, Any, List, Tuple, Union, Sequence from gymnasium import spaces from copy import deepcopy @@ -50,6 +51,22 @@ # Extra manager modules registered by third-party packages via init hooks _EXTRA_MANAGER_MODULES: list[str] = [] +_PHYSICS_BACKENDS = frozenset({"default", "newton"}) + + +def _declared_physics_backend(config: Mapping[str, object]) -> str: + """Return the single physics backend explicitly owned by a Gym config.""" + if "physics" not in config: + raise ValueError( + "Gym config must explicitly declare physics as 'default' or 'newton'." + ) + backend = config["physics"] + if type(backend) is not str or backend not in _PHYSICS_BACKENDS: + raise ValueError( + "Gym config physics must be exactly 'default' or 'newton', " + f"got {backend!r}." + ) + return backend def register_manager_modules(modules: list[str]) -> None: @@ -411,6 +428,8 @@ def config_to_cfg( policy. Inline ``robot``, ``sensor``, and scene fields remain valid when their corresponding component selector is absent. A resolved config containing ``task_program`` composes its semantic and policy components. + Every resolved environment explicitly owns one ``physics`` backend and an + optional ``physics_config`` mapping whose fields must belong to that backend. The existing :class:`~embodichain.lab.gym.envs.EmbodiedEnv` class is registered under the config's ``id`` with the decoded integration factory. @@ -439,9 +458,9 @@ def config_to_cfg( RigidObjectGroupCfg, ArticulationCfg, LightCfg, - PhysicsCfg, DLSSCfg, RenderCfg, + physics_cfg_for_backend, ) from embodichain.lab.sim import SimulationManagerCfg from embodichain.lab.visualization import VisualizationCfg, ViserServerCfg @@ -495,6 +514,24 @@ class ComponentCfg: component_resolution = _resolve_gym_components(config, base_dir=base_dir) config = component_resolution.config + physics_backend = _declared_physics_backend(config) + physics_config_value = config.get("physics_config", {}) + if not isinstance(physics_config_value, Mapping): + raise TypeError("Gym config physics_config must be a mapping.") + if not all(type(key) is str for key in physics_config_value): + raise TypeError("Gym config physics_config keys must be exact strings.") + physics_config = deepcopy(dict(physics_config_value)) + if "gravity" in physics_config: + physics_config["gravity"] = np.asarray(physics_config["gravity"]) + physics_cfg = physics_cfg_for_backend(physics_backend) + try: + physics_cfg = type(physics_cfg)(**physics_config) + except TypeError as exc: + raise ValueError( + f"physics_config does not match declared physics backend " + f"{physics_backend!r}: {exc}" + ) from exc + # Check required fields after reusable task expansion. required_keys = ["id", "env"] for key in required_keys: @@ -599,10 +636,6 @@ class ComponentCfg: env_cfg.num_envs = config.get("num_envs", 1) env_cfg.seed = config.get("seed", None) - physics_config = deepcopy(config.get("physics_config", {})) - if "gravity" in physics_config: - physics_config["gravity"] = np.asarray(physics_config["gravity"]) - render_config = deepcopy(config.get("render_cfg", {})) if isinstance(render_config.get("dlss"), dict): render_config["dlss"] = DLSSCfg(**render_config["dlss"]) @@ -622,14 +655,16 @@ class ComponentCfg: viser_server = ViserServerCfg(**viser_server_config) env_cfg.sim_cfg = SimulationManagerCfg( + startup_summary=config.get("startup_summary", "compact"), + dexsim_startup_info=config.get("dexsim_startup_info", False), headless=config.get("headless", False), enable_entity_gizmo=config.get("enable_entity_gizmo", True), robot_ik_gizmo=config.get("robot_ik_gizmo", {}), - sim_device=config.get("device", "cpu"), + device=config.get("device"), render_cfg=RenderCfg(**render_config), gpu_id=config.get("gpu_id", 0), arena_space=config.get("arena_space", 5.0), - physics_config=PhysicsCfg(**physics_config), + physics_cfg=physics_cfg, visualization=VisualizationCfg( **visualization_config, viser_server=viser_server, @@ -988,9 +1023,11 @@ def add_env_launcher_args_to_parser( This function adds the following arguments to the provided parser: --num_envs: Number of environments to run in parallel (default: 1) --seed: Task-environment seed. The task config is used when omitted. - --device: Device to run the environment on (default: 'cpu') + --device: Runtime device override. When omitted, the selected backend + supplies its own default (CPU for Default, CUDA for Newton). --headless: Whether to perform the simulation in headless mode (default: False) --renderer: Renderer backend to use for the simulation. Options are 'hybrid', 'fast-rt', and 'rt'. (default: 'hybrid') + --physics: Physics backend configuration to use. Options are 'default' and 'newton'. (default: 'default') --gpu_id: The GPU ID to use for the simulation (default: 0) --gym_config: Path to gym config file (default: '') --action_config: Path to action config file (default: None) @@ -1025,8 +1062,10 @@ def add_env_launcher_args_to_parser( parser.add_argument( "--device", type=str, - default="cpu", - help="Device to run the environment on, e.g., 'cpu' or 'cuda'.", + default=None, + help="Device used by environment tensors and the selected physics " + "backend, e.g. 'cpu' or 'cuda:0'. When omitted, the selected backend " + "default is preserved unless this option is set.", ) parser.add_argument( "--headless", @@ -1043,6 +1082,15 @@ def add_env_launcher_args_to_parser( "config, the configured render_cfg.renderer is used unless this option " "is provided.", ) + parser.add_argument( + "--physics", + type=str, + choices=["default", "newton"], + default=None if require_gym_config else "default", + help="Physics backend to use for standalone simulation. Gym configs " + "declare their backend in the file; this option may only confirm the " + "same value.", + ) parser.add_argument( "--arena_space", help="The size of the arena space.", @@ -1130,7 +1178,8 @@ def add_env_launcher_args_to_parser( def merge_args_with_gym_config(args: argparse.Namespace, gym_config: dict) -> dict: """Merge command-line arguments with gym configuration. - Command-line arguments will override the corresponding values in the gym configuration. + Command-line arguments override runtime values in the Gym configuration. + The physics backend is file-owned and cannot be changed by the launcher. Args: args (argparse.Namespace): The parsed command-line arguments. @@ -1140,15 +1189,24 @@ def merge_args_with_gym_config(args: argparse.Namespace, gym_config: dict) -> di dict: The merged gym configuration dictionary. """ merged_config = deepcopy(gym_config) + configured_physics = _declared_physics_backend(merged_config) if args.num_envs is not None: merged_config["num_envs"] = args.num_envs if getattr(args, "seed", None) is not None: merged_config["seed"] = args.seed - merged_config["device"] = args.device + if args.device is not None: + merged_config["device"] = args.device viser_enabled = bool(getattr(args, "viser", False)) merged_config["headless"] = args.headless or viser_enabled if args.renderer is not None: merged_config["renderer"] = args.renderer + requested_physics = getattr(args, "physics", None) + if requested_physics is not None and requested_physics != configured_physics: + raise ValueError( + f"Gym config declares physics={configured_physics!r}; " + f"--physics={requested_physics!r} cannot override a file-owned " + "backend. Select a Gym config for the requested backend." + ) merged_config["gpu_id"] = args.gpu_id merged_config["arena_space"] = args.arena_space if args.max_episodes is not None: diff --git a/embodichain/lab/gym/utils/trajectory_state.py b/embodichain/lab/gym/utils/trajectory_state.py index e165646d9..6e531a85e 100644 --- a/embodichain/lab/gym/utils/trajectory_state.py +++ b/embodichain/lab/gym/utils/trajectory_state.py @@ -14,7 +14,11 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Shared simulation-state capture and restore helpers for trajectories.""" +"""Shared simulation-state capture and restore helpers for trajectories. + +All seven-element root and rigid-object poses use EmbodiChain's +``(x, y, z, qx, qy, qz, qw)`` convention. +""" from __future__ import annotations diff --git a/embodichain/lab/scripts/analyze_workspace.py b/embodichain/lab/scripts/analyze_workspace.py index bc28c1b72..c6b0ed8e8 100644 --- a/embodichain/lab/scripts/analyze_workspace.py +++ b/embodichain/lab/scripts/analyze_workspace.py @@ -297,7 +297,7 @@ def _build_asset_robot_cfg( ValueError: If ``--ee-link`` is missing, or a USD/non-URDF asset is given without ``--urdf``. """ - from embodichain.lab.sim.cfg import RobotCfg + from embodichain.lab.sim.cfg import ArticulationRootPropertiesCfg, RobotCfg from embodichain.lab.sim.motion.solvers import ( PinkSolverCfg, PinocchioSolverCfg, @@ -344,8 +344,10 @@ def _build_asset_robot_cfg( cfg.fpath = asset cfg.init_pos = tuple(args.init_pos) cfg.init_rot = tuple(args.init_rot) - cfg.fix_base = args.fix_base - cfg.use_usd_properties = args.use_usd_properties + cfg.root_props = ArticulationRootPropertiesCfg( + fixed_base=args.fix_base, + ) + cfg.asset_physics_mode = args.asset_physics_mode cfg.control_parts = {control_part: joints} cfg.solver_cfg = {control_part: solver_cfg} return cfg, control_part, solver_urdf @@ -652,6 +654,7 @@ def main(args: argparse.Namespace) -> None: if robot is None: log_error("Failed to load robot into the simulation.") return + sim.prepare() control_part = _resolve_control_part(robot, control_part) joints_desc = ( robot.control_parts.get(control_part) if control_part else "all joints" @@ -864,10 +867,13 @@ def parse_args(argv: Sequence[str] | None = None) -> argparse.Namespace: help="Fix the robot base (default: fixed).", ) asset_opts.add_argument( - "--use-usd-properties", - action="store_true", - default=False, - help="Use physical properties from the USD file (USD assets only).", + "--asset-physics-mode", + choices=("preserve", "overlay"), + default="overlay", + help=( + "How asset physics is handled: preserve source-authored values or " + "overlay explicitly configured values (default: overlay for robots)." + ), ) # --- Analysis ----------------------------------------------------------- diff --git a/embodichain/lab/scripts/preview_asset.py b/embodichain/lab/scripts/preview_asset.py index 0bb4f1416..26cfa3d1a 100644 --- a/embodichain/lab/scripts/preview_asset.py +++ b/embodichain/lab/scripts/preview_asset.py @@ -59,6 +59,7 @@ from collections.abc import Sequence from typing import TYPE_CHECKING +from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.utils.logger import log_info, log_warning, log_error if TYPE_CHECKING: @@ -78,14 +79,18 @@ def build_sim_cfg(args: argparse.Namespace) -> SimulationManagerCfg: Returns: SimulationManagerCfg: Simulation configuration. """ - from embodichain.lab.sim.cfg import RenderCfg + from embodichain.lab.sim.cfg import RenderCfg, physics_cfg_for_backend from embodichain.lab.sim.sim_manager import SimulationManagerCfg from embodichain.lab.visualization import visualization_cfg_from_args return SimulationManagerCfg( headless=args.headless, - sim_device=args.sim_device, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), + gpu_id=args.gpu_id, + num_envs=args.num_envs, + arena_space=args.arena_space, visualization=visualization_cfg_from_args(args), ) @@ -108,6 +113,7 @@ def load_assets( """ from embodichain.lab.sim.cfg import ( ArticulationCfg, + ArticulationRootPropertiesCfg, LightCfg, RigidObjectCfg, ) @@ -117,6 +123,7 @@ def load_assets( init_pos = tuple(args.init_pos) init_rot = tuple(args.init_rot) spacing = float(args.asset_spacing) + asset_physics_mode = args.asset_physics_mode loaded_assets = [] for idx, asset_path in enumerate(asset_paths): @@ -155,8 +162,10 @@ def load_assets( fpath=asset_path, init_pos=asset_init_pos, init_rot=init_rot, - fix_base=args.fix_base, - use_usd_properties=args.use_usd_properties, + root_props=ArticulationRootPropertiesCfg( + fixed_base=args.fix_base, + ), + asset_physics_mode=asset_physics_mode, # The auxiliary pytorch-kinematics chain only accepts URDF XML. build_pk_chain=asset_suffix not in {".usd", ".usda", ".usdc"}, ) @@ -173,7 +182,7 @@ def load_assets( init_pos=asset_init_pos, init_rot=init_rot, body_type=args.body_type, - use_usd_properties=args.use_usd_properties, + asset_physics_mode=asset_physics_mode, ) loaded_assets.append(sim.add_rigid_object(cfg)) @@ -339,6 +348,7 @@ def main(args: argparse.Namespace) -> None: sim.set_indirect_lighting(args.env_map) assets = load_assets(sim, args) + sim.prepare() log_info(f"Loaded {len(assets)} asset(s) successfully.", color="green") joint_controller = _setup_viser_joint_control(sim, assets, args) _publish_loaded_assets(sim, args) @@ -354,6 +364,7 @@ def _create_parser() -> argparse.ArgumentParser: prog="embodichain preview-asset", description="Preview a USD or mesh asset in the EmbodiChain simulation.", ) + add_env_launcher_args_to_parser(parser) parser.add_argument( "--asset_path", @@ -408,10 +419,15 @@ def _create_parser() -> argparse.ArgumentParser: help="Body type for rigid objects (default: kinematic).", ) parser.add_argument( - "--use_usd_properties", - action="store_true", - default=False, - help="Use physical properties from the USD file instead of defaults.", + "--asset_physics_mode", + "--asset-physics-mode", + dest="asset_physics_mode", + choices=("preserve", "overlay"), + default="overlay", + help=( + "Preserve source-authored physics or overlay explicitly configured " + "values (default: overlay)." + ), ) parser.add_argument( "--fix_base", @@ -419,25 +435,6 @@ def _create_parser() -> argparse.ArgumentParser: default=True, help="Fix or unfix the base of articulations (default: fixed).", ) - parser.add_argument( - "--sim_device", - type=str, - default="cpu", - help="Simulation device (default: cpu).", - ) - parser.add_argument( - "--headless", - action="store_true", - default=False, - help="Run without rendering window.", - ) - parser.add_argument( - "--renderer", - type=str, - choices=["hybrid", "fast-rt", "rt"], - default="hybrid", - help="Renderer backend (default: hybrid).", - ) parser.add_argument( "--env_map", type=str, @@ -447,12 +444,6 @@ def _create_parser() -> argparse.ArgumentParser: "name (e.g. 'Studio') or an absolute file path (.hdr/.png/.exr)." ), ) - parser.add_argument( - "--preview", - action="store_true", - default=False, - help="Enter interactive embed mode after loading.", - ) parser.add_argument( "--joint-control", action=argparse.BooleanOptionalAction, @@ -463,9 +454,6 @@ def _create_parser() -> argparse.ArgumentParser: ), ) - from embodichain.lab.visualization import add_viser_args_to_parser - - add_viser_args_to_parser(parser) return parser diff --git a/embodichain/lab/sim/_runtime_controls.py b/embodichain/lab/sim/_runtime_controls.py new file mode 100644 index 000000000..6c0b23954 --- /dev/null +++ b/embodichain/lab/sim/_runtime_controls.py @@ -0,0 +1,124 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Internal adapters for manager-owned Newton runtime controls.""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + +from dexsim.engine.newton_physics.runtime_control import RuntimeControl + +__all__: list[str] = [] + + +class _KinematicNodalTrajectoryControl(RuntimeControl): + """Drive selected particles along offsets from their initialized positions.""" + + def __init__( + self, + target: str, + node_indices: np.ndarray, + position_offsets: np.ndarray, + *, + fps: float | None, + rebuild_self_contact_bvh: bool, + ) -> None: + self.target = target + self.node_indices = np.asarray(node_indices, dtype=np.int32).reshape(-1).copy() + self.position_offsets = ( + np.asarray(position_offsets, dtype=np.float32) + .reshape(-1, len(self.node_indices), 3) + .copy() + ) + self.fps = fps + self.rebuild_self_contact_bvh = rebuild_self_contact_bvh + self._particle_set: Any | None = None + self._initial_positions: np.ndarray | None = None + self._sample_index = 0 + self._elapsed_time = 0.0 + + def initialize(self, context: Any) -> None: + """Resolve the preconfigured inactive particles before the first substep.""" + particle_set = context.result.get_particle_set(self.target) + if np.any(self.node_indices >= particle_set.particle_count): + raise ValueError( + f"Kinematic node index exceeds particle count for {self.target!r}: " + f"max index {int(self.node_indices.max())}, particle count " + f"{particle_set.particle_count}." + ) + + positions = np.asarray( + particle_set.get_particle_positions().numpy(), + dtype=np.float32, + ).reshape(particle_set.particle_count, 3) + self._initial_positions = positions[self.node_indices].copy() + self._particle_set = particle_set + self._sample_index = 0 + self._elapsed_time = 0.0 + + def exclusive_resource_claims(self) -> tuple[object, ...]: + """Prevent multiple controls from writing the same particle set.""" + return (("kinematic_nodal_trajectory", self.target),) + + def __call__( + self, + context: Any, + substep_index: int, + substep_count: int, + substep_dt: float, + ) -> None: + """Apply the interpolated target before one Newton substep.""" + del substep_count + if self._particle_set is None or self._initial_positions is None: + raise RuntimeError("Kinematic nodal trajectory was not initialized.") + + if self.rebuild_self_contact_bvh and substep_index == 0: + rebuild_bvh = getattr(context.solver, "rebuild_bvh", None) + if callable(rebuild_bvh): + rebuild_bvh(context.current_state) + + offsets = self._current_offsets() + positions = np.asarray( + self._particle_set.get_particle_positions().numpy(), + dtype=np.float32, + ).reshape(self._particle_set.particle_count, 3) + positions[self.node_indices] = self._initial_positions + offsets + self._particle_set.set_particle_positions(positions) + + self._sample_index += 1 + self._elapsed_time += float(substep_dt) + + def _current_offsets(self) -> np.ndarray: + """Return the current sample or its time-interpolated value.""" + if self.fps is None: + sample = min(self._sample_index, len(self.position_offsets) - 1) + return self.position_offsets[sample] + + sample_position = self._elapsed_time * self.fps + lower = min(int(np.floor(sample_position)), len(self.position_offsets) - 1) + upper = min(lower + 1, len(self.position_offsets) - 1) + alpha = np.float32(sample_position - lower if upper != lower else 0.0) + return (np.float32(1.0) - alpha) * self.position_offsets[ + lower + ] + alpha * self.position_offsets[upper] + + def close(self) -> None: + """Release runtime particle references retained after initialization.""" + self._particle_set = None + self._initial_positions = None diff --git a/embodichain/lab/sim/_startup_summary.py b/embodichain/lab/sim/_startup_summary.py new file mode 100644 index 000000000..a35ec7b96 --- /dev/null +++ b/embodichain/lab/sim/_startup_summary.py @@ -0,0 +1,324 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Private, read-only startup snapshots and terminal table formatting.""" + +from __future__ import annotations + +import os +import shutil +import sys +from collections.abc import Mapping, Sequence +from typing import TYPE_CHECKING + +import dexsim +import torch +from prettytable import PrettyTable, TableStyle + +from embodichain import __version__ +from embodichain.lab.sim.cfg import DefaultPhysicsCfg, NewtonPhysicsCfg + +if TYPE_CHECKING: + from embodichain.lab.sim.sim_manager import SimulationManager + +Row = tuple[str, str, str] + + +def _switch(value: bool) -> str: + return "ON" if value else "OFF" + + +def _selection(requested: str, resolved: str | None) -> str: + if resolved is None or resolved == "auto": + return f"{requested} -> PENDING" + return f"{requested} -> {resolved}" if requested != resolved else resolved + + +def _solver(sim: SimulationManager) -> str: + if sim.physics.name == "default": + return str(sim.physics.solver_type) + return _selection(sim._requested_solver, sim.physics.solver_type) + + +def simulation_rows(sim: SimulationManager) -> list[Row]: + """Read engine configuration without preparing or stepping the scene.""" + cfg = sim.sim_config + physics = cfg.physics_cfg + render = cfg.render_cfg + gpu_name = getattr(sim, "_render_device_name", None) or "name unavailable" + gpu = f"{gpu_name} · GPU {cfg.gpu_id}" + rows = [ + ( + "Runtime", + "Versions", + f"EmbodiChain {__version__} · DexSim {dexsim.__version__}", + ), + ("Runtime", "Compute device", str(sim.device)), + ( + "Runtime", + "Parallel environments", + f"{sim.num_envs} · spacing {cfg.arena_space:g} m", + ), + ("Rendering", "Renderer", _selection(sim._requested_renderer, render.renderer)), + ("Rendering", "Graphics API", "Vulkan"), + ("Rendering", "Render GPU", gpu), + ("Rendering", "Native window", "OPEN" if sim.is_window_opened else "CLOSED"), + ( + "Rendering", + "Browser viewer", + cfg.visualization.backend if cfg.visualization.backend != "none" else "OFF", + ), + ("Rendering", "Viewer resolution", f"{cfg.width} × {cfg.height}"), + ("Rendering", "Tone mapping", _switch(render.tone_mapping_enabled)), + ( + "Physics", + "Backend", + "Default" if sim.physics.name == "default" else "Newton", + ), + ("Physics", "Solver", _solver(sim)), + ("Physics", "Collision policy", "isolated"), + ( + "Physics", + "Physics timestep", + f"{physics.physics_dt * 1000:g} ms ({1 / physics.physics_dt:g} Hz)", + ), + ( + "Physics", + "Gravity", + f"[{', '.join(f'{x:g}' for x in physics.gravity)}] m/s²", + ), + ] + if isinstance(physics, NewtonPhysicsCfg): + rows.extend( + [ + ( + "Physics", + "Solver substeps", + f"{physics.num_substeps} × {physics.physics_dt * 1000 / physics.num_substeps:g} ms", + ), + ("Physics", "CUDA Graph", sim.physics.cuda_graph_status.upper()), + ("Physics", "Gradients", _switch(physics.requires_grad)), + ] + ) + if cfg.startup_summary == "full": + rows.extend( + [ + ( + "Rendering detail", + "Samples per frame", + f"{render.spp} (configured; renderer-dependent)", + ), + ("Rendering detail", "Denoiser", "ON (configured; renderer-dependent)"), + ( + "Rendering detail", + "Exposure", + f"{render.tone_mapping_exposure:g}" + + (" (inactive)" if not render.tone_mapping_enabled else ""), + ), + ] + ) + if isinstance(physics, DefaultPhysicsCfg): + rows.extend( + [ + ( + "Physics detail", + "Bounce threshold", + f"{physics.bounce_threshold:g} m/s", + ), + ( + "Physics detail", + "Tolerance scale", + f"length {physics.length_tolerance:g} m · speed {physics.speed_tolerance:g} m/s", + ), + ] + ) + if sim.device.type == "cuda": + rows.extend( + [ + ( + "Physics detail", + "Contact capacity", + str(physics.gpu_memory.max_rigid_contact_count), + ), + ( + "Physics detail", + "Patch capacity", + str(physics.gpu_memory.max_rigid_patch_count), + ), + ( + "Physics detail", + "Heap capacity", + f"{physics.gpu_memory.heap_capacity / 2**20:g} MiB", + ), + ] + ) + elif physics.collision_cfg is not None: + collision = physics.collision_cfg + rows.extend( + [ + ( + "Physics detail", + "Broad phase", + str( + collision.broad_phase + or physics.broad_phase + or "backend default" + ), + ), + ( + "Physics detail", + "Collision update", + f"every {collision.update_interval or physics.num_substeps} solver substep(s)", + ), + ( + "Physics detail", + "Contact capacity", + str(collision.rigid_contact_max or "scene-derived"), + ), + ] + ) + if isinstance(physics, NewtonPhysicsCfg): + solver = physics.solver_cfg + if isinstance(solver, Mapping): + rows.extend( + ("Solver config", str(key), str(value)) + for key, value in solver.items() + ) + rows.extend( + [ + ( + "System detail", + "Python / PyTorch", + f"{sys.version.split()[0]} / {torch.__version__}", + ), + ( + "System detail", + "CUDA runtime", + str(torch.version.cuda or "unavailable"), + ), + ( + "System detail", + "Cache", + str(getattr(sim, "_sim_cache_dir", "not initialized")), + ), + ] + ) + return rows + + +def scene_is_ready(sim: SimulationManager) -> bool: + """Require successful preparation of the current, unchanged topology.""" + result = sim.spawn_result + if result is None or result.needs_rebuild: + return False + scene = getattr(sim, "_spawn_scene", None) + if scene is not None and scene.builder.has_pending_changes: + return False + return ( + getattr(sim, "_ready_spawn_topology_revision", -1) == result.topology_revision + ) + + +def scene_rows(sim: SimulationManager) -> list[Row]: + """Read the current scene snapshot, with counts per replicated environment.""" + if not scene_is_ready(sim): + return [("Scene", "State", "PENDING (not prepared)")] + rows = [] + if sim.physics.name == "newton": + rows.extend( + [ + ("Physics resolved", "Solver", _solver(sim)), + ( + "Physics resolved", + "CUDA Graph", + sim.physics.cuda_graph_status.upper(), + ), + ] + ) + rows.extend( + [ + ("Scene / env", "Robots", str(len(sim._robots))), + ("Scene / env", "Articulations", str(len(sim._articulations))), + ("Scene / env", "Rigid objects", str(len(sim._rigid_objects))), + ("Scene / env", "Object groups", str(len(sim._rigid_object_groups))), + ("Scene / env", "Deformables", str(len(sim._deformable_objects))), + ("Scene / env", "Sensors", str(len(sim._sensors))), + ( + "Scene", + "Default ground", + "GLOBAL" if sim._default_plane is not None else "NONE", + ), + ("Scene", "Native window", "OPEN" if sim.is_window_opened else "CLOSED"), + ] + ) + for uid, sensor in sim._sensors.items(): + sensor_cfg = sensor.cfg + description = type(sensor).__name__ + if hasattr(sensor_cfg, "width"): + description += f" · {sensor_cfg.width} × {sensor_cfg.height}" + description += " · " + "/".join(sensor_cfg.get_data_types()) + rows.append(("Sensors / env", uid, description)) + rows.append(("Scene", "State", "READY")) + return rows + + +def format_summary( + title: str, + rows: Sequence[Row], + *, + color: bool | None = None, + width: int | None = None, +) -> str: + """Render one bounded-width table; ANSI styling never changes cell layout.""" + if color is None: + color = "NO_COLOR" not in os.environ and sys.stderr.isatty() + width = max(64, min(width or shutil.get_terminal_size((100, 24)).columns, 120)) + table = PrettyTable(["Section", "Setting", "Value"]) + table.set_style(TableStyle.SINGLE_BORDER) + table.align = "l" + table.title = f"EmbodiChain · {title}" + table.max_width = {"Section": 16, "Setting": 22, "Value": width - 48} + previous = None + for index, (section, key, value) in enumerate(rows): + table.add_row( + [section if section != previous else "", key, value], + divider=index + 1 < len(rows) and rows[index + 1][0] != section, + ) + previous = section + lines = table.get_string().splitlines() + lines[0] = "╭" + lines[0][1:-1] + "╮" + lines[-1] = "╰" + lines[-1][1:-1] + "╯" + if color: + for index, line in enumerate(lines): + cells = line.split("│") + if len(cells) == 5: + if cells[1].strip(): + cells[1] = f"\033[1;36m{cells[1]}\033[0m" + value = cells[3] + code = ( + "1;33" + if "PENDING" in value + else ( + "1;32" + if value.strip() in {"READY", "ON", "CAPTURED", "OPEN"} + else "1;37" + ) + ) + cells[3] = f"\033[{code}m{value}\033[0m" + lines[index] = "│".join(cells) + elif "EmbodiChain ·" in line: + lines[index] = f"\033[1;36m{line}\033[0m" + return "\n".join(lines) diff --git a/embodichain/lab/sim/cfg.py b/embodichain/lab/sim/cfg.py deleted file mode 100644 index 05781b49f..000000000 --- a/embodichain/lab/sim/cfg.py +++ /dev/null @@ -1,2108 +0,0 @@ -# ---------------------------------------------------------------------------- -# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# ---------------------------------------------------------------------------- - -from __future__ import annotations - -import enum -import json -import math -import os - -import dexsim -import numpy as np -import torch - -from typing import Sequence, Dict, Literal, List, Any, Optional -from dataclasses import field, MISSING -from numbers import Real - -from dexsim.types import ( - DenoiserType, - Renderer, - ToneMappingType, - PhysicalAttr, - ActorType, - AxisArrowType, - AxisCornerType, - VoxelConfig, - SoftBodyAttr, - SoftBodyMaterialModel, - ClothBodyAttr, -) -from embodichain.utils import configclass, is_configclass -from embodichain.data.constants import EMBODICHAIN_DEFAULT_DATA_ROOT -from embodichain.data import get_data_path -from embodichain.utils import logger -from embodichain.utils.utility import key_in_nested_dict - -from .shapes import ShapeCfg, MeshCfg -from .motion.workspace.cfg import RobotWorkspaceCfg - -__all__ = [ - "DEFAULT_RENDERER", - "DLSSCfg", - "RenderCfg", - "PhysicsCfg", - "MarkerCfg", - "WindowRecordCfg", - "WindowCameraPoseCfg", - "GPUMemoryCfg", - "RigidBodyAttributesCfg", - "RigidBodyAttributesOverrideCfg", - "LinkPhysicsOverrideCfg", - "link_attrs_from_dict", - "SoftbodyVoxelAttributesCfg", - "SoftbodyPhysicalAttributesCfg", - "ClothPhysicalAttributesCfg", - "JointDrivePropertiesCfg", - "ObjectBaseCfg", - "LightCfg", - "RigidObjectCfg", - "SoftObjectCfg", - "ClothObjectCfg", - "RigidObjectGroupCfg", - "RigidConstraintCfg", - "URDFCfg", - "ArticulationCfg", - "RobotCfg", -] - -# Global default renderer settings for simulation. -# -# The sentinel value ``"auto"`` defers the choice to GPU-based auto-selection -# performed lazily when a :class:`SimulationManager` is constructed (see -# :func:`embodichain.lab.sim.utility.render_utils.select_default_renderer`). Assigning a -# concrete renderer here (e.g. in test fixtures) forces that renderer and takes -# precedence over auto-selection. -DEFAULT_RENDERER: Literal["auto", "hybrid", "fast-rt", "rt"] = "auto" - - -@configclass -class DLSSCfg: - """DexSim DLSS configuration for window and offscreen rendering. - - Ray Reconstruction (RR) and Super Resolution (SR) are independently - configurable on the ``"hybrid"``, ``"fast-rt"``, and ``"rt"`` renderers. - DLSS is enabled by default for both windows and offscreen cameras. - Offscreen DLSS also requires the master switch to remain enabled. - - .. attention:: - DLSS requires a Vulkan render device, a compatible NVIDIA GPU/driver, - and a DexSim build with the NGX runtime. Initialization is deferred - until rendering; configuration conversion alone cannot verify support. - Each enabled offscreen camera needs its own temporal history and - Vulkan exchange images, increasing GPU memory use. - """ - - dlss_enabled: bool = True - """Master switch for DLSS. False retains the standard rendering path.""" - - offscreen_dlss_enabled: bool = True - """Enable DLSS for offscreen cameras, including in headless simulations.""" - - rayreconstruction_enabled: bool = True - """Enable RR denoising. Can be used without SR at the target resolution.""" - - upscale_enabled: bool = True - """Enable SR upscaling. Can be used independently of RR.""" - - dlss_quality: int = 2 - """Quality mode and derived internal scale: ``-1`` auto (58%), ``0`` Ultra - Performance (~33%), ``1`` Performance (50%), ``2`` Balanced (58%), - ``3`` Quality (~67%), ``4`` Ultra Quality (77%), ``5`` DLAA (100%).""" - - upsample_ratio: float | None = None - """Optional window target/render ratio, at least 1.0. None leaves zero - render dimensions for DexSim to derive from quality. When specified, - computes each unset render dimension from the actual window size. Only - FastRT/OfflineRT windows honor these overrides; hybrid and offscreen - targets derive their internal resolution from quality.""" - - render_width: int = 0 - """Internal FastRT/OfflineRT window width; zero derives it from quality.""" - - render_height: int = 0 - """Internal FastRT/OfflineRT window height; zero derives it from quality.""" - - target_width: int = 0 - """DexSim compatibility field. Set the actual window or camera width instead.""" - - target_height: int = 0 - """DexSim compatibility field. Set the actual window or camera height instead.""" - - exposure_compensation: float = 1.0 - """Positive, finite exposure multiplier used by the RR bridge.""" - - frame_time_delta_ms: float = 0.0 - """Frame interval in milliseconds passed to DexSim's DLSS temporal path. - - The default ``0.0`` intentionally matches ``dexsim.DLSSConfig``: DexSim - measures the actual render interval automatically. Set a positive value - only for a fixed render cadence; this is a render-frame interval, not a - physics or control timestep. - """ - - def __post_init__(self) -> None: - """Validate scalar types and the ranges of numeric settings.""" - for name in ( - "dlss_enabled", - "offscreen_dlss_enabled", - "rayreconstruction_enabled", - "upscale_enabled", - ): - if not isinstance(getattr(self, name), bool): - raise ValueError(f"DLSSCfg.{name} must be a boolean.") - if type(self.dlss_quality) is not int or not -1 <= self.dlss_quality <= 5: - raise ValueError("DLSSCfg.dlss_quality must be an integer from -1 to 5.") - for name in ("render_width", "render_height", "target_width", "target_height"): - value = getattr(self, name) - if type(value) is not int or value < 0: - raise ValueError(f"DLSSCfg.{name} must be a non-negative integer.") - if self.upsample_ratio is not None and ( - isinstance(self.upsample_ratio, bool) - or not isinstance(self.upsample_ratio, Real) - or not math.isfinite(self.upsample_ratio) - or self.upsample_ratio < 1.0 - ): - raise ValueError( - "DLSSCfg.upsample_ratio must be a finite number of at least 1.0." - ) - if ( - isinstance(self.exposure_compensation, bool) - or not isinstance(self.exposure_compensation, Real) - or not math.isfinite(self.exposure_compensation) - or self.exposure_compensation <= 0.0 - ): - raise ValueError( - "DLSSCfg.exposure_compensation must be a positive, finite number." - ) - - def to_dexsim_cfg(self, window_width: int, window_height: int) -> dexsim.DLSSConfig: - """Convert settings without changing the window or camera output size. - - Args: - window_width: Window width in pixels. - window_height: Window height in pixels. - - Returns: - Populated :class:`dexsim.DLSSConfig` instance ready to assign to - ``world_config.dlss_config``. - - Raises: - ValueError: If the configuration contains invalid values. - """ - self.__post_init__() - dlss = dexsim.DLSSConfig() - dlss.dlss_enabled = self.dlss_enabled - dlss.offscreen_dlss_enabled = self.offscreen_dlss_enabled - dlss.rayreconstruction_enabled = self.rayreconstruction_enabled - dlss.upscale_enabled = self.upscale_enabled - dlss.dlss_quality = self.dlss_quality - dlss.render_width = self.render_width - dlss.render_height = self.render_height - if self.upsample_ratio is not None: - if self.render_width == 0: - dlss.render_width = max(1, int(window_width / self.upsample_ratio)) - if self.render_height == 0: - dlss.render_height = max(1, int(window_height / self.upsample_ratio)) - dlss.target_width = self.target_width - dlss.target_height = self.target_height - dlss.exposure_compensation = self.exposure_compensation - dlss.frame_time_delta_ms = self.frame_time_delta_ms - return dlss - - -@configclass -class RenderCfg: - renderer: Literal["auto", "hybrid", "fast-rt", "rt"] = "auto" - """Renderer backend to use for the simulation. Options are 'auto', 'hybrid', 'fast-rt', and 'rt'. - - Note: - - 'auto' selects a default renderer based on the detected GPU: RTX-series cards use - 'hybrid', while datacenter cards (A100/A800, H100/H800/H200/H20) use 'fast-rt'. - If no CUDA device is available or the GPU is unknown, it falls back to 'hybrid'. - - 'hybrid' uses ray tracing for shadows and reflections while keeping rasterization for primary rendering, - providing a balance between performance and visual quality. - - 'fast-rt' is a fully ray-traced renderer for maximum visual fidelity, but may have higher computational cost. - - 'rt' is an offline ray-traced renderer for maximum visual fidelity, suitable for high-quality rendering tasks. - """ - - spp: int = 1 - """Samples per pixel for ray tracing rendering. This parameter is only valid when renderer is 'hybrid', 'fast-rt' or 'rt'.""" - - dlss: DLSSCfg = field(default_factory=DLSSCfg) - """DLSS settings for hybrid, fast-rt, and rt windows and offscreen cameras.""" - - tone_mapping_enabled: bool = False - """Whether to map HDR RGB output with the modified Reinhard curve.""" - - tone_mapping_exposure: float = 1.0 - """Fixed linear exposure multiplier applied before tone mapping.""" - - def __post_init__(self) -> None: - """Validate rendering parameters.""" - if self.spp < 1: - logger.log_error("RenderCfg.spp must be at least 1.", ValueError) - if self.tone_mapping_exposure < 0.0: - logger.log_error( - "RenderCfg.tone_mapping_exposure must be non-negative.", ValueError - ) - - def to_dexsim_flags(self) -> Renderer: - """Convert the renderer name to DexSim's renderer enum.""" - if self.renderer == "hybrid": - return Renderer.HYBRID - elif self.renderer == "fast-rt": - return Renderer.FASTRT - elif self.renderer == "rt": - return Renderer.OFFLINERT - elif self.renderer == "auto": - # 'auto' is normally resolved by the SimulationManager before this is - # called. If it reaches here (e.g. used standalone), fall back safely. - logger.log_warning( - "Renderer 'auto' was not resolved before converting to dexsim flags. " - "Falling back to 'hybrid'." - ) - return Renderer.HYBRID - else: - logger.log_error( - f"Invalid renderer type '{self.renderer}' specified. Must be one of 'auto', 'hybrid', 'fast-rt', or 'rt'." - ) - - def apply_to_dexsim_config(self, world_config: dexsim.WorldConfig) -> None: - """Apply rendering settings to a DexSim world configuration. - - Args: - world_config: DexSim world configuration to update in place. - """ - world_config.renderer = self.to_dexsim_flags() - world_config.dlss_config = self.dlss.to_dexsim_cfg( - window_width=world_config.win_config.width, - window_height=world_config.win_config.height, - ) - world_config.raytrace_config.render_iterations_per_frame = self.spp - world_config.raytrace_config.open_denoise = True - world_config.raytrace_config.denoiser_type = DenoiserType.OPTIX - world_config.postprocess_config.tone_mapping_enabled = self.tone_mapping_enabled - world_config.postprocess_config.tone_mapping_type = ( - ToneMappingType.MODIFIED_REINHARD - ) - world_config.postprocess_config.tone_mapping_exposure = ( - self.tone_mapping_exposure - ) - - -@configclass -class PhysicsCfg: - gravity: np.ndarray = field(default_factory=lambda: np.array([0, 0, -9.81])) - """Gravity vector for the simulation environment.""" - - bounce_threshold: float = 2.0 - """The speed threshold below which collisions will not produce bounce effects.""" - - enable_ccd: bool = False - """Enable continuous collision detection (CCD) for fast-moving objects.""" - - length_tolerance: float = 0.05 - """The length tolerance for the simulation. - - Note: the larger the tolerance, the faster the simulation will be. - """ - speed_tolerance: float = 0.25 - """The speed tolerance for the simulation. - - Note: the larger the tolerance, the faster the simulation will be. - """ - - def to_dexsim_args(self) -> Dict[str, Any]: - """Convert to DexSim physics arguments. - - Solver implementation details that are not exposed by :class:`PhysicsCfg` - retain their established defaults here. - """ - args = { - "gravity": self.gravity.tolist(), - "bounce_threshold": self.bounce_threshold, - "enable_ccd": self.enable_ccd, - "enable_enhanced_determinism": False, - "enable_friction_every_iteration": True, - } - return args - - -@configclass -class MarkerCfg: - """Configuration for visual markers in the simulation. - - This class defines properties for creating visual markers such as coordinate frames, - lines, and points that can be used for debugging, visualization, or reference purposes - in the simulation environment. - """ - - name: str = "empty-mesh" - """Name of the marker for identification purposes.""" - - marker_type: Literal["axis", "line", "point"] = "axis" - """Type of marker to display. Can be 'axis' (3D coordinate frame), 'line', or 'point'. (only axis supported now)""" - - axis_xpos: torch.Tensor | None = None - """List of 4x4 transformation matrices defining the position and orientation of each axis marker.""" - - axis_size: float = 0.002 - """Thickness/size of the axis lines in meters.""" - - axis_len: float = 0.005 - """Length of each axis arm in meters.""" - - line_color: List[float] = [1, 1, 0, 1.0] - """RGBA color values for the marker lines. Values should be between 0.0 and 1.0.""" - - arrow_type: AxisArrowType = AxisArrowType.CONE - """Type of arrow head for axis markers (e.g., CONE, ARROW, etc.).""" - - corner_type: AxisCornerType = AxisCornerType.SPHERE - """Type of corner/joint visualization for axis markers (e.g., SPHERE, CUBE, etc.).""" - - arena_index: int = -1 - """Index of the arena where the marker should be placed. -1 means all arenas.""" - - -@configclass -class WindowRecordCfg: - """Configuration for interactive viewer window recording.""" - - enable_hotkey: bool = True - """Whether to register the ``r`` hotkey for viewer recording when the window opens.""" - - save_path: str | None = None - """Optional output path for viewer recordings. If None, use the default outputs directory.""" - - fps: int = 20 - """Frames per second for viewer recording.""" - - max_memory: int = 1024 - """Maximum buffered recording memory in MB before auto-stopping capture.""" - - video_prefix: str = "viewer_record" - """Video file prefix used when no explicit save path is provided.""" - - -@configclass -class WindowCameraPoseCfg: - """Configuration for printing the interactive viewer camera pose.""" - - enable_hotkey: bool = True - """Whether to register the ``p`` hotkey when the window opens.""" - - convert_to_look_at: bool = True - """Whether the hotkey prints a ``set_look_at`` call instead of a matrix.""" - - -@configclass -class GPUMemoryCfg: - """A gpu memory configuration dataclass that neatly holds all parameters that configure physics GPU memory for simulation""" - - temp_buffer_capacity: int = 2**24 - """Increase this if you get 'PxgPinnedHostLinearMemoryAllocator: overflowing initial allocation size, increase capacity to at least %.' """ - - max_rigid_contact_count: int = 2**19 - """Increase this if you get 'Contact buffer overflow detected'""" - - max_rigid_patch_count: int = ( - 2**18 - ) # 81920 is DexSim default but most tasks work with 2**18 - """Increase this if you get 'Patch buffer overflow detected'""" - - heap_capacity: int = 2**26 - - found_lost_pairs_capacity: int = ( - 2**25 - ) # 262144 is DexSim default but most tasks work with 2**25 - found_lost_aggregate_pairs_capacity: int = 2**10 - total_aggregate_pairs_capacity: int = 2**10 - - -@configclass -class RigidBodyAttributesCfg: - """Physical attributes for rigid bodies. - - There are three parts of attributes that can be set: - 1. The dynamic properties, such as mass, damping, etc. - 2. The collision properties. - 3. The physics material properties. - """ - - mass: float = 1.0 - """Mass of the rigid body in kilograms. - - Set to 0 will use density to calculate mass. - """ - - density: float = 1000.0 - """Density of the rigid body in kg/m^3.""" - - angular_damping: float = 0.7 - """Angular damping coefficient.""" - - linear_damping: float = 0.7 - """Linear damping coefficient.""" - - max_depenetration_velocity: float = 10.0 - """Maximum depenetration velocity.""" - - sleep_threshold: float = 0.001 - """Threshold below which the body can go to sleep.""" - - min_position_iters: int = 4 - """Minimum position iterations.""" - - min_velocity_iters: int = 1 - """Minimum velocity iterations.""" - - max_linear_velocity: float = 1e2 - """Maximum linear velocity.""" - - max_angular_velocity: float = 1e2 - """Maximum angular velocity.""" - - # collision properties. - enable_ccd: bool = False - """Enable continuous collision detection (CCD).""" - - contact_offset: float = 0.002 - """Contact offset for collision detection.""" - - rest_offset: float = 0.0 - """Rest offset for collision detection.""" - - enable_collision: bool = True - """Enable collision for the rigid body.""" - - # physics material properties. - restitution: float = 0.0 - """Restitution (bounciness) coefficient.""" - - dynamic_friction: float = 0.5 - """Dynamic friction coefficient.""" - - static_friction: float = 0.5 - """Static friction coefficient.""" - - def attr(self) -> PhysicalAttr: - """Convert to dexsim PhysicalAttr""" - attr = PhysicalAttr() - attr.mass = self.mass - attr.contact_offset = self.contact_offset - attr.rest_offset = self.rest_offset - attr.dynamic_friction = self.dynamic_friction - attr.static_friction = self.static_friction - attr.angular_damping = self.angular_damping - attr.linear_damping = self.linear_damping - attr.sleep_threshold = self.sleep_threshold - attr.restitution = self.restitution - attr.enable_ccd = self.enable_ccd - attr.max_linear_velocity = self.max_linear_velocity - attr.max_angular_velocity = self.max_angular_velocity - attr.max_depenetration_velocity = self.max_depenetration_velocity - attr.min_position_iters = self.min_position_iters - attr.min_velocity_iters = self.min_velocity_iters - return attr - - @classmethod - def from_dict( - cls, init_dict: Dict[str, str | float | int] - ) -> RigidBodyAttributesCfg: - """Initialize the configuration from a dictionary.""" - cfg = cls() - for key, value in init_dict.items(): - if hasattr(cfg, key): - setattr(cfg, key, value) - else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) - return cfg - - -@configclass -class RigidBodyAttributesOverrideCfg: - """Partial rigid-body attribute overrides for per-link physics configuration. - - Fields set to ``None`` are not applied and retain values from the base - :class:`RigidBodyAttributesCfg`. - """ - - mass: float | None = None - density: float | None = None - angular_damping: float | None = None - linear_damping: float | None = None - max_depenetration_velocity: float | None = None - sleep_threshold: float | None = None - min_position_iters: int | None = None - min_velocity_iters: int | None = None - max_linear_velocity: float | None = None - max_angular_velocity: float | None = None - enable_ccd: bool | None = None - contact_offset: float | None = None - rest_offset: float | None = None - enable_collision: bool | None = None - restitution: float | None = None - dynamic_friction: float | None = None - static_friction: float | None = None - - def merge_with(self, base: RigidBodyAttributesCfg) -> PhysicalAttr: - """Build a :class:`~dexsim.types.PhysicalAttr` from base values and overrides.""" - merged = RigidBodyAttributesCfg() - for field_name in merged.__dataclass_fields__: - override_val = getattr(self, field_name) - if override_val is not None: - setattr(merged, field_name, override_val) - else: - setattr(merged, field_name, getattr(base, field_name)) - return merged.attr() - - @classmethod - def from_dict( - cls, init_dict: Dict[str, str | float | int | bool] - ) -> RigidBodyAttributesOverrideCfg: - """Initialize the configuration from a dictionary.""" - cfg = cls() - for key, value in init_dict.items(): - if hasattr(cfg, key): - setattr(cfg, key, value) - else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) - return cfg - - -@configclass -class LinkPhysicsOverrideCfg: - """Per-link physics override matched by regex on articulation link names.""" - - link_names_expr: list[str] = MISSING - """Regex patterns matched against link names (full match).""" - - attrs: RigidBodyAttributesOverrideCfg = RigidBodyAttributesOverrideCfg() - """Partial attribute overrides applied on top of :attr:`ArticulationCfg.attrs`.""" - - replace_inertial: bool = False - """Whether to recompute inertia when mass is overridden (DexSim flag).""" - - @classmethod - def from_dict(cls, init_dict: Dict[str, Any]) -> LinkPhysicsOverrideCfg: - """Initialize the configuration from a dictionary.""" - cfg = cls() - for key, value in init_dict.items(): - if key == "attrs" and isinstance(value, dict): - setattr(cfg, key, RigidBodyAttributesOverrideCfg.from_dict(value)) - elif hasattr(cfg, key): - setattr(cfg, key, value) - else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) - return cfg - - -def link_attrs_from_dict( - value: dict[str, Any], -) -> dict[str, LinkPhysicsOverrideCfg]: - """Parse a ``link_attrs`` mapping from YAML/JSON-style dicts.""" - link_attrs: dict[str, LinkPhysicsOverrideCfg] = {} - for group_name, group_cfg in value.items(): - if isinstance(group_cfg, LinkPhysicsOverrideCfg): - link_attrs[group_name] = group_cfg - elif isinstance(group_cfg, dict): - link_attrs[group_name] = LinkPhysicsOverrideCfg.from_dict(group_cfg) - else: - raise TypeError( - f"link_attrs['{group_name}'] must be a dict or " - f"LinkPhysicsOverrideCfg, got {type(group_cfg)}." - ) - return link_attrs - - -@configclass -class SoftbodyVoxelAttributesCfg: - # voxel config - triangle_remesh_resolution: int = 8 - """Resolution to remesh the softbody mesh before building physics collision mesh.""" - - triangle_simplify_target: int = 0 - """Simplify mesh faces to target value. Do nothing if this value is zero.""" - - # TODO: this value will be automatically computed with simulation_mesh_resolution and mesh scale. - maximal_edge_length: float = 0 - # """To shorten edges that are too long, additional points get inserted at their center leading to a subdivision of the input mesh. Do nothing if this value is zero.""" - - simulation_mesh_resolution: int = 8 - """Resolution to build simulation voxelize textra mesh. This value must be greater than 0.""" - - simulation_mesh_output_obj: bool = False - """Whether to output the simulation mesh as an obj file for debugging.""" - - def attr(self) -> VoxelConfig: - """Convert to dexsim VoxelConfig""" - attr = VoxelConfig() - attr.triangle_remesh_resolution = self.triangle_remesh_resolution - attr.maximal_edge_length = self.maximal_edge_length - attr.simulation_mesh_resolution = self.simulation_mesh_resolution - attr.triangle_simplify_target = self.triangle_simplify_target - return attr - - -@configclass -class SoftbodyPhysicalAttributesCfg: - # material properties - youngs: float = 1e6 - """Young's modulus (higher = stiffer).""" - - poissons: float = 0.45 - """Poisson's ratio (higher = closer to incompressible).""" - - dynamic_friction: float = 0.0 - """Dynamic friction coefficient.""" - - elasticity_damping: float = 0.0 - """Elasticity damping factor.""" - - # soft body properties - material_model: SoftBodyMaterialModel = SoftBodyMaterialModel.CO_ROTATIONAL - """Material constitutive model.""" - - # --- Mode / collision switches --- - enable_kinematic: bool = False - """If True, (partially) kinematic behavior is enabled.""" - - enable_ccd: bool = False - """Enable continuous collision detection (CCD).""" - - enable_self_collision: bool = False - """Enable self-collision handling.""" - - has_gravity: bool = True - """Whether the soft body is affected by gravity.""" - - # --- Self-collision & simplification parameters --- - self_collision_stress_tolerance: float = 0.9 - """Stress tolerance threshold for self-collision constraints.""" - - collision_mesh_simplification: bool = True - """Whether to simplify the collision mesh for self-collision.""" - - self_collision_filter_distance: float = 0.1 - """Distance threshold below which vertex pairs may be filtered from self-collision checks.""" - - # --- Damping, sleep & settling --- - vertex_velocity_damping: float = 0.005 - """Per-vertex velocity damping.""" - - linear_damping: float = 0.0 - """Global linear damping applied to the soft body.""" - - sleep_threshold: float = 0.05 - """Velocity/energy threshold below which the soft body can go to sleep.""" - - settling_threshold: float = 0.1 - """Threshold used to decide convergence/settling state.""" - - settling_damping: float = 10.0 - """Additional damping applied during settling phase.""" - - # --- Mass / density & velocity limits --- - mass: float = -1.0 - """Total mass of the soft body. If set to a negative value, density will be used to compute mass.""" - - density: float = 1000.0 - """Material density in kg/m^3.""" - - max_depenetration_velocity: float = 1e6 - """Maximum velocity used to resolve penetrations. Must be larger than zero.""" - - max_velocity: float = 100 - """Clamp for linear (or vertex) velocity. If set to zero, the limit is ignored.""" - - # --- Solver iteration counts --- - min_position_iters: int = 4 - """Minimum solver iterations for position correction.""" - - min_velocity_iters: int = 1 - """Minimum solver iterations for velocity updates.""" - - def attr(self) -> SoftBodyAttr: - attr = SoftBodyAttr() - attr.youngs = self.youngs - attr.poissons = self.poissons - attr.dynamic_friction = self.dynamic_friction - attr.elasticity_damping = self.elasticity_damping - attr.material_model = self.material_model - attr.enable_kinematic = self.enable_kinematic - attr.enable_ccd = self.enable_ccd - attr.enable_self_collision = self.enable_self_collision - attr.has_gravity = self.has_gravity - attr.self_collision_stress_tolerance = self.self_collision_stress_tolerance - attr.collision_mesh_simplification = self.collision_mesh_simplification - attr.vertex_velocity_damping = self.vertex_velocity_damping - attr.mass = self.mass - attr.density = self.density - attr.max_depenetration_velocity = self.max_depenetration_velocity - attr.max_velocity = self.max_velocity - attr.self_collision_filter_distance = self.self_collision_filter_distance - attr.linear_damping = self.linear_damping - attr.sleep_threshold = self.sleep_threshold - attr.settling_threshold = self.settling_threshold - attr.settling_damping = self.settling_damping - attr.min_position_iters = self.min_position_iters - attr.min_velocity_iters = self.min_velocity_iters - return attr - - -@configclass -class ClothPhysicalAttributesCfg: - # material properties - youngs: float = 1e10 - """Young's modulus (higher = stiffer).""" - - poissons: float = 0.3 - """Poisson's ratio.""" - - dynamic_friction: float = 0.5 - """Dynamic friction coefficient.""" - - elasticity_damping: float = 0.0 - """Elasticity damping factor.""" - - thickness: float = 0.001 - """Cloth thickness (m).""" - - bending_stiffness: float = 0.00001 - """Bending stiffness.""" - - bending_damping: float = 0.0 - """Bending damping.""" - - # cloth body properties - enable_kinematic: bool = False - """If True, (partially) kinematic behavior is enabled.""" - - enable_ccd: bool = True - """Enable continuous collision detection (CCD).""" - - enable_self_collision: bool = False - """Enable self-collision handling.""" - - has_gravity: bool = True - """Whether the cloth is affected by gravity.""" - - self_collision_stress_tolerance: float = 0.9 - """Stress tolerance threshold for self-collision constraints.""" - - collision_mesh_simplification: bool = True - """Whether to simplify the collision mesh for self-collision.""" - - vertex_velocity_damping: float = 0.005 - """Per-vertex velocity damping.""" - - mass: float = -1.0 - """Total mass of the cloth. If negative, density is used to compute mass.""" - - density: float = 1.0 - """Material density in kg/m^3.""" - - max_depenetration_velocity: float = 1e6 - """Maximum velocity used to resolve penetrations.""" - - max_velocity: float = 100.0 - """Clamp for linear (or vertex) velocity.""" - - self_collision_filter_distance: float = 0.1 - """Distance threshold for filtering self-collision vertex pairs.""" - - linear_damping: float = 0.05 - """Global linear damping applied to the cloth.""" - - sleep_threshold: float = 0.05 - """Velocity/energy threshold below which the cloth can go to sleep.""" - - settling_threshold: float = 0.1 - """Threshold used to decide convergence/settling state.""" - - settling_damping: float = 10.0 - """Additional damping applied during settling phase.""" - - min_position_iters: int = 4 - """Minimum solver iterations for position correction.""" - - min_velocity_iters: int = 1 - """Minimum solver iterations for velocity updates.""" - - def attr(self) -> ClothBodyAttr: - """Convert to dexsim ClothBodyAttr.""" - attr = ClothBodyAttr() - attr.youngs = self.youngs - attr.poissons = self.poissons - attr.dynamic_friction = self.dynamic_friction - attr.elasticity_damping = self.elasticity_damping - attr.thickness = self.thickness - attr.bending_stiffness = self.bending_stiffness - attr.bending_damping = self.bending_damping - attr.enable_kinematic = self.enable_kinematic - attr.enable_ccd = self.enable_ccd - attr.enable_self_collision = self.enable_self_collision - attr.has_gravity = self.has_gravity - attr.self_collision_stress_tolerance = self.self_collision_stress_tolerance - attr.collision_mesh_simplification = self.collision_mesh_simplification - attr.vertex_velocity_damping = self.vertex_velocity_damping - attr.mass = self.mass - attr.density = self.density - attr.max_depenetration_velocity = self.max_depenetration_velocity - attr.max_velocity = self.max_velocity - attr.self_collision_filter_distance = self.self_collision_filter_distance - attr.linear_damping = self.linear_damping - attr.sleep_threshold = self.sleep_threshold - attr.settling_threshold = self.settling_threshold - attr.settling_damping = self.settling_damping - attr.min_position_iters = self.min_position_iters - attr.min_velocity_iters = self.min_velocity_iters - return attr - - -@configclass -class JointDrivePropertiesCfg: - """Properties to define the drive mechanism of a joint.""" - - drive_type: Literal["force", "acceleration", "none"] = "force" - """Joint drive type to apply. - - If the drive type is "force", then the joint is driven by a force and the acceleration is computed based on the force applied. - If the drive type is "acceleration", then the joint is driven by an acceleration and the force is computed based on the acceleration applied. - If the drive type is "none", then no force will be applied to joint. - """ - - stiffness: Dict[str, float] | float = 1e4 - """Stiffness of the joint drive. - - The unit depends on the joint model: - - * For linear joints, the unit is kg-m/s^2 (N/m). - * For angular joints, the unit is kg-m^2/s^2/rad (N-m/rad). - """ - - damping: Dict[str, float] | float = 1e3 - """Damping of the joint drive. - - The unit depends on the joint model: - - * For linear joints, the unit is kg-m/s (N-s/m). - * For angular joints, the unit is kg-m^2/s/rad (N-m-s/rad). - """ - - max_effort: Dict[str, float] | float = 1e10 - """Maximum effort that can be applied to the joint (in kg-m^2/s^2).""" - - max_velocity: Dict[str, float] | float = 1e10 - """Maximum velocity that the joint can reach (in rad/s or m/s). - - For linear joints, this is the maximum linear velocity with unit m/s. - For angular joints, this is the maximum angular velocity with unit rad/s. - """ - - friction: Dict[str, float] | float = 0.0 - """Friction coefficient of the joint""" - - armature: Dict[str, float] | float = 0.0 - """Joint armature added to joint-space spatial inertia. - - Units depend on the joint model: - - * For prismatic (linear) joints, the unit is mass [kg]. - * For revolute (angular) joints, the unit is mass * scene_length^2 [kg-m^2]. - """ - - @classmethod - def from_dict( - cls, - init_dict: Dict[str, str | float | int | Dict[str, float]], - *, - defaults: JointDrivePropertiesCfg | None = None, - ) -> JointDrivePropertiesCfg: - """Initialize the configuration from a dictionary. - - Args: - init_dict: Joint-drive properties to override. - defaults: Optional base properties whose unspecified values are - preserved. If omitted, the class defaults are used. - - Returns: - Parsed joint-drive properties. - """ - cfg = defaults.copy() if defaults is not None else cls() - for key, value in init_dict.items(): - if hasattr(cfg, key): - setattr(cfg, key, value) - else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) - return cfg - - -@configclass -class ObjectBaseCfg: - """Base configuration for an asset in the simulation. - - This class defines the basic properties of an asset, such as its type, initial state, and collision group. - It is used as a base class for specific asset configurations. - """ - - uid: str | None = None - - init_pos: tuple[float, float, float] = (0.0, 0.0, 0.0) - """Position of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" - - init_rot: tuple[float, float, float] = (0.0, 0.0, 0.0) - """Euler angles (in degree) of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" - - init_local_pose: np.ndarray | None = None - """4x4 transformation matrix of the root in local frame. If specified, it will override init_pos and init_rot.""" - - @classmethod - def from_dict(cls, init_dict: Dict[str, str | float | tuple]) -> ObjectBaseCfg: - """Initialize the configuration from a dictionary.""" - cfg = cls() # Create a new instance of the class (cls) - for key, value in init_dict.items(): - if hasattr(cfg, key): - attr = getattr(cfg, key) - if is_configclass(attr): - setattr( - cfg, key, attr.from_dict(value) - ) # Call from_dict on the attribute - else: - setattr(cfg, key, value) - else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) - - # Automatically infer init_local_pose if not provided - if cfg.init_local_pose is None: - # If only init_pos or init_rot are provided, generate the 4x4 pose matrix - from scipy.spatial.transform import Rotation as R - - T = np.eye(4) - T[:3, 3] = np.array(cfg.init_pos) - T[:3, :3] = R.from_euler("xyz", np.deg2rad(cfg.init_rot)).as_matrix() - cfg.init_local_pose = T - else: - # If only init_local_pose is provided, extract init_pos and init_rot - from scipy.spatial.transform import Rotation as R - - T = np.array(cfg.init_local_pose) - cfg.init_pos = tuple(T[:3, 3]) - cfg.init_rot = tuple(R.from_matrix(T[:3, :3]).as_euler("xyz", degrees=True)) - - return cfg - - -@configclass -class LightCfg(ObjectBaseCfg): - """Configuration for a light asset in the simulation. - - Supports six light types matching the dexsim rendering backend: - - - ``"point"``: Per-environment omnidirectional point light with position - and falloff radius. Created as a batched light (one per environment). - - ``"sun"``: Global directional sun light (infinite distance). Created as - a single scene-level instance. Uses direction only; position is ignored. - Sun-specific fields (``angular_radius``, ``halo_size``, ``halo_falloff``) - are reserved for future backend support. - - ``"direction"``: Global pure directional light at infinite distance. - Created as a single scene-level instance. Direction only; no position. - - ``"spot"``: Per-environment spotlight with position, direction, and - inner/outer cone angles. Created as a batched light. - - ``"rect"``: Per-environment rectangular area light with position, - direction, width, and height. Created as a batched light. - - ``"mesh"``: Per-environment mesh-based emissive light. Requires a - :class:`~dexsim.models.MeshObject` via - :meth:`embodichain.lab.sim.objects.light.Light.set_mesh` - (not tensor-batched). Created as a batched light. - - .. attention:: - The ``angular_radius``, ``halo_size``, and ``halo_falloff`` fields are - reserved for future use. The dexsim Python bindings do not yet expose - setters for these sun-specific properties. - """ - - light_type: Literal["point", "sun", "direction", "spot", "rect", "mesh"] = "point" - """Light type. Supported: ``"point"``, ``"sun"``, ``"direction"``, ``"spot"``, ``"rect"``, ``"mesh"``.""" - - # ------------------------------------------------------------------ - # Universal properties (apply to all light types) - # ------------------------------------------------------------------ - - color: tuple[float, float, float] = (1.0, 1.0, 1.0) - """RGB color of the light source. Defaults to white ``(1.0, 1.0, 1.0)``.""" - - intensity: float = 30.0 - """Intensity of the light source in watts/m^2. Defaults to ``30.0``.""" - - enable_shadow: bool = True - """Whether the light casts shadows. Defaults to ``True``.""" - - # ------------------------------------------------------------------ - # Point light - # ------------------------------------------------------------------ - - radius: float = 10.0 - """Falloff radius for point lights. Only used when ``light_type="point"``. Defaults to ``10.0``.""" - - # ------------------------------------------------------------------ - # Directional properties (sun, direction, spot, rect, mesh) - # ------------------------------------------------------------------ - - direction: tuple[float, float, float] = (0.0, 0.0, -1.0) - """Direction vector for directional, spot, rect, and mesh lights. - Defaults to ``(0.0, 0.0, -1.0)`` (pointing down along -Z).""" - - # ------------------------------------------------------------------ - # Sun light (reserved — Python bindings not yet available) - # ------------------------------------------------------------------ - - angular_radius: float = 0.5 - """Angular radius of the sun disc in degrees. Reserved for future use.""" - - halo_size: float = 10.0 - """Halo size for sun light. Reserved for future use.""" - - halo_falloff: float = 3.0 - """Halo falloff for sun light. Reserved for future use.""" - - # ------------------------------------------------------------------ - # Spot light - # ------------------------------------------------------------------ - - spot_angle_inner: float = 30.0 - """Inner cone angle of the spotlight in degrees. Only used when ``light_type="spot"``. - Defaults to ``30.0``.""" - - spot_angle_outer: float = 45.0 - """Outer cone angle of the spotlight in degrees. Only used when ``light_type="spot"``. - Defaults to ``45.0``.""" - - # ------------------------------------------------------------------ - # Rect light - # ------------------------------------------------------------------ - - rect_width: float = 1.0 - """Width of the rectangular area light. Only used when ``light_type="rect"``. - Defaults to ``1.0``.""" - - rect_height: float = 1.0 - """Height of the rectangular area light. Only used when ``light_type="rect"``. - Defaults to ``1.0``.""" - - # ------------------------------------------------------------------ - # Mesh light - # ------------------------------------------------------------------ - - mesh_path: str = "" - """Asset path for mesh-based emissive lights. Only used when ``light_type="mesh"``. - The actual mesh assignment is done via - :meth:`embodichain.lab.sim.objects.light.Light.set_mesh` which accepts a - :class:`dexsim.models.MeshObject`. This field stores the path for reference.""" - - -@configclass -class RigidObjectCfg(ObjectBaseCfg): - """Configuration for a rigid body asset in the simulation. - - This class extends the base asset configuration to include specific properties for rigid bodies, - such as physical attributes and collision group. - """ - - shape: ShapeCfg = ShapeCfg() - """Shape configuration for the rigid body. """ - - # TODO: supoort basic primitive shapes, such as box, sphere, etc cfg and spawn method. - - attrs: RigidBodyAttributesCfg = RigidBodyAttributesCfg() - - body_type: Literal["dynamic", "kinematic", "static"] = "dynamic" - - max_convex_hull_num: int = MISSING - """The maximum number of convex hulls that will be created for the rigid body. - - .. deprecated:: - Use :attr:`MeshCfg.max_convex_hull_num` instead. This field is kept for - backward compatibility and overrides the shape-level value when explicitly set. - - If set to larger than 1, the rigid body will be decomposed into multiple convex hulls - using the approximate convex decomposition method specified by :attr:`acd_method`. - """ - - acd_method: str = MISSING - """The method used for approximate convex decomposition (ACD) of the mesh. - - .. deprecated:: - Use :attr:`MeshCfg.acd_method` instead. This field is kept for - backward compatibility and overrides the shape-level value when explicitly set. - - ``"visacd"``, ``"coacd"``, and ``"vhacd"`` are supported. Only used when - :attr:`max_convex_hull_num` is set to larger than 1. ``"visacd"`` requires - CUDA support. - """ - - sdf_resolution: int = MISSING - """Resolution for the signed distance field (SDF) of the rigid body. - - .. deprecated:: - Use :attr:`MeshCfg.sdf_resolution` instead. This field is kept for - backward compatibility and overrides the shape-level value when explicitly set. - - The spacing of the uniformly sampled SDF is equal to the largest AABB extent - of the mesh, divided by the resolution. If ``sdf_resolution`` is set to larger - than 0, an SDF will be generated for collision detection. SDF will increase the - accuracy of collision, but also takes more time to initialize and simulate. - """ - - body_scale: tuple | list = (1.0, 1.0, 1.0) - """Scale of the rigid body in the simulation world frame.""" - - use_usd_properties: bool = False - """Whether to use physical properties from USD file instead of config. - - When True: Keep all physical properties (drive, physics attrs, etc.) from USD file. - When False (default): Override USD properties with config values. - Only effective for USD files. - """ - - def to_dexsim_body_type(self) -> ActorType: - """Convert the body type to dexsim ActorType.""" - if self.body_type == "dynamic": - return ActorType.DYNAMIC - elif self.body_type == "kinematic": - return ActorType.KINEMATIC - elif self.body_type == "static": - return ActorType.STATIC - else: - logger.log_error( - f"Invalid body type '{self.body_type}' specified. Must be one of 'dynamic', 'kinematic', or 'static'." - ) - - -@configclass -class SoftObjectCfg(ObjectBaseCfg): - """Configuration for a soft body asset in the simulation. - - This class extends the base asset configuration to include specific properties for soft bodies, - such as physical attributes and collision group. - """ - - voxel_attr: SoftbodyVoxelAttributesCfg = SoftbodyVoxelAttributesCfg() - """Tetra mesh voxelization attributes for the soft body.""" - - physical_attr: SoftbodyPhysicalAttributesCfg = SoftbodyPhysicalAttributesCfg() - """Physical attributes for the soft body.""" - - shape: MeshCfg = MeshCfg() - """Mesh configuration for the soft body.""" - - -@configclass -class ClothObjectCfg(ObjectBaseCfg): - """Configuration for a cloth body asset in the simulation. - - This class extends the base asset configuration to include specific properties for cloth bodies, - such as physical attributes and collision group. - """ - - physical_attr: ClothPhysicalAttributesCfg = ClothPhysicalAttributesCfg() - """Physical attributes for the cloth body.""" - - shape: MeshCfg = MeshCfg() - """Mesh configuration for the cloth body.""" - - -@configclass -class RigidObjectGroupCfg: - """Configuration for a rigid object group asset in the simulation. - - Rigid object groups can be initialized from multiple rigid object configurations specified in a folder. - If `folder_path` is specified, user should provide a RigidObjectCfg in `rigid_objects` as a template configuration for - all objects in the group. - - For example: - ```python - rigid_object_group: RigidObjectGroupCfg( - folder_path="path/to/folder", - max_num=5, - rigid_objects={ - "template_obj": RigidObjectCfg( - shape=MeshCfg( - fpath="", # fpath will be ignored when folder_path is specified - ), - body_type="dynamic", - ) - } - ) - """ - - uid: str | None = None - - rigid_objects: Dict[str, RigidObjectCfg] = MISSING - """Configuration for the rigid objects in the group.""" - - body_type: Literal["dynamic", "kinematic"] = "dynamic" - """Body type for all rigid objects in the group. """ - - folder_path: str | None = None - """Path to the folder containing the rigid object assets. - - This is used to initialize multiple rigid object configurations from a folder. - """ - - max_num: int = 1 - """Maximum number of rigid objects to initialize from the folder. - - This is only used when `folder_path` is specified. - """ - - ext: str = ".obj" - """File extension for the rigid object assets. - - This is only used when `folder_path` is specified. - """ - - @classmethod - def from_dict(cls, init_dict: Dict[str, Any]) -> RigidObjectGroupCfg: - """Initialize the configuration from a dictionary.""" - cfg = cls() - for key, value in init_dict.items(): - if hasattr(cfg, key): - attr = getattr(cfg, key) - if is_configclass(attr): - setattr( - cfg, key, attr.from_dict(value) - ) # Call from_dict on the attribute - elif key == "rigid_objects" and "folder_path" not in init_dict: - rigid_objects_cfg = {} - for obj_name, obj_cfg in value.items(): - rigid_objects_cfg[obj_name] = RigidObjectCfg.from_dict(obj_cfg) - setattr(cfg, key, rigid_objects_cfg) - elif key == "rigid_objects" and "folder_path" in init_dict: - folder_path = init_dict["folder_path"] - max_num = init_dict.get("max_num", 1) - rigid_objects_cfg = {} - if os.path.exists(folder_path) and os.path.isdir(folder_path): - files = os.listdir(folder_path) - files = [f for f in files if f.endswith(cfg.ext)] - # select files up to max_num - n_file = len(files) - select_files = [] - for i in range(max_num): - select_files.append(files[i % n_file]) - - for i, file_name in enumerate(select_files): - file_path = os.path.join(folder_path, file_name) - rigid_obj_cfg: RigidObjectCfg = RigidObjectCfg.from_dict( - list(init_dict["rigid_objects"].values())[0] - ) - rigid_obj_cfg.uid = f"{cfg.uid}_obj_{i}" - rigid_obj_cfg.shape.fpath = file_path - rigid_objects_cfg[rigid_obj_cfg.uid] = rigid_obj_cfg - setattr(cfg, "rigid_objects", rigid_objects_cfg) - else: - logger.log_error( - f"Folder '{folder_path}' does not exist or is not a directory." - ) - else: - setattr(cfg, key, value) - else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) - return cfg - - -@configclass -class RigidConstraintCfg: - """Configuration for a fixed constraint between two RigidObjects. - - The constraint binds rigid_object_a's entity[i] to rigid_object_b's entity[i] - within arena[i] (one constraint per arena). - - Args: - name: Base constraint name. Per-arena names are derived as ``f"{name}"`` - (single env) or ``f"{name}_{i}"`` (multi env). - rigid_object_a_uid: UID of the first RigidObject (must exist in the sim). - rigid_object_b_uid: UID of the second RigidObject (must exist in the sim). - local_frame_a: 4x4 joint frame in object A's local coordinates. - ``None`` -> identity (object A's origin). Accepts a single - ``(4, 4)`` matrix (shared by all envs) or an ``(N, 4, 4)`` array - (one frame per env). Defaults to None. - local_frame_b: 4x4 joint frame in object B's local coordinates. - ``None`` -> the frame is computed per env as ``inv(pose_B) @ pose_A`` - from the objects' current poses, so the constraint welds the objects - at their *current* relative pose (rather than pulling their origins - together). An explicit ``(4, 4)`` or ``(N, 4, 4)`` value is used - verbatim. Defaults to None. - constraint_type: Reserved for future typed constraints (prismatic, - revolute, spherical, d6). Only ``"fixed"`` is supported in v1. - - .. attention:: - Both objects must be :class:`RigidObject` instances and must share the - same number of arenas. - """ - - name: str = MISSING - """Base name of the constraint (per-arena names are derived from this).""" - - rigid_object_a_uid: str = MISSING - """UID of the first RigidObject.""" - - rigid_object_b_uid: str = MISSING - """UID of the second RigidObject.""" - - local_frame_a: np.ndarray | None = None - """Local joint frame on object A. None -> identity (object A's origin).""" - - local_frame_b: np.ndarray | None = None - """Local joint frame on object B. None -> ``inv(pose_B) @ pose_A`` per env - (weld at the objects' current relative pose).""" - - constraint_type: Literal["fixed"] = "fixed" - """Constraint type. Only ``"fixed"`` is supported in v1.""" - - -@configclass -class URDFCfg: - """Standalone configuration class for URDF assembly.""" - - components: Dict[str, Dict[str, str | Dict | np.ndarray]] = field( - default_factory=dict - ) - """Dictionary of robot components to be assembled.""" - - sensors: Dict[str, Dict[str, str | np.ndarray]] = field(default_factory=dict) - """Dictionary of sensors to be attached to the robot.""" - - use_signature_check: bool = True - """Whether to use signature check when merging URDFs.""" - - base_link_name: str = "base_link" - """Name of the base link in the assembled robot.""" - - fpath: str | None = None - """Full output file path for the assembled URDF. If specified, overrides fname and fpath_prefix.""" - - fname: str | None = None - """Name used for output file and directory. If not specified, auto-generated from component names.""" - - fpath_prefix: str = EMBODICHAIN_DEFAULT_DATA_ROOT + "/assembled" - """Output directory prefix for the assembled URDF file.""" - - component_prefix: List[tuple[str, str | None]] = field( - default_factory=lambda: [ - ("chassis", None), - ("legs", None), - ("torso", None), - ("head", None), - ("left_arm", "left_"), - ("right_arm", "right_"), - ("left_hand", "left_"), - ("right_hand", "right_"), - ("arm", None), - ("hand", None), - ] - ) - """Component name prefixes used during URDF assembly. - - Preferred form is a list of ``(component_name, prefix)`` tuples. For - convenience, a mapping ``{component_name: prefix}`` is also accepted when - constructing :class:`URDFCfg` and will be normalized internally. - """ - - name_case: dict[str, str] = field( - default_factory=lambda: { - "joint": "original", - "link": "original", - } - ) - """Case normalization policy applied to joint/link names during URDF assembly. - - Supported values per key are ``"upper"``, ``"lower"`` or ``"original"`` - (legacy alias ``"none"``). The default preserves source URDF casing. - """ - - def __init__( - self, - components: list[dict[str, str | np.ndarray]] | None = None, - sensors: dict[str, dict[str, str | np.ndarray]] | None = None, - fpath: str | None = None, - fname: str | None = None, - fpath_prefix: str = EMBODICHAIN_DEFAULT_DATA_ROOT + "/assembled", - use_signature_check: bool = True, - base_link_name: str = "base_link", - component_prefix: list[tuple[str, str | None]] | None = None, - name_case: dict[str, str] | None = None, - ): - """ - Initialize URDFCfg with optional list of components and output path settings. - - Args: - components (list[dict[str, str | np.ndarray]] | None): List of component configurations. Each dict should contain: - - 'component_type' (str): The type/name of the component (e.g., 'chassis', 'arm', 'hand'). - - 'urdf_path' (str): Path to the component's URDF file. - - 'transform' (np.ndarray | None): 4x4 transformation matrix (optional). - - Additional params can be included as extra keys. - sensors (dict[str, dict[str, str | np.ndarray]] | None): Sensor configurations for the robot. - fpath (str | None): Full output file path for the assembled URDF. If specified, overrides fname and fpath_prefix. - fname (str | None): Name used for output file and directory. If not specified, auto-generated from component names. - fpath_prefix (str): Output directory prefix for the assembled URDF file. - use_signature_check (bool): Whether to use signature check when merging URDFs. - base_link_name (str): Name of the base link in the assembled robot. - component_prefix (list[tuple[str, str | None]] | None): Optional - list of (component_type, prefix) pairs to override default - component name prefixes. - """ - self.components = {} - self.sensors = sensors or {} - self.fpath = fpath - self.use_signature_check = use_signature_check - self.base_link_name = base_link_name - self.fname = fname - self.fpath_prefix = fpath_prefix - - # Initialize component prefixes (patch-style mapping per component type) - if component_prefix is None: - # Use the same default as the dataclass field - self.component_prefix = [ - ("chassis", None), - ("legs", None), - ("torso", None), - ("head", None), - ("left_arm", "left_"), - ("right_arm", "right_"), - ("left_hand", "left_"), - ("right_hand", "right_"), - ("arm", None), - ("hand", None), - ] - elif isinstance(component_prefix, dict): - # Allow dict-style config: {"left_hand": "l_", ...} - self.component_prefix = list(component_prefix.items()) - else: - # Assume caller provided a list of (component_name, prefix) tuples - self.component_prefix = component_prefix - - if name_case is None: - self.name_case = { - "joint": "original", - "link": "original", - } - else: - self.name_case = name_case - - # Auto-add components if provided - if components: - for comp_config in components: - if not isinstance(comp_config, dict): - logger.log_error( - f"Component configuration must be a dict, got {type(comp_config)}" - ) - continue - - # Extract required fields - component_type = comp_config.get("component_type") - urdf_path = comp_config.get("urdf_path") - - if not component_type or not urdf_path: - logger.log_error( - f"Component configuration must contain 'component_type' and 'urdf_path', got {comp_config}" - ) - continue - - # Extract optional fields - transform = comp_config.get("transform", np.eye(4)) - - # Extract additional params (exclude known keys) - params = { - k: v - for k, v in comp_config.items() - if k not in ["component_type", "urdf_path", "transform"] - } - - # Add the component - self.add_component(component_type, urdf_path, transform, **params) - - if sensors is not None: - # Accept both list and dict; serialization round-trips an empty - # dict when no sensors are configured (the field default). - if isinstance(sensors, dict) and not sensors: - self.sensors = [] - elif not isinstance(sensors, (list, dict)): - logger.log_error( - f"sensors must be a list of dicts or a dict, got {type(sensors)}" - ) - self.sensors = [] - elif isinstance(sensors, dict): - # dict keyed by sensor_name -> config - self.sensors = list(sensors.values()) - else: - # Optionally check each sensor dict - valid_sensors = [] - for sensor_config in sensors: - if not isinstance(sensor_config, dict): - logger.log_error( - f"Sensor configuration must be a dict, got {type(sensor_config)}" - ) - continue - sensor_name = sensor_config.get("sensor_name") - if not sensor_name: - logger.log_error( - f"Sensor configuration must contain 'sensor_name', got {sensor_config}" - ) - continue - valid_sensors.append(sensor_config) - self.sensors = valid_sensors - - def set_urdf(self, urdf_path: str) -> "URDFCfg": - """Directly specify a single URDF file for the robot, compatible with the single-URDF robot case. - - Args: - urdf_path (str): Path to the robot's URDF file. - - Returns: - URDFCfg: Returns self to allow method chaining. - """ - self.components.clear() - urdf_file = os.path.splitext(os.path.basename(urdf_path))[0] - self.components[urdf_file] = { - "urdf_path": urdf_path, - "transform": None, - "params": {}, - } - self.fpath = urdf_path - return self - - def add_component( - self, - component_type: str, - urdf_path: str, - transform: np.ndarray | None = None, - **params, - ) -> URDFCfg: - """Add a robot component to the assembly configuration. - - Args: - component_type (str): The type/name of the component. Should be one of SUPPORTED_COMPONENTS - (e.g., 'chassis', 'torso', 'head', 'left_arm', 'right_hand', 'arm', 'hand', etc.). - urdf_path (str): Path to the component's URDF file. - transform (np.ndarray | None): 4x4 transformation matrix for the component in the robot frame (default: None). - **params: Additional keyword parameters for the component (e.g., color, material, etc.). - - Returns: - URDFCfg: Returns self to allow method chaining. - """ - if urdf_path: - if not os.path.exists(urdf_path): - urdf_path_candidate = get_data_path(urdf_path) - if os.path.exists(urdf_path_candidate): - urdf_path = urdf_path_candidate - else: - logger.log_error(f"URDF path '{urdf_path}' does not exist.") - raise FileNotFoundError(f"URDF path '{urdf_path}' does not exist.") - - if transform is None: - transform = np.eye(4) - - self.components[component_type] = { - "urdf_path": urdf_path, - "transform": np.array(transform), - "params": params, - } - - if self.fname: - self.fpath = f"{self.fpath_prefix}/{self.fname}/{self.fname}.urdf" - else: - # Update output_path to use all component urdf file names joined by underscores as directory - if len(self.components) == 1: - # Only one component, use its urdf file name - urdf_file = os.path.splitext(os.path.basename(urdf_path))[0] - name = urdf_file - else: - # Multiple components, join all urdf file names - urdf_files = [ - os.path.splitext(os.path.basename(v["urdf_path"]))[0] - for v in self.components.values() - ] - name = "_".join(urdf_files) - self.fpath = f"{self.fpath_prefix}/{name}/{name}.urdf" - - return self - - def add_sensor(self, sensor_name: str, **sensor_config) -> URDFCfg: - """Add a sensor to the robot configuration. - - Args: - sensor_name (str): The name of the sensor. - **sensor_config: Additional configuration parameters for the sensor. - - Returns: - URDFCfg: Returns self to allow method chaining. - """ - self.sensors.append({"sensor_name": sensor_name, **sensor_config}) - return self - - def assemble_urdf(self) -> str: - """Assemble URDF files for the robot based on the configuration. - - Returns: - str: The path to the resulting (possibly merged) URDF file. - """ - components = list(self.components.items()) - # If there is only one component, return its URDF path directly. - if len(components) == 1: - _, comp_config = components[0] - return comp_config["urdf_path"] - - from embodichain.toolkits.urdf_assembly import URDFAssemblyManager - - # If there are multiple components, merge them into a single URDF file. - manager = URDFAssemblyManager() - manager.base_link_name = self.base_link_name - - if self.component_prefix is None: - self.component_prefix = [ - ("left_arm", "left_"), - ("right_arm", "right_"), - ("left_hand", "left_"), - ("right_hand", "right_"), - ] - if isinstance(self.component_prefix, dict): - self.component_prefix = list(self.component_prefix.items()) - # Forward configured component prefixes to the assembly manager - manager.component_prefix = self.component_prefix - - if self.name_case is not None: - manager.name_case = self.name_case - - for comp_type, comp_config in components: - params = comp_config.get("params", {}) - success = manager.add_component( - comp_type, - comp_config["urdf_path"], - comp_config.get("transform"), - **params, - ) - if not success: - logger.log_error( - f"Failed to add component '{comp_type}' with config: {comp_config}" - ) - - for sensor in self.sensors: - manager.attach_sensor( - sensor_name=sensor.get("sensor_name"), - sensor_source=sensor.get("sensor_source"), - parent_component=sensor.get("parent_component"), - parent_link=sensor.get("parent_link"), - sensor_type=sensor.get("sensor_type"), - **{ - k: v - for k, v in sensor.items() - if k - not in [ - "sensor_name", - "sensor_source", - "parent_component", - "parent_link", - "sensor_type", - ] - }, - ) - - try: - # Merge all added components into a single URDF file at the specified output path. - merged_urdf_xml = manager.merge_urdfs(self.fpath, self.use_signature_check) - except Exception as e: - logger.log_error(f"URDF merge failed: {e}") - - return self.fpath - - @classmethod - def from_dict(cls, init_dict: Dict) -> "URDFCfg": - if isinstance(init_dict, cls): - return init_dict - components = init_dict.get("components", None) - if isinstance(components, dict): - components = [{"component_type": k, **v} for k, v in components.items()] - sensors = init_dict.get("sensors", None) - fpath = init_dict.get("fpath", None) - use_signature_check = init_dict.get("use_signature_check", True) - base_link_name = init_dict.get("base_link_name", "base_link") - component_prefix = init_dict.get("component_prefix", None) - name_case = init_dict.get("name_case", None) - return cls( - components=components, - sensors=sensors, - fpath=fpath, - use_signature_check=use_signature_check, - base_link_name=base_link_name, - component_prefix=component_prefix, - name_case=name_case, - ) - - -@configclass -class ArticulationCfg(ObjectBaseCfg): - """Configuration for an articulation asset in the simulation. - - This class extends the base asset configuration to include specific properties for articulations, - such as joint drive properties, physical attributes. - """ - - fpath: str = None - """Path to the articulation asset file.""" - - drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg(drive_type="none") - """Properties to define the drive mechanism of a joint.""" - - body_scale: tuple | list = (1.0, 1.0, 1.0) - """Scale of the articulation in the simulation world frame.""" - - attrs: RigidBodyAttributesCfg = RigidBodyAttributesCfg() - """Physical attributes for all links. We use default mass from the USD/URDF file if available. - The mass and density in attrs will only be used if specified. - """ - - link_attrs: dict[str, LinkPhysicsOverrideCfg] | None = None - """Named per-link physics override groups keyed by regex on link names. - - Each group applies :attr:`LinkPhysicsOverrideCfg.attrs` on top of :attr:`attrs` for - matched links only. A link must not match more than one group. - """ - - fix_base: bool = True - """Whether to fix the base of the articulation. - - Set to True for articulations that should not move, such as a fixed base robot arm or a door. - Set to False for articulations that should move freely, such as a mobile robot or a humanoid robot. - """ - - disable_self_collision: bool = True - """Whether to enable or disable self-collisions.""" - - enable_gravity: bool = True - """Whether gravity is enabled for the articulation. - - This runtime flag is applied regardless of :attr:`use_usd_properties`. - """ - - init_qpos: torch.Tensor | np.ndarray | Sequence[float] = None - """Initial joint positions of the articulation. - - If None, the joint positions will be set to zero. - If provided, it should be a array of shape (num_joints,). - """ - - qpos_limits: ( - torch.Tensor | np.ndarray | Sequence[float] | Dict[str, List[float]] | None - ) = None - """Override joint position limits of the articulation. - - If None, the joint position limits from the asset file (URDF/USD) are used. - If provided as a tensor/array of shape (num_joints, 2), it is applied to all - joints in the order of ``joint_names``. - If provided as a dictionary, keys are joint names or regular expressions and - values are ``[min, max]`` limits. - - This field replaces the asset limits for the articulation and can be used to - either tighten or expand the allowed range. - """ - - sleep_threshold: float = 0.005 - """Energy below which the articulation may go to sleep. Range: [0, max_float32]""" - - min_position_iters: int = 4 - """Number of position iterations the solver should perform for this articulation. Range: [1,255].""" - - min_velocity_iters: int = 1 - """Number of velocity iterations the solver should perform for this articulation. Range: [0,255].""" - - build_pk_chain: bool = True - """Whether to build pytorch-kinematics chain for forward kinematics and jacobian computation.""" - - compute_uv: bool = False - """Whether to compute the UV mapping for the articulation link. - - Currently, the uv mapping is computed for each link with projection uv mapping method. - """ - - use_usd_properties: bool = False - """Whether to use physical properties from USD file instead of config. - - When True: Keep all physical properties (drive, physics attrs, etc.) from USD file. - When False (default): Override USD properties with config values (URDF behavior). - Only effective for USD files, ignored for URDF files. - """ - - @classmethod - def from_dict( - cls, init_dict: Dict[str, str | float | tuple | dict] - ) -> ArticulationCfg: - """Initialize the configuration from a dictionary.""" - cfg = cls() - for key, value in init_dict.items(): - if key == "link_attrs" and isinstance(value, dict): - cfg.link_attrs = link_attrs_from_dict(value) - elif hasattr(cfg, key): - attr = getattr(cfg, key) - if isinstance(attr, JointDrivePropertiesCfg) and isinstance( - value, dict - ): - setattr( - cfg, - key, - JointDrivePropertiesCfg.from_dict(value, defaults=attr), - ) - elif is_configclass(attr): - setattr(cfg, key, attr.from_dict(value)) - else: - setattr(cfg, key, value) - else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) - - if cfg.init_local_pose is None: - from scipy.spatial.transform import Rotation as R - - T = np.eye(4) - T[:3, 3] = np.array(cfg.init_pos) - T[:3, :3] = R.from_euler("xyz", np.deg2rad(cfg.init_rot)).as_matrix() - cfg.init_local_pose = T - else: - from scipy.spatial.transform import Rotation as R - - cfg.init_pos = tuple(cfg.init_local_pose[:3, 3]) - cfg.init_rot = tuple( - R.from_matrix(cfg.init_local_pose[:3, :3]).as_euler("xyz", degrees=True) - ) - - return cfg - - -@configclass -class RobotCfg(ArticulationCfg): - from embodichain.lab.sim.motion.solvers import SolverCfg - - """Configuration for a robot asset in the simulation. - """ - - drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg(drive_type="force") - """Properties to define the drive mechanism of a joint.""" - - control_parts: Dict[str, List[str]] | None = None - """Control parts is the mapping from part name to joint names. - - For example, {'left_arm': ['joint1', 'joint2'], 'right_arm': ['joint3', 'joint4']} - If no control part is specified, the robot will use all joints as a single control part. - - Note: - - `control_parts` can be used without `solver_cfg`. If `solver_cfg` is a - dictionary, its keys must correspond to control-part names. - - The joint names in the control parts support regular expressions, e.g., 'joint[1-6]'. - After initialization of robot, the names will be expanded to a list of full joint names. - - `Robot` is a derived class of `Articulation`, with control parts support. So the `drive_pros` - in `ArticulationCfg` can use control part as key to specify the corresponding joint drive properties, - which will be overridden if these joint names are already specified. - """ - - urdf_cfg: URDFCfg | None = None - """URDF assembly configuration which allows for assembling a robot from multiple URDF components. - """ - - # TODO: how to support one solver for multiple parts? - solver_cfg: SolverCfg | Dict[str, SolverCfg] | None = None - """Solver is used to compute forward and inverse kinematics for the robot. - """ - - workspace_cfg: Dict[str, RobotWorkspaceCfg] | None = None - """Runtime workspace cache configuration keyed by control-part name.""" - - @classmethod - def from_dict(cls, init_dict: Dict[str, str | float | tuple]) -> RobotCfg: - """Initialize the configuration from a dictionary.""" - if isinstance(init_dict, cls): - return init_dict - - import importlib - - solver_module = importlib.import_module("embodichain.lab.sim.motion.solvers") - - cfg = cls() # Create a new instance of the class (cls) - for key, value in init_dict.items(): - if key == "link_attrs" and isinstance(value, dict): - cfg.link_attrs = link_attrs_from_dict(value) - elif hasattr(cfg, key): - attr = getattr(cfg, key) - if key == "urdf_cfg": - from embodichain.lab.sim.cfg import URDFCfg - - setattr(cfg, key, URDFCfg.from_dict(value)) - elif key == "workspace_cfg" and isinstance(value, dict): - setattr( - cfg, - key, - { - part: ( - part_cfg - if isinstance(part_cfg, RobotWorkspaceCfg) - else RobotWorkspaceCfg(**part_cfg) - ) - for part, part_cfg in value.items() - }, - ) - elif key == "fpath": - setattr(cfg, key, get_data_path(value)) - elif isinstance(attr, JointDrivePropertiesCfg) and isinstance( - value, dict - ): - setattr( - cfg, - key, - JointDrivePropertiesCfg.from_dict(value, defaults=attr), - ) - elif is_configclass(attr): - setattr( - cfg, key, attr.from_dict(value) - ) # Call from_dict on the attribute - elif isinstance(value, dict) and "class_type" in value: - setattr( - cfg, - key, - getattr(solver_module, f"{value['class_type']}Cfg").from_dict( - value - ), - ) - elif isinstance(value, dict) and key_in_nested_dict( - value, "class_type" - ): - setattr( - cfg, - key, - { - k: getattr( - solver_module, f"{v['class_type']}Cfg" - ).from_dict(v) - for k, v in value.items() - }, - ) - - else: - setattr(cfg, key, value) - else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) - return cfg - - def _build_defaults(self, init_dict: dict | None = None) -> None: - """Populate default config fields from ``init_dict``. - - Subclasses override this to read variant/version fields from - ``init_dict``, set them on ``self``, and populate ``urdf_cfg``, - ``control_parts``, ``solver_cfg``, ``drive_pros`` and ``attrs``. - The base implementation is a no-op. - - .. attention:: - Do NOT call :func:`merge_robot_cfg` from here -- the subclass - ``from_dict`` calls this hook first, then ``merge_robot_cfg``. - Calling ``merge_robot_cfg`` here would recurse, because - ``merge_robot_cfg`` itself calls ``RobotCfg.from_dict``. - - Args: - init_dict: The raw override dict passed to ``from_dict``. - """ - return None - - def to_dict(self): - """Serialize config to a plain dict (enums, numpy, nested configclass).""" - - def serialize(obj, _visited=None): - if _visited is None: - _visited = set() - if isinstance(obj, enum.Enum): - return obj.value - if isinstance(obj, (dict, object)) and not isinstance( - obj, (str, int, float, bool, type(None)) - ): - obj_id = id(obj) - if obj_id in _visited: - return None - _visited.add(obj_id) - - if isinstance(obj, np.ndarray): - return obj.tolist() - if isinstance(obj, dict): - return { - (k.value if isinstance(k, enum.Enum) else str(k)): serialize( - v, _visited - ) - for k, v in obj.items() - } - if isinstance(obj, (list, tuple)): - return [serialize(v, _visited) for v in obj] - if hasattr(obj, "to_dict") and obj is not self: - return serialize(obj.to_dict(), _visited) - if hasattr(obj, "__dict__"): - return { - k: serialize(v, _visited) - for k, v in obj.__dict__.items() - if v is not None - } - return obj - - return serialize(self) - - def to_string(self): - """Return config as a JSON string.""" - return json.dumps(self.to_dict(), indent=2) - - def save_to_file(self, filepath): - """Save config to a local file as JSON.""" - with open(filepath, "w") as f: - f.write(self.to_string()) - - def build_pk_serial_chain( - self, device: torch.device = torch.device("cpu"), **kwargs - ) -> Dict[str, "pk.SerialChain"]: - """Build the serial chain from the URDF file. - - Note: - This method is usually used in imitation dataset saving (compute eef pose from qpos using FK) - and model training (provide a differentiable FK layer or loss computation). - - Args: - device (torch.device): The device to which the chain will be moved. Defaults to CPU. - **kwargs: Additional arguments for building the serial chain. - - Returns: - Dict[str, pk.SerialChain]: The serial chain of the robot for specified control part. - """ - return {} diff --git a/embodichain/lab/sim/cfg/__init__.py b/embodichain/lab/sim/cfg/__init__.py new file mode 100644 index 000000000..7bbdd99c4 --- /dev/null +++ b/embodichain/lab/sim/cfg/__init__.py @@ -0,0 +1,136 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Public simulation-configuration facade. + +The implementation is split by domain while this package preserves the +historical ``embodichain.lab.sim.cfg`` import surface. +""" + +from __future__ import annotations + +from typing import Literal + +from embodichain.data import get_data_path + +from ..shapes import MeshCfg, MeshCollisionApproximation, MeshCollisionCfg, ShapeCfg +from ..motion.workspace.cfg import RobotWorkspaceCfg +from .articulation import ( + ArticulationCfg, + ArticulationRootPropertiesCfg, + JointDrivePropertiesCfg, + LinkPhysicsOverrideCfg, + NewtonJointDrivePropertiesCfg, + _normalize_joint_target_mode, + _raise_removed_articulation_cfg_fields, + link_attrs_from_dict, +) +from .asset import AssetPhysicsMode, ObjectBaseCfg, _resolve_asset_physics_mode +from .deformable import ( + SurfaceElementPropertiesCfg, + VolumeDeformableMeshingCfg, + VolumeDeformablePhysicsCfg, + SurfaceDeformablePhysicsCfg, + DeformableObjectCfg, + SurfaceDeformableObjectCfg, + VolumeDeformableObjectCfg, +) +from .rigid import ( + CollisionPropertiesCfg, + DefaultCollisionPropertiesCfg, + DefaultRigidBodyPropertiesCfg, + MassPropertiesCfg, + NewtonCollisionPropertiesCfg, + NewtonRigidBodyMaterialCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, +) +from .rigid_object import RigidObjectCfg, RigidObjectGroupCfg +from .scene import LightCfg, RigidConstraintCfg +from .simulation import ( + DefaultPhysicsCfg, + DLSSCfg, + GPUMemoryCfg, + NewtonCollisionPipelineCfg, + NewtonPhysicsCfg, + PhysicsBackendCfg, + RenderCfg, + physics_backend_from_cfg, + physics_cfg_for_backend, + validate_physics_cfg, +) +from .urdf import URDFCfg +from .viewer import MarkerCfg, WindowCameraPoseCfg, WindowRecordCfg + +# The renderer selection code intentionally mutates this package-level value. +DEFAULT_RENDERER: Literal["auto", "hybrid", "fast-rt", "rt"] = "auto" + +# Robot imports are kept last because SolverCfg discovery imports simulation +# modules that themselves rely on the public facade above. +from .robot import RobotCfg, RobotPresetCfg # noqa: E402 + +__all__ = [ + "DEFAULT_RENDERER", + "AssetPhysicsMode", + "DLSSCfg", + "RenderCfg", + "GPUMemoryCfg", + "PhysicsBackendCfg", + "DefaultPhysicsCfg", + "NewtonCollisionPipelineCfg", + "NewtonPhysicsCfg", + "physics_cfg_for_backend", + "physics_backend_from_cfg", + "validate_physics_cfg", + "MarkerCfg", + "WindowRecordCfg", + "WindowCameraPoseCfg", + "ShapeCfg", + "MeshCfg", + "MeshCollisionApproximation", + "MeshCollisionCfg", + "MassPropertiesCfg", + "DefaultRigidBodyPropertiesCfg", + "CollisionPropertiesCfg", + "DefaultCollisionPropertiesCfg", + "NewtonCollisionPropertiesCfg", + "RigidBodyMaterialCfg", + "NewtonRigidBodyMaterialCfg", + "RigidBodyPhysicsCfg", + "ObjectBaseCfg", + "LightCfg", + "RigidObjectCfg", + "DeformableObjectCfg", + "VolumeDeformableObjectCfg", + "SurfaceDeformableObjectCfg", + "RigidObjectGroupCfg", + "RigidConstraintCfg", + "SurfaceElementPropertiesCfg", + "VolumeDeformableMeshingCfg", + "VolumeDeformablePhysicsCfg", + "SurfaceDeformablePhysicsCfg", + "ArticulationRootPropertiesCfg", + "LinkPhysicsOverrideCfg", + "link_attrs_from_dict", + "JointDrivePropertiesCfg", + "NewtonJointDrivePropertiesCfg", + "ArticulationCfg", + "URDFCfg", + "RobotCfg", + "RobotPresetCfg", + "RobotWorkspaceCfg", + "get_data_path", +] diff --git a/embodichain/lab/sim/cfg/articulation.py b/embodichain/lab/sim/cfg/articulation.py new file mode 100644 index 000000000..cd54c6440 --- /dev/null +++ b/embodichain/lab/sim/cfg/articulation.py @@ -0,0 +1,524 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Articulation-root, per-link, joint, and articulation configuration.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import MISSING, fields +import numbers +from typing import Any, Dict, List, Literal, Sequence + +import numpy as np +import torch + +from embodichain.utils import configclass, is_configclass, logger + +from .asset import AssetPhysicsMode, ObjectBaseCfg, _resolve_asset_physics_mode +from .rigid import ( + RigidBodyPhysicsCfg, + _rigid_body_physics_from_dict, +) + + +def _normalize_joint_target_mode(value: object) -> int: + """Normalize a portable joint target mode to its backend integer value.""" + if isinstance(value, str): + normalized = value.replace("-", "_").lower() + modes = { + "none": 0, + "position": 1, + "velocity": 2, + "position_velocity": 3, + "effort": 4, + } + if normalized not in modes: + raise ValueError( + f"Unsupported joint target mode {value!r}; expected one of " + f"{tuple(modes)}." + ) + return modes[normalized] + if isinstance(value, numbers.Integral) and not isinstance(value, bool): + mode = int(value) + if 0 <= mode <= 4: + return mode + raise ValueError("Joint target-mode integers must be in [0, 4].") + raise TypeError("Joint target mode must be a string or an integer in [0, 4].") + + +@configclass +class ArticulationRootPropertiesCfg: + """Articulation-root properties shared by robot definitions. + + ``fixed_base`` and ``self_collision_enabled`` are consumed by both + backends. ``sleep_threshold`` and the solver-iteration fields are supported + only by the Default backend and are ignored by Newton. By default, the + articulation root is fixed and self-collision is disabled. Explicit + ``None`` values preserve the source value or backend/import default. + """ + + fixed_base: bool | None = True + """Whether the articulation root is rigidly fixed to the world frame. + + Set to ``None`` to preserve the source value or backend/import default. + """ + + self_collision_enabled: bool | None = False + """Whether non-filtered link pairs in the articulation may self-collide. + + Newton may still apply source-authored or Spawn-owned filtering to adjacent + parent-child bodies. Set to ``None`` to preserve the source value or + backend/import default. + """ + + sleep_threshold: float | None = None + """Default-only articulation sleep threshold; Newton ignores this field.""" + + min_position_iters: int | None = None + """Default-only minimum root position-solver iterations (1 to 255).""" + + min_velocity_iters: int | None = None + """Default-only minimum root velocity-solver iterations (0 to 255).""" + + def __post_init__(self) -> None: + """Require the two values consumed by the atomic Default setter.""" + if (self.min_position_iters is None) != (self.min_velocity_iters is None): + raise ValueError( + "Articulation-root min_position_iters and min_velocity_iters " + "must be configured together." + ) + + @classmethod + def from_dict( + cls, + init_dict: Mapping[str, Any], + ) -> ArticulationRootPropertiesCfg: + """Parse articulation-root properties without a backend subtype.""" + return cls(**dict(init_dict)) + + +_REMOVED_ARTICULATION_CFG_FIELDS = { + "fix_base": "root_props.fixed_base", + "disable_self_collision": ( + "root_props.self_collision_enabled (invert the old boolean)" + ), + "sleep_threshold": "root_props.sleep_threshold", + "min_position_iters": "root_props.min_position_iters", + "min_velocity_iters": "root_props.min_velocity_iters", + "articulation_props": "root_props", + "drive_pros": "joint_drive_props", + "joint_props": "joint_drive_props", +} + + +def _raise_removed_articulation_cfg_fields(init_dict: Mapping[str, Any]) -> None: + """Reject removed flat articulation fields with actionable replacements.""" + removed = _REMOVED_ARTICULATION_CFG_FIELDS.keys() & init_dict.keys() + if not removed: + return + replacements = ", ".join( + f"{name} -> {_REMOVED_ARTICULATION_CFG_FIELDS[name]}" + for name in sorted(removed) + ) + raise ValueError(f"Removed ArticulationCfg fields: {replacements}.") + + +@configclass +class LinkPhysicsOverrideCfg: + """Partial physics overlay for a selected set of articulation links. + + Regex/control-group resolution happens before Spawn updates exact source + link names. A link may match only one override group. + """ + + link_names_expr: list[str] = MISSING + """Regular expressions matched against complete source link names.""" + + attrs: RigidBodyPhysicsCfg = RigidBodyPhysicsCfg() + """Partial grouped overlay for the selected links. + + Configure source-inertia recomputation through + :attr:`RigidBodyPhysicsCfg.mass_props`. + """ + + @classmethod + def from_dict(cls, init_dict: Dict[str, Any]) -> LinkPhysicsOverrideCfg: + """Initialize the configuration from a dictionary.""" + if "replace_inertial" in init_dict: + raise ValueError( + "LinkPhysicsOverrideCfg.replace_inertial was removed; use " + "attrs.mass_props.recompute_inertia instead." + ) + cfg = cls() + for key, value in init_dict.items(): + if key == "attrs" and isinstance(value, dict): + setattr(cfg, key, _rigid_body_physics_from_dict(value)) + elif hasattr(cfg, key): + setattr(cfg, key, value) + else: + logger.log_warning( + f"Key '{key}' not found in {cfg.__class__.__name__}." + ) + return cfg + + +def link_attrs_from_dict( + value: dict[str, Any], +) -> dict[str, LinkPhysicsOverrideCfg]: + """Parse a ``link_attrs`` mapping from YAML/JSON-style dicts.""" + link_attrs: dict[str, LinkPhysicsOverrideCfg] = {} + for group_name, group_cfg in value.items(): + if isinstance(group_cfg, LinkPhysicsOverrideCfg): + link_attrs[group_name] = group_cfg + elif isinstance(group_cfg, dict): + link_attrs[group_name] = LinkPhysicsOverrideCfg.from_dict(group_cfg) + else: + raise TypeError( + f"link_attrs['{group_name}'] must be a dict or " + f"LinkPhysicsOverrideCfg, got {type(group_cfg)}." + ) + return link_attrs + + +@configclass +class JointDrivePropertiesCfg: + """Portable joint-drive and joint-dynamics properties. + + A scalar applies to every resolved joint. A dictionary maps exact joint + names, full-match regular expressions, or robot control-part names to + values; exact/regex rules override broader control-part rules. ``None`` + preserves source/backend ownership of a field. + + ``drive_type`` retains the Default drive response (force, acceleration, or + disabled), while ``target_mode`` selects the commanded target components. + Spawn resolves the two concepts before lowering them to the Default drive + descriptor and Newton ``JointDofConfig``. + + Effort and velocity limits, friction, and armature share the same matching + rules and descriptor compilation boundary as the actuator target and gains. + + Newton stores all fields in the model, but individual solvers may ignore + limits, friction, armature, or target modes; consult the `Newton solver + feature matrix + `_. + """ + + drive_type: Literal["force", "acceleration", "none"] | None = None + """Joint drive type to apply. + + On the Default backend, ``"force"`` applies a force/torque drive, + ``"acceleration"`` applies a mass-independent acceleration drive, and + ``"none"`` disables the drive. Newton has no acceleration-drive + equivalent. Unless :attr:`target_mode` is explicit, ``"force"`` and + ``"acceleration"`` select ``"position_velocity"`` while ``"none"`` + selects ``"none"``. + """ + + target_mode: ( + Literal[ + "none", + "position", + "velocity", + "position_velocity", + "effort", + ] + | Dict[ + str, + Literal[ + "none", + "position", + "velocity", + "position_velocity", + "effort", + ] + | int, + ] + | int + | None + ) = None + """Portable actuator target mode, as a scalar or joint-rule mapping. + + Accepted names and integer values are ``"none"``/``0`` (passive), + ``"position"``/``1``, ``"velocity"``/``2``, + ``"position_velocity"``/``3``, and ``"effort"``/``4``. Default emulates + these modes through its drive mode and effective gains. Newton authors the + corresponding ``JointTargetMode``; solvers without native target-mode + support use deterministic gain-based fallbacks where possible. + """ + + stiffness: Dict[str, float] | float | None = None + """Proportional position gain of the joint drive. + + The unit depends on the joint model: + + * For linear joints, the unit is kg-m/s^2 (N/m). + * For angular joints, the unit is kg-m^2/s^2/rad (N-m/rad). + """ + + damping: Dict[str, float] | float | None = None + """Derivative velocity gain of the joint drive. + + The unit depends on the joint model: + + * For linear joints, the unit is kg-m/s (N-s/m). + * For angular joints, the unit is kg-m^2/s/rad (N-m-s/rad). + """ + + max_effort: Dict[str, float] | float | None = None + """Maximum drive effort [N for prismatic, N*m for revolute joints]. + + The value is authored for both backends, but the selected Newton solver may + not enforce it. + """ + + max_velocity: Dict[str, float] | float | None = None + """Maximum joint speed [m/s for prismatic, rad/s for revolute joints]. + + The value is authored for both backends, but support is solver-dependent in + Newton. + """ + + friction: Dict[str, float] | float | None = None + """Passive friction value applied along the joint degree of freedom. + + Interpretation and enforcement are backend/solver-dependent. + """ + + armature: Dict[str, float] | float | None = None + """Artificial inertia added to the joint-space diagonal. + + Units depend on the joint model: + + * For prismatic (linear) joints, the unit is mass [kg]. + * For revolute (angular) joints, the unit is mass * scene_length^2 [kg-m^2]. + + Armature changes the physical model and should normally reflect actuator or + gearbox inertia. Newton solver support varies. + """ + + def _resolve_modes(self) -> tuple[object, str | None]: + """Resolve the target default implied by the original drive type.""" + target_mode = self.target_mode + drive_type = self.drive_type + if drive_type not in {None, "force", "acceleration", "none"}: + raise ValueError(f"Unsupported joint drive type {drive_type!r}.") + if target_mode is None: + target_mode = { + None: None, + "force": "position_velocity", + "acceleration": "position_velocity", + "none": "none", + }[drive_type] + return target_mode, drive_type + + @classmethod + def from_dict( + cls, + init_dict: Dict[str, Any], + *, + defaults: JointDrivePropertiesCfg | None = None, + ) -> JointDrivePropertiesCfg: + """Initialize the configuration from a dictionary. + + Args: + init_dict: Joint-drive properties to override. + defaults: Optional base properties whose unspecified values are + preserved. If omitted, the class defaults are used. + + Returns: + Parsed joint-drive properties. + """ + data = dict(init_dict) + backend = str(data.pop("backend", "common")).replace("-", "_").lower() + wants_newton = backend == "newton" + if backend not in {"common", "default", "newton"}: + raise ValueError( + "joint_drive_props.backend must be 'common', 'default', or 'newton', " + f"got {backend!r}." + ) + if wants_newton and not isinstance(defaults, NewtonJointDrivePropertiesCfg): + cfg = NewtonJointDrivePropertiesCfg() + if defaults is not None: + for item in fields(JointDrivePropertiesCfg): + setattr(cfg, item.name, getattr(defaults, item.name)) + else: + cfg = defaults.copy() if defaults is not None else cls() + for key, value in data.items(): + if hasattr(cfg, key): + setattr(cfg, key, value) + else: + logger.log_warning( + f"Key '{key}' not found in {cfg.__class__.__name__}." + ) + return cfg + + def to_dict(self) -> dict[str, Any]: + """Serialize joint properties with their backend subtype.""" + data = {item.name: getattr(self, item.name) for item in fields(self)} + if isinstance(self, NewtonJointDrivePropertiesCfg): + data["backend"] = "newton" + return data + + +@configclass +class NewtonJointDrivePropertiesCfg(JointDrivePropertiesCfg): + """Compatibility subtype for serialized Newton joint-drive configs. + + ``target_mode`` is now portable and lives on + :class:`JointDrivePropertiesCfg`. The subtype remains so existing + ``backend="newton"`` dictionaries and round trips retain their type; new + robot definitions should use the common class. + """ + + +@configclass +class ArticulationCfg(ObjectBaseCfg): + """Configuration for an articulation asset in the simulation. + + This class extends the base asset configuration to include specific properties for articulations, + such as joint drive properties, physical attributes. + """ + + fpath: str = None + """Path to the articulation asset file.""" + + body_scale: tuple | list = (1.0, 1.0, 1.0) + """Scale of the articulation in the simulation world frame.""" + + compute_uv: bool = False + """Whether to compute the UV mapping for the articulation link. + + Currently, the uv mapping is computed for each link with projection uv mapping method. + """ + + asset_physics_mode: AssetPhysicsMode = "preserve" + """How source-authored articulation physics is handled. + + ``"preserve"`` keeps link, joint-drive, and joint-limit properties from + either USD or URDF. ``"overlay"`` applies only explicitly configured + values after the source has been resolved. + + Import policy such as root fixation and body scale remains controlled by + :attr:`root_props` and :attr:`body_scale`. + """ + + attrs: RigidBodyPhysicsCfg = RigidBodyPhysicsCfg() + """Physical attributes for all links. We use default mass from the USD/URDF file if available. + The mass and density in attrs will only be used if specified. + """ + + link_attrs: dict[str, LinkPhysicsOverrideCfg] | None = None + """Named per-link physics override groups keyed by regex on link names. + + Each group applies :attr:`LinkPhysicsOverrideCfg.attrs` on top of :attr:`attrs` for + matched links only. A link must not match more than one group. + """ + + root_props: ArticulationRootPropertiesCfg = ArticulationRootPropertiesCfg() + """Grouped articulation-root properties. + + Fixed-base and self-collision intent is portable. Root sleep and solver + iterations are Default-only fields and are ignored by Newton. The portable + fields default to a fixed base with self-collision disabled. Set either + field to ``None`` to preserve an authored USD/backend value; URDF imports + then use the established fixed-base, self-collision-off defaults. + """ + + joint_drive_props: JointDrivePropertiesCfg | None = None + """Optional joint-drive and joint-dynamics overrides. + + ``None`` preserves source drive properties. Individual ``None`` fields in + a provided config also preserve the corresponding source values. + """ + + init_qpos: torch.Tensor | np.ndarray | Sequence[float] = None + """Initial joint positions of the articulation. + + If None, the joint positions will be set to zero. + If provided, it should be an array of shape ``(num_dofs,)``. + """ + + qpos_limits: ( + torch.Tensor + | np.ndarray + | Sequence[Sequence[float]] + | Dict[str, List[float]] + | None + ) = None + """Override joint position limits of the articulation. + + If None, the joint position limits from the asset file (URDF/USD) are used. + If provided as a tensor/array of shape ``(num_dofs, 2)``, it is applied in + flattened source-resolved DOF order before the backend model is built. + If provided as a dictionary, keys are joint names or regular expressions and + values are ``[min, max]`` limits. + + This field replaces the asset limits for the articulation and can be used to + either tighten or expand the allowed range. + """ + + build_pk_chain: bool = True + """Whether to build pytorch-kinematics chain for forward kinematics and jacobian computation.""" + + def resolve_asset_physics_mode(self) -> AssetPhysicsMode: + """Return the effective file-backed physics policy.""" + return _resolve_asset_physics_mode(self.asset_physics_mode) + + @classmethod + def from_dict( + cls, init_dict: Dict[str, str | float | tuple | dict] + ) -> ArticulationCfg: + """Initialize the configuration from a dictionary.""" + _raise_removed_articulation_cfg_fields(init_dict) + cfg = cls() + for key, value in init_dict.items(): + if key == "link_attrs" and isinstance(value, dict): + cfg.link_attrs = link_attrs_from_dict(value) + elif key == "attrs" and isinstance(value, Mapping): + cfg.attrs = _rigid_body_physics_from_dict(value) + elif key == "joint_drive_props" and isinstance(value, Mapping): + cfg.joint_drive_props = JointDrivePropertiesCfg.from_dict( + dict(value), + defaults=cfg.joint_drive_props, + ) + elif hasattr(cfg, key): + attr = getattr(cfg, key) + if is_configclass(attr): + setattr(cfg, key, attr.from_dict(value)) + else: + setattr(cfg, key, value) + else: + logger.log_warning( + f"Key '{key}' not found in {cfg.__class__.__name__}." + ) + + if cfg.init_local_pose is None: + from scipy.spatial.transform import Rotation as R + + T = np.eye(4) + T[:3, 3] = np.array(cfg.init_pos) + T[:3, :3] = R.from_euler("xyz", np.deg2rad(cfg.init_rot)).as_matrix() + cfg.init_local_pose = T + else: + from scipy.spatial.transform import Rotation as R + + cfg.init_pos = tuple(cfg.init_local_pose[:3, 3]) + cfg.init_rot = tuple( + R.from_matrix(cfg.init_local_pose[:3, :3]).as_euler("xyz", degrees=True) + ) + + return cfg diff --git a/embodichain/lab/sim/cfg/asset.py b/embodichain/lab/sim/cfg/asset.py new file mode 100644 index 000000000..36b77bf67 --- /dev/null +++ b/embodichain/lab/sim/cfg/asset.py @@ -0,0 +1,103 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Base asset configuration and file-backed physics policy.""" + +from __future__ import annotations + +from collections.abc import Mapping +from typing import Dict, Literal + +import numpy as np + +from embodichain.utils import configclass, is_configclass, logger + +AssetPhysicsMode = Literal["preserve", "overlay"] +"""Policy for applying EmbodiChain physics to a file-backed asset.""" + + +def _resolve_asset_physics_mode( + mode: AssetPhysicsMode, +) -> AssetPhysicsMode: + """Validate and return a source-agnostic asset-physics policy.""" + if mode not in ("preserve", "overlay"): + raise ValueError( + f"asset_physics_mode must be 'preserve' or 'overlay', got {mode!r}." + ) + return mode + + +@configclass +class ObjectBaseCfg: + """Base configuration for an asset in the simulation. + + This class defines the basic properties of an asset, such as its type, initial state, and collision group. + It is used as a base class for specific asset configurations. + """ + + uid: str | None = None + + init_pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Position of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + + init_rot: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Euler angles (in degree) of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + + init_local_pose: np.ndarray | None = None + """4x4 transformation matrix of the root in local frame. If specified, it will override init_pos and init_rot.""" + + @classmethod + def from_dict(cls, init_dict: Dict[str, str | float | tuple]) -> ObjectBaseCfg: + """Initialize the configuration from a dictionary.""" + cfg = cls() # Create a new instance of the class (cls) + for key, value in init_dict.items(): + if hasattr(cfg, key): + attr = getattr(cfg, key) + if key == "attrs" and isinstance(value, Mapping): + # Keep the base module independent of rigid schemas at + # import time; only rigid-derived configs expose this key. + from .rigid import _rigid_body_physics_from_dict + + setattr(cfg, key, _rigid_body_physics_from_dict(value)) + elif is_configclass(attr): + setattr( + cfg, key, attr.from_dict(value) + ) # Call from_dict on the attribute + else: + setattr(cfg, key, value) + else: + logger.log_warning( + f"Key '{key}' not found in {cfg.__class__.__name__}." + ) + + # Automatically infer init_local_pose if not provided + if cfg.init_local_pose is None: + # If only init_pos or init_rot are provided, generate the 4x4 pose matrix + from scipy.spatial.transform import Rotation as R + + T = np.eye(4) + T[:3, 3] = np.array(cfg.init_pos) + T[:3, :3] = R.from_euler("xyz", np.deg2rad(cfg.init_rot)).as_matrix() + cfg.init_local_pose = T + else: + # If only init_local_pose is provided, extract init_pos and init_rot + from scipy.spatial.transform import Rotation as R + + T = np.array(cfg.init_local_pose) + cfg.init_pos = tuple(T[:3, 3]) + cfg.init_rot = tuple(R.from_matrix(T[:3, :3]).as_euler("xyz", degrees=True)) + + return cfg diff --git a/embodichain/lab/sim/cfg/deformable.py b/embodichain/lab/sim/cfg/deformable.py new file mode 100644 index 000000000..57423ecc6 --- /dev/null +++ b/embodichain/lab/sim/cfg/deformable.py @@ -0,0 +1,272 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Deformable-body physical and object configuration.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import MISSING +from typing import Any +from typing import Literal, Sequence + +import numpy as np + +from embodichain.utils import configclass + +from ..shapes import MeshCfg +from .asset import ObjectBaseCfg + +__all__: list[str] = [] + + +@configclass +class VolumeDeformableMeshingCfg: + """Newton tetrahedralization and render-volume binding parameters.""" + + triangle_remesh_resolution: int = 8 + """Resolution used to remesh the source surface before tetrahedralization.""" + + triangle_simplify_target: int = 0 + """Target face count for the proxy surface; zero disables simplification.""" + + simulation_mesh_resolution: int = 8 + """Voxel resolution used to build the tetrahedral simulation mesh.""" + + voxel_num_relaxation_iters: int = 5 + """Number of tetrahedral-mesh relaxation iterations.""" + + voxel_rel_min_tet_volume: float = 0.05 + """Minimum tetrahedron volume relative to the voxel volume.""" + + voxel_surface_dist_ratio: float = 0.2 + """Maximum surface distance expressed as a voxel-size ratio.""" + + embedding_impl: str = "dexsim_exact_cpu" + """DexSim implementation used to bind render vertices to tetrahedra.""" + + +@configclass +class SurfaceElementPropertiesCfg: + """Newton surface triangle, bending, and aerodynamic properties. + + ``None`` preserves Newton's cloth defaults; volume objects resolve it to + zero, matching their disabled-by-default surface forces. + """ + + tri_ke: float | None = None + """Triangle elastic stiffness.""" + + tri_ka: float | None = None + """Triangle area stiffness.""" + + tri_kd: float | None = None + """Triangle damping.""" + + tri_drag: float | None = None + """Aerodynamic drag coefficient.""" + + tri_lift: float | None = None + """Aerodynamic lift coefficient.""" + + edge_ke: float | None = None + """Bending-edge stiffness.""" + + edge_kd: float | None = None + """Bending-edge damping.""" + + +_SURFACE_FIELDS = ( + "tri_ke", + "tri_ka", + "tri_kd", + "tri_drag", + "tri_lift", + "edge_ke", + "edge_kd", +) + + +@configclass +class VolumeDeformablePhysicsCfg: + """Volume density, elasticity, and optional Newton surface constraints.""" + + youngs: float = 1e6 + """Young's modulus [Pa]; higher values make the volume stiffer.""" + + poissons: float = 0.45 + """Poisson's ratio; values approaching 0.5 resist volume change.""" + + elasticity_damping: float = 0.0 + """Volumetric damping coefficient forwarded as Newton ``k_damp``.""" + + density: float = 1000.0 + """Volume density [kg/m³].""" + + surface_props: SurfaceElementPropertiesCfg = SurfaceElementPropertiesCfg( + **dict.fromkeys(_SURFACE_FIELDS, 0.0) + ) + """Optional surface forces; all coefficients default to zero.""" + + add_surface_edges: bool = True + """Whether Newton creates surface bending-edge constraints.""" + + def __post_init__(self) -> None: + if isinstance(self.surface_props, Mapping): + self.surface_props = SurfaceElementPropertiesCfg(**self.surface_props) + + +@configclass +class SurfaceDeformablePhysicsCfg: + """Surface density, Newton surface elements, and optional mesh springs.""" + + density: float = 1.0 + """Surface density [kg/m²].""" + + surface_props: SurfaceElementPropertiesCfg = SurfaceElementPropertiesCfg() + """Triangle, bending, and aerodynamic overrides; None uses Newton defaults.""" + + add_springs: bool = False + """Whether Newton creates explicit mesh-edge springs.""" + + spring_ke: float | None = None + """Spring stiffness; ``None`` uses the Newton default.""" + + spring_kd: float | None = None + """Spring damping; ``None`` uses the Newton default.""" + + def __post_init__(self) -> None: + if isinstance(self.surface_props, Mapping): + self.surface_props = SurfaceElementPropertiesCfg(**self.surface_props) + + +def _mesh_cfg_from_dict(data: Mapping[str, Any]) -> MeshCfg: + cfg = MeshCfg.from_dict({"shape_type": "Mesh", **data}) + if not isinstance(cfg, MeshCfg): + raise TypeError("Deformable shape must be a MeshCfg.") + return cfg + + +@configclass +class DeformableObjectCfg(ObjectBaseCfg): + """Common configuration contract for one deformable asset. + + Concrete volume and surface configurations author Newton particle-set + properties. The discriminator is explicit so manager and visualization + code do not need to infer topology from a mesh or material type. + """ + + deformable_type: Literal["volume", "surface"] = MISSING + """Physical topology represented by the asset.""" + + shape: MeshCfg = MeshCfg() + """Render and source-mesh configuration.""" + + particle_radius: float | None = None + """Newton particle radius; ``None`` uses the active solver default.""" + + particle_flags: int | Sequence[int] | np.ndarray | None = None + """Newton particle flags, provided as one broadcast value or one value per node. + + Clear the Newton ``ACTIVE`` bit for nodes that will be driven kinematically. + Per-node arrays must follow the resolved simulation-particle order. For a + surface deformable, an array-backed + :class:`~embodichain.lab.sim.shapes.MeshCfg` preserves this order. A volume + deformable is voxelized into a separate tetrahedral simulation mesh, so its + particle indices do not correspond to source-mesh vertex indices. + """ + + validate_mesh: bool = False + """Whether Newton reports source-mesh quality validation warnings.""" + + def __post_init__(self) -> None: + if isinstance(self.shape, Mapping): + self.shape = _mesh_cfg_from_dict(self.shape) + visual_shape = getattr(self, "visual_shape", None) + if isinstance(visual_shape, Mapping): + self.visual_shape = _mesh_cfg_from_dict(visual_shape) + + @classmethod + def from_dict(cls, init_dict: Mapping[str, Any]) -> DeformableObjectCfg: + """Parse nested deformable configs using the current schema. + + Args: + init_dict: Configuration fields from a dictionary or YAML loader. + + Returns: + A typed configuration with the base asset pose conventions applied. + """ + cfg = cls(**dict(init_dict)) + pose = ObjectBaseCfg.from_dict( + { + "init_pos": cfg.init_pos, + "init_rot": cfg.init_rot, + "init_local_pose": cfg.init_local_pose, + } + ) + cfg.init_pos, cfg.init_rot = pose.init_pos, pose.init_rot + cfg.init_local_pose = pose.init_local_pose + return cfg + + +@configclass +class VolumeDeformableObjectCfg(DeformableObjectCfg): + """Configuration for a Newton volume-deformable particle set.""" + + deformable_type: Literal["volume"] = "volume" + + meshing: VolumeDeformableMeshingCfg = VolumeDeformableMeshingCfg() + """Tetrahedral simulation-mesh voxelization attributes.""" + + attrs: VolumeDeformablePhysicsCfg = VolumeDeformablePhysicsCfg() + """Newton volume-deformable physical attributes.""" + + def __post_init__(self) -> None: + super().__post_init__() + if isinstance(self.attrs, Mapping): + self.attrs = VolumeDeformablePhysicsCfg(**self.attrs) + if isinstance(self.meshing, Mapping): + self.meshing = VolumeDeformableMeshingCfg(**self.meshing) + + +@configclass +class SurfaceDeformableObjectCfg(DeformableObjectCfg): + """Configuration for a Newton surface-deformable particle set.""" + + deformable_type: Literal["surface"] = "surface" + + visual_shape: MeshCfg | None = None + """Optional render mesh driven by the simulation surface. + + When omitted, :attr:`shape` supplies both simulation topology and rendering. + Use a separately indexed mesh here when the visual asset needs authored UVs, + seam vertices, or other detail that should not change the simulation mesh. + """ + + visual_binding_mode: Literal["auto", "nearest_vertex"] = "auto" + """Binding used to drive :attr:`visual_shape` from simulation particles. + + ``"auto"`` uses DexSim's surface embedding. ``"nearest_vertex"`` is useful + when the render mesh duplicates simulation vertices along texture seams. + """ + + attrs: SurfaceDeformablePhysicsCfg = SurfaceDeformablePhysicsCfg() + """Newton surface-deformable physical attributes.""" + + def __post_init__(self) -> None: + super().__post_init__() + if isinstance(self.attrs, Mapping): + self.attrs = SurfaceDeformablePhysicsCfg(**self.attrs) diff --git a/embodichain/lab/sim/cfg/rigid.py b/embodichain/lab/sim/cfg/rigid.py new file mode 100644 index 000000000..42e701065 --- /dev/null +++ b/embodichain/lab/sim/cfg/rigid.py @@ -0,0 +1,701 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Rigid-body mass, collision, material, and backend property schemas.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import fields +from typing import Any, Sequence + +import numpy as np +from dexsim.types import PhysicalAttr + +from embodichain.utils import configclass +from embodichain.utils.math import convert_quat + + +@configclass +class MassPropertiesCfg: + """Backend-neutral rigid-body mass properties. + + ``None`` means that the source asset or selected backend keeps ownership of + that value. For a non-static body, explicit inertia requires a positive + mass. A source-backed body retains authored inertia unless + :attr:`recompute_inertia` is enabled; procedural or recomputed bodies derive + inertia from collision geometry and the effective mass or density. Static + bodies omit all mass properties during Spawn compilation. + """ + + mass: float | None = None + """Rigid-body mass [kg]. + + A positive value takes precedence over :attr:`density`. Zero explicitly + selects density-based derivation and therefore requires a positive density. + Negative values are invalid. + """ + + density: float | None = None + """Uniform density used to derive mass properties from collision shapes [kg/m^3]. + + The value must be positive and is ignored when :attr:`mass` is positive. + For a source-backed body with valid authored inertia, set + :attr:`recompute_inertia` to ``True`` when supplying density: deriving + density-based properties necessarily replaces the source tensor. + """ + + inertia: Sequence[float] | np.ndarray | None = None + """Inertia about the center of mass [kg*m^2]. + + Supply either three positive principal moments or a symmetric, + positive-definite 3-by-3 tensor in the body frame. Explicit inertia is + accepted only together with a positive :attr:`mass`. For one definition + shared by both backends, prefer principal moments plus + :attr:`com_quaternion`; the current Default adapter consumes the principal- + moment representation, while Newton can retain a full tensor. + """ + + recompute_inertia: bool | None = None + """Whether collision geometry should replace source-authored inertia. + + ``True`` discards source inertia so the backend recomputes it from the + collision geometry and effective mass or density. ``False`` preserves the + source inertia. ``None`` inherits an outer rigid-body overlay and otherwise + behaves like ``False``. An invalid/all-zero source tensor is never + preserved: when collision geometry exists, both backends use it as the + common fallback. Explicit :attr:`inertia` cannot be combined with + recomputation. + """ + + com_position: Sequence[float] | np.ndarray | None = None + """Center-of-mass position expressed in the rigid body's local frame [m].""" + + com_quaternion: Sequence[float] | np.ndarray | None = None + """Orientation of the center-of-mass/inertia frame in ``xyzw`` order. + + Spawn normalizes the quaternion and converts it to the backend descriptor's + ``wxyz`` convention. A zero quaternion is invalid. + """ + + +@configclass +class DefaultRigidBodyPropertiesCfg: + """Rigid-body properties consumed only by the Default backend. + + Every field defaults to ``None`` so a partial overlay preserves an authored + USD/URDF value or the backend default. + """ + + linear_damping: float | None = None + """Non-negative damping coefficient applied to linear velocity.""" + + angular_damping: float | None = None + """Non-negative damping coefficient applied to angular velocity.""" + + has_gravity: bool | None = None + """Whether world gravity accelerates this body (Default backend only). + + ``None`` preserves source/backend intent. Articulation ``attrs`` applies + to all links; ``link_attrs`` can override individual links. File-backed + assets require ``asset_physics_mode="overlay"`` to apply this property. + Explicit gravity overrides are rejected by the Newton integration. + """ + + max_linear_velocity: float | None = None + """Maximum rigid-body linear speed [m/s].""" + + max_angular_velocity: float | None = None + """Maximum rigid-body angular speed [rad/s].""" + + max_depenetration_velocity: float | None = None + """Maximum separation speed introduced to resolve penetration [m/s].""" + + retain_acceleration: bool | None = None + """Whether accumulated acceleration is retained across simulation steps.""" + + enable_ccd: bool | None = None + """Whether continuous collision detection is enabled for this body. + + Scene-level CCD must also be enabled through + :attr:`DefaultPhysicsCfg.enable_ccd`. + """ + + min_position_iters: int | None = None + """Minimum number of position-solver iterations for this body (1 to 255).""" + + min_velocity_iters: int | None = None + """Minimum number of velocity-solver iterations for this body (0 to 255).""" + + sleep_threshold: float | None = None + """Mass-normalized kinetic-energy threshold below which the body may sleep.""" + + +@configclass +class CollisionPropertiesCfg: + """Collision-shape properties with identical intent across both backends. + + The framework default contact envelope is ``contact_offset=0.002`` and + ``rest_offset=0.001``. The contact envelope is expressed once with + Default-backend terminology and is compiled to Newton's ``margin``/``gap`` + representation at the Spawn boundary. Pass ``None`` explicitly for a + sparse source-asset overlay. Mesh approximation and SDF cooking belong to + :class:`~embodichain.lab.sim.shapes.MeshCollisionCfg`. + """ + + collision_enabled: bool | None = None + """Whether the shape participates in rigid shape-shape collision. + + On Newton this maps to ``ShapeConfig.has_shape_collision``. ``None`` + preserves the source/backend value. + """ + + contact_offset: float | None = 0.002 + """Per-shape distance at which contact generation starts [m]. + + The pair threshold is the sum of both shapes' contact offsets. This value + must be non-negative and no smaller than :attr:`rest_offset`. Default + consumes it directly; Newton compiles it together with :attr:`rest_offset` + to ``gap = contact_offset - rest_offset``. + """ + + rest_offset: float | None = 0.0 + """Per-shape target separation at rest [m]. + + Pairwise rest separation is the sum of both shapes' values. Positive + values leave an air gap, zero targets touching surfaces, and negative + values permit limited penetration. Default consumes it directly; Newton + maps it to ``margin``. + """ + + +@configclass +class DefaultCollisionPropertiesCfg(CollisionPropertiesCfg): + """Collision-solver properties consumed only by the Default backend. + + ``contact_offset`` and ``rest_offset`` are portable fields defined by + :class:`CollisionPropertiesCfg`. This backend extension keeps them sparse + so a backend-native source overlay does not accidentally replace an + authored portable envelope. + """ + + contact_offset: float | None = None + """Optional portable contact-generation distance [m].""" + + rest_offset: float | None = None + """Optional portable target separation at rest [m].""" + + torsional_patch_radius: float | None = None + """Contact-patch radius used to approximate torsional friction [m].""" + + min_torsional_patch_radius: float | None = None + """Minimum contact-patch radius used for torsional friction [m].""" + + disable_strong_friction: bool | None = None + """Whether to disable Default-backend strong-friction contact anchoring.""" + + +@configclass +class NewtonCollisionPropertiesCfg(CollisionPropertiesCfg): + """Newton-native contact-envelope and particle-contact properties. + + Mesh construction belongs to ``MeshCfg.collision``; filtering, visual, and + semantic-site policies are deliberately not part of rigid-body physics. + Its portable fields remain sparse so a Newton-native source overlay can + modify only the requested native properties. + + See `Newton Shape Configuration + `_. + """ + + has_particle_collision: bool | None = None + """Whether Newton particles collide with this shape. + + ``None`` preserves the source/backend value. This controls deformable + contact participation; scene-level collision isolation remains scene-owned. + """ + + contact_offset: float | None = None + """Optional portable contact-generation distance [m].""" + + rest_offset: float | None = None + """Optional portable target separation at rest [m].""" + + margin: float | None = None + """Outward collision-surface offset [m]. + + Margins from both shapes are added. They determine where contact is placed + and also affect inertia/SDF handling for hollow shapes. + """ + + gap: float | None = None + """Additional contact-detection distance outside :attr:`margin` [m]. + + Gaps from both shapes are added. Broad phase expands each shape by + ``margin + gap``; increasing the gap detects approaching contact earlier. + """ + + +@configclass +class RigidBodyMaterialCfg: + """Common rigid-contact material intent. + + All fields use sparse-overlay semantics: ``None`` preserves the source or + backend default. The Default backend consumes all three values. Newton + has one Coulomb friction coefficient, so it maps :attr:`dynamic_friction` + to ``ShapeConfig.mu`` and currently has no separate static-friction input; + restitution is consumed only by Newton solvers that support it. + """ + + static_friction: float | None = None + """Static friction coefficient used before tangential slip begins. + + This is currently consumed only by the Default backend. + """ + + dynamic_friction: float | None = None + """Sliding friction coefficient. + + The Default backend uses it as dynamic friction; Newton uses it as its + single Coulomb friction coefficient ``mu``. + """ + + restitution: float | None = None + """Coefficient of restitution, where zero is inelastic and one is elastic. + + The active backend/solver may further restrict or ignore restitution. + """ + + +@configclass +class NewtonRigidBodyMaterialCfg(RigidBodyMaterialCfg): + """Newton contact-material extensions. + + Solver support differs by field. Semi-implicit and Featherstone consume + ``ke``, ``kd``, ``kf``, ``ka``, ``mu``, and ``kh``; MuJoCo Warp consumes + ``ke``, ``kd``, ``mu``, ``kh``, and the torsional/rolling coefficients; + XPBD consumes ``mu``, restitution, and torsional/rolling friction. DexSim + warns when an explicitly changed contact field is ignored by the selected + solver. + """ + + ke: float | None = None + """Elastic contact stiffness coefficient.""" + + kd: float | None = None + """Normal contact damping coefficient.""" + + kf: float | None = None + """Tangential/friction damping coefficient.""" + + ka: float | None = None + """Contact adhesion distance [m].""" + + kh: float | None = None + """Hydroelastic contact stiffness used when hydroelastic contact is enabled.""" + + torsional_friction: float | None = None + """Torsional friction coefficient resisting spin at a contact point.""" + + rolling_friction: float | None = None + """Rolling friction coefficient resisting rolling motion.""" + + +_RIGID_PHYSICS_GROUP_FIELDS = frozenset( + { + "mass_props", + "rigid_props", + "collision_props", + "material_props", + } +) + +_REMOVED_RIGID_PHYSICS_GROUP_FIELDS = { + "default_props": "the corresponding polymorphic property slot", + "newton_props": "the corresponding polymorphic property slot", + "mesh_collision_props": "MeshCfg.collision", +} + + +def _default_rigid_props_from_dict( + value: Mapping[str, Any] | object | None, +) -> DefaultRigidBodyPropertiesCfg | None: + """Parse the currently Default-only rigid-body property slot.""" + if value is None or isinstance(value, DefaultRigidBodyPropertiesCfg): + return value + if not isinstance(value, Mapping): + raise TypeError( + "rigid_props must be a mapping or DefaultRigidBodyPropertiesCfg." + ) + data = dict(value) + backend = str(data.pop("backend", "default")).replace("-", "_").lower() + if backend != "default": + raise ValueError( + "rigid_props.backend must be 'default'; Newton currently exposes no " + "body-level property config." + ) + try: + return DefaultRigidBodyPropertiesCfg(**data) + except TypeError as exc: + raise TypeError(f"Invalid rigid_props configuration: {exc}") from exc + + +def _physics_property_cfg_from_dict( + value: Mapping[str, Any] | object | None, + *, + common_type: type, + backend_types: Mapping[str, type], + field_name: str, +) -> object | None: + """Parse one polymorphic rigid-physics property slot.""" + if value is None: + return None + supported_types = (common_type, *backend_types.values()) + if isinstance(value, supported_types): + return value + if not isinstance(value, Mapping): + raise TypeError(f"{field_name} must be a mapping or {common_type.__name__}.") + data = dict(value) + configured_backend = data.pop("backend", None) + if configured_backend is None: + common_fields = {item.name for item in fields(common_type)} + matching_backends = [ + backend + for backend, config_type in backend_types.items() + if ( + {item.name for item in fields(config_type)} - common_fields + ).intersection(data) + ] + if len(matching_backends) > 1: + raise ValueError( + f"{field_name} mixes Default and Newton-only fields; select one " + "backend-specific property config." + ) + backend = matching_backends[0] if matching_backends else "common" + else: + backend = str(configured_backend).replace("-", "_").lower() + config_type = common_type if backend == "common" else backend_types.get(backend) + if config_type is None: + supported_backends = ("common", *backend_types) + raise ValueError( + f"{field_name}.backend must be one of {supported_backends}, got " + f"{backend!r}." + ) + try: + return config_type(**data) + except TypeError as exc: + raise TypeError(f"Invalid {field_name} configuration: {exc}") from exc + + +def _physics_property_cfg_to_dict( + value: object | None, + *, + common_type: type, + backend_types: Mapping[str, type], + field_name: str, +) -> dict[str, Any] | None: + """Serialize one polymorphic property slot with a stable discriminator.""" + if value is None: + return None + backend = next( + ( + name + for name, config_type in backend_types.items() + if isinstance(value, config_type) + ), + None, + ) + if backend is None and type(value) is not common_type: + raise TypeError( + f"Unsupported {field_name} config type {type(value).__name__!r}." + ) + data = dict(value.to_dict()) + if backend is not None: + data["backend"] = backend + return data + + +def _copy_dexsim_physical_attr(source: PhysicalAttr) -> PhysicalAttr: + """Copy a native ``PhysicalAttr`` without relying on pickle support. + + DexSim exposes ``PhysicalAttr`` through a pybind extension object, so + :func:`copy.deepcopy` cannot clone it. Copy scalar fields from the native + mapping and clone its array-valued mass/COM fields explicitly before a + sparse grouped overlay is applied. + """ + copied = PhysicalAttr() + for field_name, value in source.as_dict().items(): + setattr(copied, field_name, value) + for field_name in ("inertia", "com_position", "com_quaternion"): + value = getattr(source, field_name, None) + if value is not None: + setattr(copied, field_name, np.array(value, dtype=np.float32, copy=True)) + return copied + + +@configclass +class RigidBodyPhysicsCfg: + """Grouped rigid-body physics configuration used by Spawn. + + Every nested field defaults to ``None``. With + ``asset_physics_mode="overlay"``, Spawn therefore changes only explicitly + configured values and preserves all other USD/URDF or backend defaults. + For a newly authored procedural rigid shape, an absent ``collision_props`` + block receives :class:`CollisionPropertiesCfg`'s common contact envelope. + Each physical concept has exactly one slot. Dict/YAML input selects a + backend subclass with a local discriminator, while a unique native field + may infer that subclass. Mesh collision construction belongs to + :class:`~embodichain.lab.sim.shapes.MeshCfg`, not this body-physics schema. + """ + + mass_props: MassPropertiesCfg | None = None + """Backend-neutral mass, inertia, COM, and recomputation overrides.""" + + rigid_props: DefaultRigidBodyPropertiesCfg | None = None + """Optional Default-native body properties. + + Newton currently exposes no body-level property group beyond common mass + properties, so there is no empty Newton marker config. + """ + + collision_props: CollisionPropertiesCfg | None = None + """Portable collision envelope plus one optional backend-specific subtype.""" + + material_props: RigidBodyMaterialCfg | None = None + """Portable contact material values plus optional backend-native coefficients.""" + + @classmethod + def from_dict(cls, init_dict: Mapping[str, Any]) -> RigidBodyPhysicsCfg: + """Parse grouped physics properties from a YAML/JSON-style mapping.""" + removed = _REMOVED_RIGID_PHYSICS_GROUP_FIELDS.keys() & init_dict.keys() + if removed: + replacements = ", ".join( + f"{name} -> {_REMOVED_RIGID_PHYSICS_GROUP_FIELDS[name]}" + for name in sorted(removed) + ) + raise ValueError(f"Removed RigidBodyPhysicsCfg fields: {replacements}.") + unknown = set(init_dict) - _RIGID_PHYSICS_GROUP_FIELDS + if unknown: + raise KeyError(f"Unknown RigidBodyPhysicsCfg fields: {sorted(unknown)}") + cfg = cls() + if "mass_props" in init_dict: + value = init_dict["mass_props"] + if value is not None: + if not isinstance(value, (MassPropertiesCfg, Mapping)): + raise TypeError( + "mass_props must be a mapping or MassPropertiesCfg." + ) + cfg.mass_props = ( + value + if isinstance(value, MassPropertiesCfg) + else MassPropertiesCfg(**value) + ) + if "rigid_props" in init_dict: + cfg.rigid_props = _default_rigid_props_from_dict(init_dict["rigid_props"]) + if "collision_props" in init_dict: + cfg.collision_props = _physics_property_cfg_from_dict( + init_dict["collision_props"], + common_type=CollisionPropertiesCfg, + backend_types={ + "default": DefaultCollisionPropertiesCfg, + "newton": NewtonCollisionPropertiesCfg, + }, + field_name="collision_props", + ) + if "material_props" in init_dict: + cfg.material_props = _physics_property_cfg_from_dict( + init_dict["material_props"], + common_type=RigidBodyMaterialCfg, + backend_types={"newton": NewtonRigidBodyMaterialCfg}, + field_name="material_props", + ) + return cfg + + def to_dict(self) -> dict[str, Any]: + """Serialize grouped properties without losing backend subclasses.""" + return { + "mass_props": ( + None if self.mass_props is None else self.mass_props.to_dict() + ), + "rigid_props": ( + None + if self.rigid_props is None + else {**self.rigid_props.to_dict(), "backend": "default"} + ), + "collision_props": _physics_property_cfg_to_dict( + self.collision_props, + common_type=CollisionPropertiesCfg, + backend_types={ + "default": DefaultCollisionPropertiesCfg, + "newton": NewtonCollisionPropertiesCfg, + }, + field_name="collision_props", + ), + "material_props": _physics_property_cfg_to_dict( + self.material_props, + common_type=RigidBodyMaterialCfg, + backend_types={"newton": NewtonRigidBodyMaterialCfg}, + field_name="material_props", + ), + } + + @property + def enable_collision(self) -> bool: + """Compatibility view used by legacy object initialization.""" + value = ( + None + if self.collision_props is None + else self.collision_props.collision_enabled + ) + return True if value is None else bool(value) + + def to_dexsim_physical_attr( + self, + *, + base: PhysicalAttr | None = None, + ) -> PhysicalAttr: + """Translate configured Default-compatible values to ``PhysicalAttr``. + + Args: + base: Optional native attributes to overlay. This is used by the + retained raw Default articulation path for sparse per-link + updates. + + Returns: + A DexSim physical-attribute object using its defaults for every + unconfigured grouped field. + """ + attr = PhysicalAttr() if base is None else _copy_dexsim_physical_attr(base) + configs = ( + (self.mass_props, {"recompute_inertia": None}), + (self.rigid_props, {}), + (self.collision_props, {"collision_enabled": "enable_collision"}), + (self.material_props, {}), + ) + for cfg, field_map in configs: + if cfg is None: + continue + for item in fields(cfg): + value = getattr(cfg, item.name) + target_name = field_map.get(item.name, item.name) + if ( + value is None + or target_name is None + or not hasattr(attr, target_name) + ): + continue + if target_name in {"inertia", "com_position"}: + value = np.asarray(value, dtype=np.float32) + elif target_name == "com_quaternion": + value = convert_quat(np.asarray(value, dtype=np.float32), to="wxyz") + setattr(attr, target_name, value) + return attr + + @classmethod + def from_dexsim_physical_attr( + cls, + attr: PhysicalAttr, + ) -> RigidBodyPhysicsCfg: + """Capture native Default attributes in the grouped configuration.""" + + def _array(name: str) -> np.ndarray | None: + value = getattr(attr, name, None) + return None if value is None else np.asarray(value, dtype=np.float32) + + com_quaternion = _array("com_quaternion") + if com_quaternion is not None: + com_quaternion = convert_quat(com_quaternion, to="xyzw") + return cls( + mass_props=MassPropertiesCfg( + mass=getattr(attr, "mass", None), + density=getattr(attr, "density", None), + inertia=_array("inertia"), + com_position=_array("com_position"), + com_quaternion=com_quaternion, + ), + rigid_props=DefaultRigidBodyPropertiesCfg( + has_gravity=getattr(attr, "has_gravity", None), + angular_damping=getattr(attr, "angular_damping", None), + linear_damping=getattr(attr, "linear_damping", None), + max_depenetration_velocity=getattr( + attr, "max_depenetration_velocity", None + ), + sleep_threshold=getattr(attr, "sleep_threshold", None), + min_position_iters=getattr(attr, "min_position_iters", None), + min_velocity_iters=getattr(attr, "min_velocity_iters", None), + max_linear_velocity=getattr(attr, "max_linear_velocity", None), + max_angular_velocity=getattr(attr, "max_angular_velocity", None), + enable_ccd=getattr(attr, "enable_ccd", None), + ), + collision_props=DefaultCollisionPropertiesCfg( + collision_enabled=getattr(attr, "enable_collision", None), + contact_offset=getattr(attr, "contact_offset", None), + rest_offset=getattr(attr, "rest_offset", None), + torsional_patch_radius=getattr(attr, "torsional_patch_radius", None), + min_torsional_patch_radius=getattr( + attr, "min_torsional_patch_radius", None + ), + disable_strong_friction=getattr(attr, "disable_strong_friction", None), + ), + material_props=RigidBodyMaterialCfg( + restitution=getattr(attr, "restitution", None), + dynamic_friction=getattr(attr, "dynamic_friction", None), + static_friction=getattr(attr, "static_friction", None), + ), + ) + + +_REMOVED_FLAT_RIGID_BODY_FIELDS = frozenset( + { + "mass", + "density", + "inertia", + "com_position", + "com_quaternion", + "angular_damping", + "linear_damping", + "max_depenetration_velocity", + "sleep_threshold", + "min_position_iters", + "min_velocity_iters", + "max_linear_velocity", + "max_angular_velocity", + "enable_ccd", + "contact_offset", + "rest_offset", + "enable_collision", + "restitution", + "dynamic_friction", + "static_friction", + } +) + + +def _rigid_body_physics_from_dict(value: Mapping[str, Any]) -> RigidBodyPhysicsCfg: + """Parse the grouped rigid-body physics schema. + + Flat ``attrs`` fields and their compatibility configuration types were + removed. Reject them at the config boundary so no input silently changes + physical meaning. + """ + flat_fields = _REMOVED_FLAT_RIGID_BODY_FIELDS.intersection(value) + if flat_fields: + raise ValueError( + "Removed flat rigid-body attrs fields: " + f"{sorted(flat_fields)}. Use grouped mass_props, rigid_props, " + "collision_props, and material_props." + ) + return RigidBodyPhysicsCfg.from_dict(value) diff --git a/embodichain/lab/sim/cfg/rigid_object.py b/embodichain/lab/sim/cfg/rigid_object.py new file mode 100644 index 000000000..facc35b76 --- /dev/null +++ b/embodichain/lab/sim/cfg/rigid_object.py @@ -0,0 +1,219 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Rigid object and rigid-object-group configuration.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import MISSING +import os +import warnings +from typing import Any, Dict, Literal + +from dexsim.types import ActorType + +from embodichain.utils import configclass, is_configclass, logger + +from ..shapes import ShapeCfg +from .asset import AssetPhysicsMode, ObjectBaseCfg, _resolve_asset_physics_mode +from .rigid import RigidBodyPhysicsCfg + + +@configclass +class RigidObjectCfg(ObjectBaseCfg): + """Configuration for a rigid body asset in the simulation. + + This class extends the base asset configuration to include specific properties for rigid bodies, + such as physical attributes and collision group. + """ + + shape: ShapeCfg = ShapeCfg() + """Shape configuration for the rigid body. """ + + # TODO: supoort basic primitive shapes, such as box, sphere, etc cfg and spawn method. + + attrs: RigidBodyPhysicsCfg = RigidBodyPhysicsCfg() + """Rigid-body physics. + + :class:`RigidBodyPhysicsCfg` groups portable and backend-native intent. + """ + + body_type: Literal["dynamic", "kinematic", "static"] = "dynamic" + + body_scale: tuple | list = (1.0, 1.0, 1.0) + """Scale of the rigid body in the simulation world frame.""" + + asset_physics_mode: AssetPhysicsMode = "preserve" + """How a file-backed asset's physical properties are handled. + + ``"preserve"`` keeps the USD-authored physics. ``"overlay"`` applies + configured properties on top of the parsed asset. Procedural shapes always + use config. + """ + + def resolve_asset_physics_mode(self) -> AssetPhysicsMode: + """Return the effective file-backed physics policy.""" + return _resolve_asset_physics_mode(self.asset_physics_mode) + + @classmethod + def from_dict(cls, init_dict: Dict[str, Any]) -> RigidObjectCfg: + """Parse a rigid object and normalize legacy mesh collision ownership.""" + data = dict(init_dict) + attrs_value = data.get("attrs") + if isinstance(attrs_value, Mapping) and "mesh_collision_props" in attrs_value: + shape_value = data.get("shape") + if not isinstance(shape_value, Mapping): + raise ValueError( + "Legacy attrs.mesh_collision_props requires a mapping-valued " + "MeshCfg shape so it can migrate to shape.collision." + ) + shape_data = dict(shape_value) + if shape_data.get("shape_type") != "Mesh": + raise ValueError( + "Legacy attrs.mesh_collision_props can migrate only to a " + "MeshCfg shape." + ) + if shape_data.get("collision") is not None: + raise ValueError( + "attrs.mesh_collision_props cannot be combined with " + "shape.collision." + ) + attrs_data = dict(attrs_value) + shape_data["collision"] = attrs_data.pop("mesh_collision_props") + data["shape"] = shape_data + data["attrs"] = attrs_data + warnings.warn( + "RigidBodyPhysicsCfg.mesh_collision_props is deprecated; use " + "MeshCfg.collision.", + DeprecationWarning, + stacklevel=2, + ) + return super().from_dict(data) + + def to_dexsim_body_type(self) -> ActorType: + """Convert the body type to dexsim ActorType.""" + if self.body_type == "dynamic": + return ActorType.DYNAMIC + elif self.body_type == "kinematic": + return ActorType.KINEMATIC + elif self.body_type == "static": + return ActorType.STATIC + else: + logger.log_error( + f"Invalid body type '{self.body_type}' specified. Must be one of 'dynamic', 'kinematic', or 'static'." + ) + + +@configclass +class RigidObjectGroupCfg: + """Configuration for a rigid object group asset in the simulation. + + Rigid object groups can be initialized from multiple rigid object configurations specified in a folder. + If `folder_path` is specified, user should provide a RigidObjectCfg in `rigid_objects` as a template configuration for + all objects in the group. + + For example: + ```python + rigid_object_group: RigidObjectGroupCfg( + folder_path="path/to/folder", + max_num=5, + rigid_objects={ + "template_obj": RigidObjectCfg( + shape=MeshCfg( + fpath="", # fpath will be ignored when folder_path is specified + ), + body_type="dynamic", + ) + } + ) + """ + + uid: str | None = None + + rigid_objects: Dict[str, RigidObjectCfg] = MISSING + """Configuration for the rigid objects in the group.""" + + body_type: Literal["dynamic", "kinematic"] = "dynamic" + """Body type for all rigid objects in the group. """ + + folder_path: str | None = None + """Path to the folder containing the rigid object assets. + + This is used to initialize multiple rigid object configurations from a folder. + """ + + max_num: int = 1 + """Maximum number of rigid objects to initialize from the folder. + + This is only used when `folder_path` is specified. + """ + + ext: str = ".obj" + """File extension for the rigid object assets. + + This is only used when `folder_path` is specified. + """ + + @classmethod + def from_dict(cls, init_dict: Dict[str, Any]) -> RigidObjectGroupCfg: + """Initialize the configuration from a dictionary.""" + cfg = cls() + for key, value in init_dict.items(): + if hasattr(cfg, key): + attr = getattr(cfg, key) + if is_configclass(attr): + setattr( + cfg, key, attr.from_dict(value) + ) # Call from_dict on the attribute + elif key == "rigid_objects" and "folder_path" not in init_dict: + rigid_objects_cfg = {} + for obj_name, obj_cfg in value.items(): + rigid_objects_cfg[obj_name] = RigidObjectCfg.from_dict(obj_cfg) + setattr(cfg, key, rigid_objects_cfg) + elif key == "rigid_objects" and "folder_path" in init_dict: + folder_path = init_dict["folder_path"] + max_num = init_dict.get("max_num", 1) + rigid_objects_cfg = {} + if os.path.exists(folder_path) and os.path.isdir(folder_path): + files = os.listdir(folder_path) + files = [f for f in files if f.endswith(cfg.ext)] + # select files up to max_num + n_file = len(files) + select_files = [] + for i in range(max_num): + select_files.append(files[i % n_file]) + + for i, file_name in enumerate(select_files): + file_path = os.path.join(folder_path, file_name) + rigid_obj_cfg: RigidObjectCfg = RigidObjectCfg.from_dict( + list(init_dict["rigid_objects"].values())[0] + ) + rigid_obj_cfg.uid = f"{cfg.uid}_obj_{i}" + rigid_obj_cfg.shape.fpath = file_path + rigid_objects_cfg[rigid_obj_cfg.uid] = rigid_obj_cfg + setattr(cfg, "rigid_objects", rigid_objects_cfg) + else: + logger.log_error( + f"Folder '{folder_path}' does not exist or is not a directory." + ) + else: + setattr(cfg, key, value) + else: + logger.log_warning( + f"Key '{key}' not found in {cfg.__class__.__name__}." + ) + return cfg diff --git a/embodichain/lab/sim/cfg/robot.py b/embodichain/lab/sim/cfg/robot.py new file mode 100644 index 000000000..c26dac132 --- /dev/null +++ b/embodichain/lab/sim/cfg/robot.py @@ -0,0 +1,378 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Robot configuration, serialization, and backend preset selection.""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import MISSING, fields +import enum +import json +from typing import Dict, List + +import numpy as np +import torch + +from embodichain.utils import configclass, is_configclass, logger +from embodichain.utils.utility import key_in_nested_dict + +from ..motion.workspace.cfg import RobotWorkspaceCfg +from .articulation import ( + ArticulationCfg, + JointDrivePropertiesCfg, + _raise_removed_articulation_cfg_fields, + link_attrs_from_dict, +) +from .asset import AssetPhysicsMode +from .rigid import _rigid_body_physics_from_dict +from .simulation import ( + PhysicsBackendCfg, + _normalize_newton_solver_type, + physics_backend_from_cfg, +) +from .urdf import URDFCfg + + +def _get_data_path(path: str) -> str: + """Resolve data through the public facade for monkeypatch compatibility.""" + from . import get_data_path + + return get_data_path(path) + + +@configclass +class RobotCfg(ArticulationCfg): + from embodichain.lab.sim.motion.solvers import SolverCfg + + """Configuration for a robot asset in the simulation. + """ + + joint_drive_props: JointDrivePropertiesCfg = JointDrivePropertiesCfg( + drive_type="force", + stiffness=1e4, + damping=1e3, + max_effort=1e10, + max_velocity=1e10, + friction=0.0, + armature=0.0, + ) + """Joint drive, limit, friction, and armature properties.""" + + asset_physics_mode: AssetPhysicsMode = "overlay" + """Apply configured robot physics on top of source-authored values.""" + + control_parts: Dict[str, List[str]] | None = None + """Control parts is the mapping from part name to joint names. + + For example, {'left_arm': ['joint1', 'joint2'], 'right_arm': ['joint3', 'joint4']} + If no control part is specified, the robot will use all joints as a single control part. + + Note: + - if `control_parts` is specified, `solver_cfg` must be a dict with part names as + keys corresponding to the control parts name. + - The joint names in the control parts support regular expressions, e.g., 'joint[1-6]'. + After initialization of robot, the names will be expanded to a list of full joint names. + - `Robot` is a derived class of `Articulation`, with control parts support. So the `joint_drive_props` + in `ArticulationCfg` can use control part as key to specify the corresponding joint drive properties, + which will be overridden if these joint names are already specified. + """ + + urdf_cfg: URDFCfg | None = None + """URDF assembly configuration which allows for assembling a robot from multiple URDF components. + """ + + # TODO: how to support one solver for multiple parts? + solver_cfg: SolverCfg | Dict[str, SolverCfg] | None = None + """Solver is used to compute forward and inverse kinematics for the robot. + """ + + workspace_cfg: Dict[str, RobotWorkspaceCfg] | None = None + """Runtime workspace cache configuration keyed by control-part name.""" + + @classmethod + def from_dict(cls, init_dict: Dict[str, str | float | tuple]) -> RobotCfg: + """Initialize the configuration from a dictionary.""" + if isinstance(init_dict, cls): + return init_dict + + _raise_removed_articulation_cfg_fields(init_dict) + + import importlib + + solver_module = importlib.import_module("embodichain.lab.sim.motion.solvers") + + cfg = cls() # Create a new instance of the class (cls) + for key, value in init_dict.items(): + if key == "link_attrs" and isinstance(value, dict): + cfg.link_attrs = link_attrs_from_dict(value) + elif key == "attrs" and isinstance(value, Mapping): + cfg.attrs = _rigid_body_physics_from_dict(value) + elif hasattr(cfg, key): + attr = getattr(cfg, key) + if key == "urdf_cfg": + from embodichain.lab.sim.cfg import URDFCfg + + setattr(cfg, key, URDFCfg.from_dict(value)) + elif key == "workspace_cfg" and isinstance(value, dict): + setattr( + cfg, + key, + { + part: ( + part_cfg + if isinstance(part_cfg, RobotWorkspaceCfg) + else RobotWorkspaceCfg(**part_cfg) + ) + for part, part_cfg in value.items() + }, + ) + elif key == "fpath": + setattr(cfg, key, _get_data_path(value)) + elif isinstance(attr, JointDrivePropertiesCfg) and isinstance( + value, dict + ): + setattr( + cfg, + key, + JointDrivePropertiesCfg.from_dict(value, defaults=attr), + ) + elif is_configclass(attr): + setattr( + cfg, key, attr.from_dict(value) + ) # Call from_dict on the attribute + elif isinstance(value, dict) and "class_type" in value: + setattr( + cfg, + key, + getattr(solver_module, f"{value['class_type']}Cfg").from_dict( + value + ), + ) + elif isinstance(value, dict) and key_in_nested_dict( + value, "class_type" + ): + setattr( + cfg, + key, + { + k: getattr( + solver_module, f"{v['class_type']}Cfg" + ).from_dict(v) + for k, v in value.items() + }, + ) + + else: + setattr(cfg, key, value) + else: + logger.log_warning( + f"Key '{key}' not found in {cfg.__class__.__name__}." + ) + return cfg + + def _build_defaults(self, init_dict: dict | None = None) -> None: + """Populate default config fields from ``init_dict``. + + Subclasses override this to read variant/version fields from + ``init_dict``, set them on ``self``, and populate ``urdf_cfg``, + ``control_parts``, ``solver_cfg``, ``joint_drive_props`` and ``attrs``. + The base implementation is a no-op. + + .. attention:: + Do NOT call :func:`merge_robot_cfg` from here -- the subclass + ``from_dict`` calls this hook first, then ``merge_robot_cfg``. + Calling ``merge_robot_cfg`` here would recurse, because + ``merge_robot_cfg`` itself calls ``RobotCfg.from_dict``. + + Args: + init_dict: The raw override dict passed to ``from_dict``. + """ + return None + + def to_dict(self): + """Serialize config to a plain dict (enums, numpy, nested configclass).""" + + def serialize(obj, _visited=None): + if _visited is None: + _visited = set() + if isinstance(obj, enum.Enum): + return obj.value + tracked_id = None + if not isinstance(obj, (str, int, float, bool, type(None))): + tracked_id = id(obj) + if tracked_id in _visited: + return None + _visited.add(tracked_id) + + try: + if isinstance(obj, np.ndarray): + return obj.tolist() + if isinstance(obj, dict): + return { + (k.value if isinstance(k, enum.Enum) else str(k)): serialize( + v, _visited + ) + for k, v in obj.items() + } + if isinstance(obj, (list, tuple)): + return [serialize(v, _visited) for v in obj] + if hasattr(obj, "to_dict") and obj is not self: + return serialize(obj.to_dict(), _visited) + if hasattr(obj, "__dict__"): + return { + k: serialize(v, _visited) + for k, v in obj.__dict__.items() + if v is not None + } + return obj + finally: + if tracked_id is not None: + _visited.remove(tracked_id) + + return serialize(self) + + def to_string(self): + """Return config as a JSON string.""" + return json.dumps(self.to_dict(), indent=2) + + def save_to_file(self, filepath): + """Save config to a local file as JSON.""" + with open(filepath, "w") as f: + f.write(self.to_string()) + + def build_pk_serial_chain( + self, device: torch.device = torch.device("cpu"), **kwargs + ) -> Dict[str, "pk.SerialChain"]: + """Build the serial chain from the URDF file. + + Note: + This method is usually used in imitation dataset saving (compute eef pose from qpos using FK) + and model training (provide a differentiable FK layer or loss computation). + + Args: + device (torch.device): The device to which the chain will be moved. Defaults to CPU. + **kwargs: Additional arguments for building the serial chain. + + Returns: + Dict[str, pk.SerialChain]: The serial chain of the robot for specified control part. + """ + return {} + + +@configclass +class RobotPresetCfg: + """Base class for replace-only robot configurations across physics backends. + + Subclasses declare complete :class:`RobotCfg` alternatives as fields. A + ``default`` field is required; optional fields use Newton backend or solver + profile names such as ``newton``, ``newton_mujoco_warp``, or + ``newton_mjwarp``. The active :class:`PhysicsBackendCfg` selects one + complete alternative at + :meth:`SimulationManager.add_robot`; alternatives are never field-merged. + + Portable robot properties should remain on one ordinary :class:`RobotCfg`. + Use this wrapper only when an asset, actuator model, or native physics value + genuinely requires a different complete robot definition. + + Example:: + + @configclass + class MyRobotPresetCfg(RobotPresetCfg): + default: RobotCfg = MyRobotCfg() + newton_mujoco_warp: RobotCfg = MyNewtonRobotCfg() + """ + + def resolve( + self, + physics_cfg: PhysicsBackendCfg, + *, + newton_solver_type: str | None = None, + ) -> RobotCfg: + """Return an isolated complete robot config for the active backend. + + Args: + physics_cfg: The scene's backend-selecting physics configuration. + newton_solver_type: Resolved Newton solver name when it is already + available from the runtime. If omitted, it is inferred from + ``physics_cfg``. + + Returns: + A deep copy of the highest-priority complete robot alternative. + + Raises: + TypeError: If a preset name is unsupported, ``default`` is + undeclared, or a selected alternative is not a + :class:`RobotCfg`. + ValueError: If no declared alternative can satisfy the backend. + """ + options = {item.name: getattr(self, item.name) for item in fields(self)} + invalid_names = { + name + for name in options + if name != "default" and name != "newton" and not name.startswith("newton_") + } + if invalid_names: + raise TypeError( + f"{type(self).__name__} uses unsupported preset name(s) " + f"{sorted(invalid_names)}; use 'default' or 'newton[_]'." + ) + if "default" not in options: + raise TypeError( + f"{type(self).__name__} must declare a 'default' RobotCfg preset." + ) + + backend = physics_backend_from_cfg(physics_cfg) + if backend == "default": + candidates = ("default",) + else: + solver_type = newton_solver_type + if solver_type is None: + solver_cfg = physics_cfg.solver_cfg + if solver_cfg is None: + solver_type = "auto" + elif isinstance(solver_cfg, Mapping): + solver_type = str( + solver_cfg.get("solver_type") + or solver_cfg.get("class_type") + or "auto" + ) + else: + solver_type = str(getattr(solver_cfg, "solver_type")) + solver_type = _normalize_newton_solver_type(solver_type) + solver_candidates = [] + if solver_type != "auto": + solver_candidates.append(f"newton_{solver_type}") + if solver_type == "mujoco_warp": + solver_candidates.append("newton_mjwarp") + candidates = (*solver_candidates, "newton", "default") + + for candidate in candidates: + selected = options.get(candidate) + if selected is None or selected is MISSING: + continue + if not isinstance(selected, RobotCfg): + raise TypeError( + f"{type(self).__name__}.{candidate} must be a RobotCfg, " + f"got {type(selected).__name__}." + ) + return deepcopy(selected) + + raise ValueError( + f"{type(self).__name__} has no usable preset for {candidates!r}; " + f"declared options are {sorted(options)}." + ) diff --git a/embodichain/lab/sim/cfg/scene.py b/embodichain/lab/sim/cfg/scene.py new file mode 100644 index 000000000..256978f74 --- /dev/null +++ b/embodichain/lab/sim/cfg/scene.py @@ -0,0 +1,186 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Light and inter-object constraint configuration.""" + +from __future__ import annotations + +from dataclasses import MISSING +from typing import Literal + +import numpy as np + +from embodichain.utils import configclass + +from .asset import ObjectBaseCfg + + +@configclass +class LightCfg(ObjectBaseCfg): + """Configuration for a light asset in the simulation. + + Supports six light types matching the dexsim rendering backend: + + - ``"point"``: Per-environment omnidirectional point light with position + and falloff radius. Created as a batched light (one per environment). + - ``"sun"``: Global directional sun light (infinite distance). Created as + a single scene-level instance. Uses direction only; position is ignored. + Sun-specific fields (``angular_radius``, ``halo_size``, ``halo_falloff``) + are reserved for future backend support. + - ``"direction"``: Global pure directional light at infinite distance. + Created as a single scene-level instance. Direction only; no position. + - ``"spot"``: Per-environment spotlight with position, direction, and + inner/outer cone angles. Created as a batched light. + - ``"rect"``: Per-environment rectangular area light with position, + direction, width, and height. Created as a batched light. + - ``"mesh"``: Per-environment mesh-based emissive light. Requires a + :class:`~dexsim.models.MeshObject` via + :meth:`embodichain.lab.sim.objects.light.Light.set_mesh` + (not tensor-batched). Created as a batched light. + + .. attention:: + The ``angular_radius``, ``halo_size``, and ``halo_falloff`` fields are + reserved for future use. The dexsim Python bindings do not yet expose + setters for these sun-specific properties. + """ + + light_type: Literal["point", "sun", "direction", "spot", "rect", "mesh"] = "point" + """Light type. Supported: ``"point"``, ``"sun"``, ``"direction"``, ``"spot"``, ``"rect"``, ``"mesh"``.""" + + # ------------------------------------------------------------------ + # Universal properties (apply to all light types) + # ------------------------------------------------------------------ + + color: tuple[float, float, float] = (1.0, 1.0, 1.0) + """RGB color of the light source. Defaults to white ``(1.0, 1.0, 1.0)``.""" + + intensity: float = 30.0 + """Intensity of the light source in watts/m^2. Defaults to ``30.0``.""" + + enable_shadow: bool = True + """Whether the light casts shadows. Defaults to ``True``.""" + + # ------------------------------------------------------------------ + # Point light + # ------------------------------------------------------------------ + + radius: float = 10.0 + """Falloff radius for point lights. Only used when ``light_type="point"``. Defaults to ``10.0``.""" + + # ------------------------------------------------------------------ + # Directional properties (sun, direction, spot, rect, mesh) + # ------------------------------------------------------------------ + + direction: tuple[float, float, float] = (0.0, 0.0, -1.0) + """Direction vector for directional, spot, rect, and mesh lights. + Defaults to ``(0.0, 0.0, -1.0)`` (pointing down along -Z).""" + + # ------------------------------------------------------------------ + # Sun light (reserved — Python bindings not yet available) + # ------------------------------------------------------------------ + + angular_radius: float = 0.5 + """Angular radius of the sun disc in degrees. Reserved for future use.""" + + halo_size: float = 10.0 + """Halo size for sun light. Reserved for future use.""" + + halo_falloff: float = 3.0 + """Halo falloff for sun light. Reserved for future use.""" + + # ------------------------------------------------------------------ + # Spot light + # ------------------------------------------------------------------ + + spot_angle_inner: float = 30.0 + """Inner cone angle of the spotlight in degrees. Only used when ``light_type="spot"``. + Defaults to ``30.0``.""" + + spot_angle_outer: float = 45.0 + """Outer cone angle of the spotlight in degrees. Only used when ``light_type="spot"``. + Defaults to ``45.0``.""" + + # ------------------------------------------------------------------ + # Rect light + # ------------------------------------------------------------------ + + rect_width: float = 1.0 + """Width of the rectangular area light. Only used when ``light_type="rect"``. + Defaults to ``1.0``.""" + + rect_height: float = 1.0 + """Height of the rectangular area light. Only used when ``light_type="rect"``. + Defaults to ``1.0``.""" + + # ------------------------------------------------------------------ + # Mesh light + # ------------------------------------------------------------------ + + mesh_path: str = "" + """Asset path for mesh-based emissive lights. Only used when ``light_type="mesh"``. + The actual mesh assignment is done via + :meth:`embodichain.lab.sim.objects.light.Light.set_mesh` which accepts a + :class:`dexsim.models.MeshObject`. This field stores the path for reference.""" + + +@configclass +class RigidConstraintCfg: + """Configuration for a fixed constraint between two RigidObjects. + + The constraint binds rigid_object_a's entity[i] to rigid_object_b's entity[i] + within arena[i] (one constraint per arena). + + Args: + name: Base constraint name. Per-arena names are derived as ``f"{name}"`` + (single env) or ``f"{name}_{i}"`` (multi env). + rigid_object_a_uid: UID of the first RigidObject (must exist in the sim). + rigid_object_b_uid: UID of the second RigidObject (must exist in the sim). + local_frame_a: 4x4 joint frame in object A's local coordinates. + ``None`` -> identity (object A's origin). Accepts a single + ``(4, 4)`` matrix (shared by all envs) or an ``(N, 4, 4)`` array + (one frame per env). Defaults to None. + local_frame_b: 4x4 joint frame in object B's local coordinates. + ``None`` -> the frame is computed per env as ``inv(pose_B) @ pose_A`` + from the objects' current poses, so the constraint welds the objects + at their *current* relative pose (rather than pulling their origins + together). An explicit ``(4, 4)`` or ``(N, 4, 4)`` value is used + verbatim. Defaults to None. + constraint_type: Reserved for future typed constraints (prismatic, + revolute, spherical, d6). Only ``"fixed"`` is supported in v1. + + .. attention:: + Both objects must be :class:`RigidObject` instances and must share the + same number of arenas. + """ + + name: str = MISSING + """Base name of the constraint (per-arena names are derived from this).""" + + rigid_object_a_uid: str = MISSING + """UID of the first RigidObject.""" + + rigid_object_b_uid: str = MISSING + """UID of the second RigidObject.""" + + local_frame_a: np.ndarray | None = None + """Local joint frame on object A. None -> identity (object A's origin).""" + + local_frame_b: np.ndarray | None = None + """Local joint frame on object B. None -> ``inv(pose_B) @ pose_A`` per env + (weld at the objects' current relative pose).""" + + constraint_type: Literal["fixed"] = "fixed" + """Constraint type. Only ``"fixed"`` is supported in v1.""" diff --git a/embodichain/lab/sim/cfg/simulation.py b/embodichain/lab/sim/cfg/simulation.py new file mode 100644 index 000000000..46c1d07b1 --- /dev/null +++ b/embodichain/lab/sim/cfg/simulation.py @@ -0,0 +1,760 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""World-level rendering and physics-backend configuration.""" + +from __future__ import annotations + +import math + +from collections.abc import Mapping +from dataclasses import field, fields +from numbers import Real +from typing import Any, Literal, Sequence, TYPE_CHECKING + +import dexsim +import numpy as np +import torch +from dexsim.types import DenoiserType, Renderer, ToneMappingType + +from embodichain.utils import configclass, logger + +if TYPE_CHECKING: + from dexsim.engine.newton_physics import NewtonCfg + from dexsim.engine.newton_physics.solvers_cfg import NewtonSolverCfg + + +@configclass +class DLSSCfg: + """DexSim DLSS configuration for window and offscreen rendering. + + Ray Reconstruction (RR) and Super Resolution (SR) are independently + configurable on the ``"hybrid"``, ``"fast-rt"``, and ``"rt"`` renderers. + DLSS is enabled by default for both windows and offscreen cameras. + Offscreen DLSS also requires the master switch to remain enabled. + + .. attention:: + DLSS requires a Vulkan render device, a compatible NVIDIA GPU/driver, + and a DexSim build with the NGX runtime. Initialization is deferred + until rendering; configuration conversion alone cannot verify support. + Each enabled offscreen camera needs its own temporal history and + Vulkan exchange images, increasing GPU memory use. + """ + + dlss_enabled: bool = True + """Master switch for DLSS. False retains the standard rendering path.""" + + offscreen_dlss_enabled: bool = True + """Enable DLSS for offscreen cameras, including in headless simulations.""" + + rayreconstruction_enabled: bool = True + """Enable RR denoising. Can be used without SR at the target resolution.""" + + upscale_enabled: bool = True + """Enable SR upscaling. Can be used independently of RR.""" + + dlss_quality: int = 2 + """Quality mode and derived internal scale: ``-1`` auto (58%), ``0`` Ultra + Performance (~33%), ``1`` Performance (50%), ``2`` Balanced (58%), + ``3`` Quality (~67%), ``4`` Ultra Quality (77%), ``5`` DLAA (100%).""" + + upsample_ratio: float | None = None + """Optional window target/render ratio, at least 1.0. None leaves zero + render dimensions for DexSim to derive from quality. When specified, + computes each unset render dimension from the actual window size. Only + FastRT/OfflineRT windows honor these overrides; hybrid and offscreen + targets derive their internal resolution from quality.""" + + render_width: int = 0 + """Internal FastRT/OfflineRT window width; zero derives it from quality.""" + + render_height: int = 0 + """Internal FastRT/OfflineRT window height; zero derives it from quality.""" + + target_width: int = 0 + """DexSim compatibility field. Set the actual window or camera width instead.""" + + target_height: int = 0 + """DexSim compatibility field. Set the actual window or camera height instead.""" + + exposure_compensation: float = 1.0 + """Positive, finite exposure multiplier used by the RR bridge.""" + + frame_time_delta_ms: float = 0.0 + """Frame interval in milliseconds passed to DexSim's DLSS temporal path. + + The default ``0.0`` intentionally matches ``dexsim.DLSSConfig``: DexSim + measures the actual render interval automatically. Set a positive value + only for a fixed render cadence; this is a render-frame interval, not a + physics or control timestep. + """ + + def __post_init__(self) -> None: + """Validate scalar types and the ranges of numeric settings.""" + for name in ( + "dlss_enabled", + "offscreen_dlss_enabled", + "rayreconstruction_enabled", + "upscale_enabled", + ): + if not isinstance(getattr(self, name), bool): + raise ValueError(f"DLSSCfg.{name} must be a boolean.") + if type(self.dlss_quality) is not int or not -1 <= self.dlss_quality <= 5: + raise ValueError("DLSSCfg.dlss_quality must be an integer from -1 to 5.") + for name in ("render_width", "render_height", "target_width", "target_height"): + value = getattr(self, name) + if type(value) is not int or value < 0: + raise ValueError(f"DLSSCfg.{name} must be a non-negative integer.") + if self.upsample_ratio is not None and ( + isinstance(self.upsample_ratio, bool) + or not isinstance(self.upsample_ratio, Real) + or not math.isfinite(self.upsample_ratio) + or self.upsample_ratio < 1.0 + ): + raise ValueError( + "DLSSCfg.upsample_ratio must be a finite number of at least 1.0." + ) + if ( + isinstance(self.exposure_compensation, bool) + or not isinstance(self.exposure_compensation, Real) + or not math.isfinite(self.exposure_compensation) + or self.exposure_compensation <= 0.0 + ): + raise ValueError( + "DLSSCfg.exposure_compensation must be a positive, finite number." + ) + + def to_dexsim_cfg(self, window_width: int, window_height: int) -> dexsim.DLSSConfig: + """Convert settings without changing the window or camera output size. + + Args: + window_width: Window width in pixels. + window_height: Window height in pixels. + + Returns: + Populated :class:`dexsim.DLSSConfig` instance ready to assign to + ``world_config.dlss_config``. + + Raises: + ValueError: If the configuration contains invalid values. + """ + self.__post_init__() + dlss = dexsim.DLSSConfig() + dlss.dlss_enabled = self.dlss_enabled + dlss.offscreen_dlss_enabled = self.offscreen_dlss_enabled + dlss.rayreconstruction_enabled = self.rayreconstruction_enabled + dlss.upscale_enabled = self.upscale_enabled + dlss.dlss_quality = self.dlss_quality + dlss.render_width = self.render_width + dlss.render_height = self.render_height + if self.upsample_ratio is not None: + if self.render_width == 0: + dlss.render_width = max(1, int(window_width / self.upsample_ratio)) + if self.render_height == 0: + dlss.render_height = max(1, int(window_height / self.upsample_ratio)) + dlss.target_width = self.target_width + dlss.target_height = self.target_height + dlss.exposure_compensation = self.exposure_compensation + dlss.frame_time_delta_ms = self.frame_time_delta_ms + return dlss + + +@configclass +class RenderCfg: + renderer: Literal["auto", "hybrid", "fast-rt", "rt"] = "auto" + """Renderer backend to use for the simulation. Options are 'auto', 'hybrid', 'fast-rt', and 'rt'. + + Note: + - 'auto' selects a default renderer based on the detected GPU: RTX-series cards use + 'hybrid', while datacenter cards (A100/A800, H100/H800/H200/H20) use 'fast-rt'. + If no CUDA device is available or the GPU is unknown, it falls back to 'hybrid'. + - 'hybrid' uses ray tracing for shadows and reflections while keeping rasterization for primary rendering, + providing a balance between performance and visual quality. + - 'fast-rt' is a fully ray-traced renderer for maximum visual fidelity, but may have higher computational cost. + - 'rt' is an offline ray-traced renderer for maximum visual fidelity, suitable for high-quality rendering tasks. + """ + + spp: int = 1 + """Samples per pixel for ray tracing rendering. This parameter is only valid when renderer is 'hybrid', 'fast-rt' or 'rt'.""" + + dlss: DLSSCfg = field(default_factory=DLSSCfg) + """DLSS settings for hybrid, fast-rt, and rt windows and offscreen cameras.""" + + tone_mapping_enabled: bool = False + """Whether to map HDR RGB output with the modified Reinhard curve.""" + + tone_mapping_exposure: float = 1.0 + """Fixed linear exposure multiplier applied before tone mapping.""" + + def __post_init__(self) -> None: + """Validate rendering parameters.""" + if self.spp < 1: + logger.log_error("RenderCfg.spp must be at least 1.", ValueError) + if self.tone_mapping_exposure < 0.0: + logger.log_error( + "RenderCfg.tone_mapping_exposure must be non-negative.", ValueError + ) + + def to_dexsim_flags(self) -> Renderer: + """Convert the renderer name to DexSim's renderer enum.""" + if self.renderer == "hybrid": + return Renderer.HYBRID + elif self.renderer == "fast-rt": + return Renderer.FASTRT + elif self.renderer == "rt": + return Renderer.OFFLINERT + elif self.renderer == "auto": + # 'auto' is normally resolved by the SimulationManager before this is + # called. If it reaches here (e.g. used standalone), fall back safely. + logger.log_warning( + "Renderer 'auto' was not resolved before converting to dexsim flags. " + "Falling back to 'hybrid'." + ) + return Renderer.HYBRID + else: + logger.log_error( + f"Invalid renderer type '{self.renderer}' specified. Must be one of 'auto', 'hybrid', 'fast-rt', or 'rt'." + ) + + def apply_to_dexsim_config(self, world_config: dexsim.WorldConfig) -> None: + """Apply rendering settings to a DexSim world configuration. + + Args: + world_config: DexSim world configuration to update in place. + """ + world_config.renderer = self.to_dexsim_flags() + world_config.dlss_config = self.dlss.to_dexsim_cfg( + window_width=world_config.win_config.width, + window_height=world_config.win_config.height, + ) + world_config.raytrace_config.render_iterations_per_frame = self.spp + world_config.raytrace_config.open_denoise = True + world_config.raytrace_config.denoiser_type = DenoiserType.OPTIX + world_config.postprocess_config.tone_mapping_enabled = self.tone_mapping_enabled + world_config.postprocess_config.tone_mapping_type = ( + ToneMappingType.MODIFIED_REINHARD + ) + world_config.postprocess_config.tone_mapping_exposure = ( + self.tone_mapping_exposure + ) + + +@configclass +class GPUMemoryCfg: + """GPU buffer capacities for the Default backend's GPU dynamics pipeline. + + Default-backend GPU buffers cannot all grow dynamically. Values that are + too small may therefore produce overflow warnings, dropped contacts, or an + invalid simulation. These settings are applied only when the Default + backend runs on CUDA; they have no effect on Default CPU or Newton. + """ + + temp_buffer_capacity: int = 2**24 + """Temporary pinned-host buffer capacity in bytes. + + Increase this when the Default backend reports a pinned-host linear + allocator overflow. + """ + + max_rigid_contact_count: int = 2**19 + """Maximum number of rigid-contact records in the GPU contact stream. + + Increase this when the Default backend reports + ``Contact buffer overflow detected``. + """ + + max_rigid_patch_count: int = ( + 2**18 + ) # 81920 is DexSim default but most tasks work with 2**18 + """Maximum number of rigid-contact patches in the GPU patch stream. + + A patch groups nearby contact points that share a contact normal. Increase + this when the Default backend reports ``Patch buffer overflow detected``. + """ + + heap_capacity: int = 2**26 + """Initial capacity in bytes of the GPU and pinned-host memory heaps.""" + + found_lost_pairs_capacity: int = ( + 2**25 + ) # 262144 is DexSim default but most tasks work with 2**25 + """Capacity of broad-phase found/lost pair records.""" + + found_lost_aggregate_pairs_capacity: int = 2**10 + """Capacity of found/lost pair records generated by aggregates.""" + + total_aggregate_pairs_capacity: int = 2**10 + """Capacity of all aggregate-pair records in the GPU pipeline.""" + + +def _gravity_vector( + gravity: Sequence[float] | np.ndarray, +) -> list[float]: + """Validate and normalize a backend-neutral gravity vector.""" + values = np.asarray(gravity, dtype=np.float64).reshape(-1) + if values.size != 3 or not np.all(np.isfinite(values)): + raise ValueError("Gravity must contain three finite values.") + return values.tolist() + + +@configclass +class PhysicsBackendCfg: + """Backend-neutral simulation timing, device, and gravity configuration. + + Concrete backend configs inherit this class. The config type selects the + backend; no independent backend string can disagree with it. + """ + + physics_dt: float = 1.0 / 100.0 + """Duration of one physics step in seconds. + + Environment control steps may contain multiple physics steps. For Newton, + this interval is further divided by :attr:`NewtonPhysicsCfg.num_substeps`. + """ + + device: str | torch.device = "cpu" + """Compute device used to build and step the selected physics backend. + + Concrete backend configurations may redeclare this field when their native + runtime has a different default. In particular, + :class:`NewtonPhysicsCfg` intentionally shadows this CPU default with + ``"cuda:0"``. Callers can still provide an explicit device override. + """ + + gravity: Sequence[float] | np.ndarray = field( + default_factory=lambda: np.array([0.0, 0.0, -9.81]) + ) + """World-frame gravity vector in meters per second squared.""" + + +@configclass +class DefaultPhysicsCfg(PhysicsBackendCfg): + """Configuration selector for the Default physics backend.""" + + bounce_threshold: float = 2.0 + """Relative normal-speed threshold below which contacts do not bounce [m/s].""" + + enable_ccd: bool = False + """Whether to enable scene-level continuous collision detection (CCD). + + A rigid body must also set :attr:`DefaultRigidBodyPropertiesCfg.enable_ccd` + for CCD to be used on that body. + """ + + length_tolerance: float = 0.05 + """Representative scene length used by the Default backend's tolerance scale [m]. + + Set this near the characteristic size of simulated objects. It is a scene + scale, not an accuracy knob, and must be configured before world creation. + """ + + speed_tolerance: float = 0.25 + """Representative scene speed used by the Default backend's tolerance scale [m/s]. + + The backend derives several internal thresholds from this value and + :attr:`length_tolerance`. + """ + + gpu_memory: GPUMemoryCfg = field(default_factory=GPUMemoryCfg) + """Fixed-capacity GPU buffers used by Default-backend CUDA simulation.""" + + def to_dexsim_args(self) -> dict[str, Any]: + """Convert to DexSim physics arguments. + + Solver implementation details that are not exposed by + :class:`DefaultPhysicsCfg` retain their established defaults here. + """ + args = { + "gravity": _gravity_vector(self.gravity), + "bounce_threshold": self.bounce_threshold, + "enable_ccd": self.enable_ccd, + "enable_enhanced_determinism": False, + "enable_friction_every_iteration": True, + } + return args + + +@configclass +class NewtonCollisionPipelineCfg: + """Newton collision-pipeline settings owned at scene scope. + + Construction values map to DexSim's ``NewtonCollisionPipelineCfg``. + ``update_interval`` controls the DexSim-side scheduling of that external + pipeline. Per-shape contact and SDF values belong to + :class:`NewtonCollisionPropertiesCfg` instead. The pipeline performs + broad-phase pair selection, narrow-phase contact generation, and optional + contact reduction for the complete scene. + + See the `Newton collision guide + `_ + for the native pipeline semantics. + """ + + reduce_contacts: bool = True + """Whether to reduce dense mesh contacts to a representative subset. + + Reduction lowers contact count and usually improves performance and solver + stability for mesh-heavy scenes. + """ + + rigid_contact_max: int | None = None + """Maximum number of allocated rigid contacts. + + ``None`` uses the model-provided capacity when available and otherwise lets + Newton estimate it from the scene's shapes and candidate pairs. + """ + + max_triangle_pairs: int = 4_000_000 + """Maximum triangle-pair candidates allocated by the narrow phase. + + Increase this only when complex meshes or heightfields report triangle-pair + overflow. EmbodiChain intentionally uses a larger default than upstream + Newton for mesh-heavy robotics scenes. + """ + + soft_contact_max: int | None = None + """Maximum number of allocated particle/soft contacts. + + ``None`` lets Newton derive the capacity from shape and particle counts. + """ + + soft_contact_margin: float = 0.01 + """Distance margin used to generate particle/soft contacts [m].""" + + broad_phase: Literal["nxn", "sap", "explicit"] | Any | None = None + """Built-in broad-phase mode or a prebuilt Newton broad-phase object. + + ``"explicit"`` tests precomputed pairs, ``"nxn"`` performs an all-pairs + test, and ``"sap"`` uses sweep-and-prune. ``None`` keeps Newton's default. + A prebuilt object is an expert path and must be compatible with + :attr:`narrow_phase`. + """ + + shape_pairs_filtered: Any | None = None + """Optional precomputed pairs for ``"explicit"`` broad phase. + + When provided, this must be a Warp array of shape-index pairs with + ``dtype=wp.vec2i``. ``None`` uses the model's contact-pair list. + """ + + narrow_phase: Any | None = None + """Optional prebuilt Newton narrow-phase object for expert pipelines.""" + + sdf_hydroelastic_config: Any | None = None + """Optional Newton ``HydroelasticSDF.Config``-compatible object. + + ``None`` disables the hydroelastic pipeline. Individual participating + procedural meshes must also opt in through + :attr:`~embodichain.lab.sim.shapes.MeshCollisionCfg.is_hydroelastic`. + """ + + update_interval: int | None = 1 + """External-pipeline updates within one EmbodiChain physics step. + + The default ``1`` updates before every solver substep. An integer + ``k >= 1`` updates at substeps ``0, k, 2k, ...``; ``None`` updates once + at the first solver substep of each physics step. + """ + + def __post_init__(self) -> None: + """Validate external collision-pipeline scheduling.""" + if self.update_interval is not None and ( + type(self.update_interval) is not int or self.update_interval < 1 + ): + raise ValueError( + "NewtonCollisionPipelineCfg.update_interval must be a positive " + "integer or None." + ) + + +@configclass +class NewtonPhysicsCfg(PhysicsBackendCfg): + """Configuration selector for the Newton physics backend. + + DexSim wraps and extends Newton for EmbodiChain. The selected solver and + collision pipeline are scene-wide. Shape, contact, material, and joint + values are configured separately on object and articulation configs and + compiled into DexSim Spawn descriptors. + """ + + device: str | torch.device = "cuda:0" + """Warp device used to build and step Newton, for example ``"cuda:0"``. + + This redeclaration intentionally takes precedence over + :class:`PhysicsBackendCfg.device`, so ``NewtonPhysicsCfg()`` always starts + on CUDA unless the caller explicitly supplies another device. + """ + + num_substeps: int = 10 + """Number of Newton solver substeps per EmbodiChain physics step. + + The effective solver interval is ``physics_dt / num_substeps``. + """ + + requires_grad: bool = False + """Whether to finalize the Newton model with differentiable state enabled. + + EmbodiChain currently requires the Semi-implicit solver for this mode and + disables CUDA graph capture when gradients are enabled. + """ + + use_cuda_graph: bool = True + """Whether to capture Newton stepping in a CUDA graph when supported. + + This is ignored for gradient mode and is unavailable on a CPU device. + """ + + debug_mode: bool = False + """Whether to enable additional Newton runtime diagnostics.""" + + suppress_warp_kernel_logs: bool = True + """Whether to hide Warp startup and kernel compile/load messages. + + Genuine Newton/Warp warnings and errors are not suppressed. + """ + + solver_cfg: Mapping[str, Any] | NewtonSolverCfg | None = None + """Optional Newton solver configuration. + + A mapping is converted to the matching DexSim Newton solver config. Include + ``solver_type`` or ``class_type`` to select the solver, then add any + parameters accepted by that DexSim solver config. If omitted, EmbodiChain + preserves DexSim's scene-aware ``AutoSolverCfg`` default. A DexSim build + exporting ``AutoSolverCfg`` is required; no concrete-solver fallback is used. + """ + + collision_cfg: NewtonCollisionPipelineCfg | Mapping[str, Any] | None = field( + default_factory=NewtonCollisionPipelineCfg + ) + """Optional external collision-pipeline configuration. + + The default preserves external contact generation for ordinary rigid-body + scenes. Set to ``None`` to avoid constructing or executing the external + pipeline, for example when particle solvers use rigid shapes solely as + one-way SDF boundaries. + """ + + broad_phase: Literal["nxn", "sap", "explicit"] | None = None + """Deprecated shortcut for ``collision_cfg.broad_phase``. + + If both are set, ``collision_cfg.broad_phase`` wins. + """ + + visualizer_enabled: bool = False + """Whether to enable DexSim Newton's optional diagnostic visualizer.""" + + def __post_init__(self) -> None: + """Normalize dictionary collision settings at the config boundary.""" + if isinstance(self.collision_cfg, Mapping): + self.collision_cfg = NewtonCollisionPipelineCfg(**self.collision_cfg) + self._validate_collision_pipeline_configuration() + + def _validate_collision_pipeline_configuration(self) -> None: + """Keep the deprecated broad-phase shortcut meaningful.""" + if self.collision_cfg is None and self.broad_phase is not None: + logger.log_error( + "NewtonPhysicsCfg.broad_phase requires collision_cfg to be configured.", + ValueError, + ) + + def to_dexsim_cfg( + self, + gpu_id: int, + ) -> NewtonCfg: + """Convert this config to ``dexsim.engine.newton_physics.NewtonCfg``.""" + self._validate_collision_pipeline_configuration() + from dexsim.engine.newton_physics import ( + AutoSolverCfg, + DexUniSolverCfg, + FeatherstoneSolverCfg, + MJWarpSolverCfg, + NewtonCfg, + NewtonCollisionPipelineCfg as DexsimNewtonCollisionPipelineCfg, + SemiImplicitSolverCfg, + VBDSolverCfg, + XPBDSolverCfg, + ) + + torch_device = ( + torch.device(self.device) if isinstance(self.device, str) else self.device + ) + device = ( + f"cuda:{gpu_id}" + if torch_device.type == "cuda" and torch_device.index is None + else str(torch_device) + ) + + solver_cfg_map: dict[str, type] = { + "auto": AutoSolverCfg, + "dexuni": DexUniSolverCfg, + "mujoco_warp": MJWarpSolverCfg, + "xpbd": XPBDSolverCfg, + "semi_implicit": SemiImplicitSolverCfg, + "featherstone": FeatherstoneSolverCfg, + "vbd": VBDSolverCfg, + } + solver_cfg = _newton_solver_cfg_to_dexsim( + solver_cfg=self.solver_cfg, + solver_cfg_map=solver_cfg_map, + ) + + if self.requires_grad and ( + solver_cfg is None or solver_cfg.solver_type != "semi_implicit" + ): + logger.log_error( + "Newton gradient mode requires an explicit " + "solver_type='semi_implicit'; AutoSolver does not select a " + "differentiable solver." + ) + + collision_pipeline_cfg = None + if self.collision_cfg is not None: + collision_values = { + item.name: getattr(self.collision_cfg, item.name) + for item in fields(self.collision_cfg) + } + if collision_values["broad_phase"] is None: + collision_values["broad_phase"] = self.broad_phase + collision_values["requires_grad"] = self.requires_grad + collision_pipeline_cfg = DexsimNewtonCollisionPipelineCfg( + **collision_values + ) + + newton_cfg_args: dict[str, Any] = { + "dt": self.physics_dt, + "num_substeps": self.num_substeps, + "device": device, + "gravity": _gravity_vector(self.gravity), + "debug_mode": self.debug_mode, + "requires_grad": self.requires_grad, + "suppress_warp_kernel_logs": self.suppress_warp_kernel_logs, + "collision_pipeline_cfg": collision_pipeline_cfg, + "sync_to_dexsim": True, + } + if solver_cfg is not None: + newton_cfg_args["solver_cfg"] = solver_cfg + + cfg = NewtonCfg( + **newton_cfg_args, + ) + cfg.use_cuda_graph = self.use_cuda_graph and not self.requires_grad + cfg._visualizer_enabled = self.visualizer_enabled + return cfg + + +def _normalize_newton_solver_type(solver_type: str) -> str: + """Normalize public EmbodiChain and DexSim Newton solver aliases.""" + key = solver_type.replace("-", "_").lower() + aliases = { + "auto": "auto", + "autosolver": "auto", + "autosolvercfg": "auto", + "auto_solver": "auto", + "auto_solver_cfg": "auto", + "mjwarp": "mujoco_warp", + "mjwarpsolver": "mujoco_warp", + "mjwarpsolvercfg": "mujoco_warp", + "mjwarp_solver": "mujoco_warp", + "mjwarp_solver_cfg": "mujoco_warp", + "mujoco_warp": "mujoco_warp", + "mujocowarp": "mujoco_warp", + "mujocowarpsolver": "mujoco_warp", + "mujocowarpsolvercfg": "mujoco_warp", + "dexuni": "dexuni", + "dexunisolver": "dexuni", + "dexunisolvercfg": "dexuni", + "dexuni_solver": "dexuni", + "dexuni_solver_cfg": "dexuni", + "xpbdsolver": "xpbd", + "xpbdsolvercfg": "xpbd", + "xpbd": "xpbd", + "semiimplicit": "semi_implicit", + "semi_implicit": "semi_implicit", + "semiimplicitsolver": "semi_implicit", + "semiimplicitsolvercfg": "semi_implicit", + "featherstone": "featherstone", + "featherstonesolver": "featherstone", + "featherstonesolvercfg": "featherstone", + "vbd": "vbd", + "vbdsolver": "vbd", + "vbdsolvercfg": "vbd", + } + if key not in aliases: + logger.log_error( + f"Unsupported Newton solver type '{solver_type}'. " + "Expected one of 'auto', 'dexuni', 'mjwarp', 'xpbd', 'semi_implicit', " + "'featherstone', or 'vbd'." + ) + return aliases[key] + + +def _newton_solver_cfg_to_dexsim( + solver_cfg: Mapping[str, Any] | object | None, + solver_cfg_map: Mapping[str, type], +) -> object | None: + """Convert EmbodiChain Newton solver config input to a DexSim config.""" + if solver_cfg is None: + return None + + if not isinstance(solver_cfg, Mapping): + if not hasattr(solver_cfg, "solver_type"): + logger.log_error( + "Newton solver_cfg must be a mapping or a DexSim Newton solver " + "config object with a 'solver_type' attribute." + ) + return solver_cfg + + solver_cfg_data = dict(solver_cfg) + configured_solver_type = ( + solver_cfg_data.pop("solver_type", None) + or solver_cfg_data.pop("class_type", None) + or "auto" + ) + normalized_solver_type = _normalize_newton_solver_type(str(configured_solver_type)) + solver_cfg_type = solver_cfg_map[normalized_solver_type] + return solver_cfg_type(**solver_cfg_data) + + +def physics_cfg_for_backend( + backend: Literal["default", "newton"], +) -> DefaultPhysicsCfg | NewtonPhysicsCfg: + """Return a default physics configuration instance for the given backend.""" + if backend == "newton": + return NewtonPhysicsCfg() + if backend == "default": + return DefaultPhysicsCfg() + raise ValueError( + f"Unsupported physics backend {backend!r}; expected 'default' or 'newton'." + ) + + +def physics_backend_from_cfg( + physics_cfg: PhysicsBackendCfg, +) -> Literal["default", "newton"]: + """Infer the physics backend name from a physics configuration instance.""" + if isinstance(physics_cfg, NewtonPhysicsCfg): + return "newton" + if isinstance(physics_cfg, DefaultPhysicsCfg): + return "default" + logger.log_error( + f"Unsupported physics_cfg type '{type(physics_cfg).__name__}'. " + "Expected DefaultPhysicsCfg or NewtonPhysicsCfg." + ) + + +def validate_physics_cfg(physics_cfg: PhysicsBackendCfg) -> None: + """Validate that ``physics_cfg`` is a supported backend configuration.""" + physics_backend_from_cfg(physics_cfg) diff --git a/embodichain/lab/sim/cfg/urdf.py b/embodichain/lab/sim/cfg/urdf.py new file mode 100644 index 000000000..49a82f4d8 --- /dev/null +++ b/embodichain/lab/sim/cfg/urdf.py @@ -0,0 +1,414 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""URDF assembly configuration.""" + +from __future__ import annotations + +from dataclasses import field +import os +from typing import Any, Dict, List + +import numpy as np + +from embodichain.data.constants import EMBODICHAIN_DEFAULT_DATA_ROOT +from embodichain.utils import configclass, logger + + +def _get_data_path(path: str) -> str: + """Resolve data through the public facade for monkeypatch compatibility.""" + from . import get_data_path + + return get_data_path(path) + + +@configclass +class URDFCfg: + """Standalone configuration class for URDF assembly.""" + + components: Dict[str, Dict[str, str | Dict | np.ndarray]] = field( + default_factory=dict + ) + """Dictionary of robot components to be assembled.""" + + sensors: Dict[str, Dict[str, str | np.ndarray]] = field(default_factory=dict) + """Dictionary of sensors to be attached to the robot.""" + + use_signature_check: bool = True + """Whether to use signature check when merging URDFs.""" + + base_link_name: str = "base_link" + """Name of the base link in the assembled robot.""" + + fpath: str | None = None + """Full output file path for the assembled URDF. If specified, overrides fname and fpath_prefix.""" + + fname: str | None = None + """Name used for output file and directory. If not specified, auto-generated from component names.""" + + fpath_prefix: str = EMBODICHAIN_DEFAULT_DATA_ROOT + "/assembled" + """Output directory prefix for the assembled URDF file.""" + + component_prefix: List[tuple[str, str | None]] = field( + default_factory=lambda: [ + ("chassis", None), + ("legs", None), + ("torso", None), + ("head", None), + ("left_arm", "left_"), + ("right_arm", "right_"), + ("left_hand", "left_"), + ("right_hand", "right_"), + ("arm", None), + ("hand", None), + ] + ) + """Component name prefixes used during URDF assembly. + + Preferred form is a list of ``(component_name, prefix)`` tuples. For + convenience, a mapping ``{component_name: prefix}`` is also accepted when + constructing :class:`URDFCfg` and will be normalized internally. + """ + + name_case: dict[str, str] = field( + default_factory=lambda: { + "joint": "original", + "link": "original", + } + ) + """Case normalization policy applied to joint/link names during URDF assembly. + + Supported values per key are ``"upper"``, ``"lower"`` or ``"original"`` + (legacy alias ``"none"``). The default preserves source URDF casing. + """ + + def __init__( + self, + components: list[dict[str, str | np.ndarray]] | None = None, + sensors: dict[str, dict[str, str | np.ndarray]] | None = None, + fpath: str | None = None, + fname: str | None = None, + fpath_prefix: str = EMBODICHAIN_DEFAULT_DATA_ROOT + "/assembled", + use_signature_check: bool = True, + base_link_name: str = "base_link", + component_prefix: list[tuple[str, str | None]] | None = None, + name_case: dict[str, str] | None = None, + ): + """ + Initialize URDFCfg with optional list of components and output path settings. + + Args: + components (list[dict[str, str | np.ndarray]] | None): List of component configurations. Each dict should contain: + - 'component_type' (str): The type/name of the component (e.g., 'chassis', 'arm', 'hand'). + - 'urdf_path' (str): Path to the component's URDF file. + - 'transform' (np.ndarray | None): 4x4 transformation matrix (optional). + - Additional params can be included as extra keys. + sensors (dict[str, dict[str, str | np.ndarray]] | None): Sensor configurations for the robot. + fpath (str | None): Full output file path for the assembled URDF. If specified, overrides fname and fpath_prefix. + fname (str | None): Name used for output file and directory. If not specified, auto-generated from component names. + fpath_prefix (str): Output directory prefix for the assembled URDF file. + use_signature_check (bool): Whether to use signature check when merging URDFs. + base_link_name (str): Name of the base link in the assembled robot. + component_prefix (list[tuple[str, str | None]] | None): Optional + list of (component_type, prefix) pairs to override default + component name prefixes. + """ + self.components = {} + self.sensors = sensors or {} + self.fpath = fpath + self.use_signature_check = use_signature_check + self.base_link_name = base_link_name + self.fname = fname + self.fpath_prefix = fpath_prefix + + # Initialize component prefixes (patch-style mapping per component type) + if component_prefix is None: + # Use the same default as the dataclass field + self.component_prefix = [ + ("chassis", None), + ("legs", None), + ("torso", None), + ("head", None), + ("left_arm", "left_"), + ("right_arm", "right_"), + ("left_hand", "left_"), + ("right_hand", "right_"), + ("arm", None), + ("hand", None), + ] + elif isinstance(component_prefix, dict): + # Allow dict-style config: {"left_hand": "l_", ...} + self.component_prefix = list(component_prefix.items()) + else: + # Assume caller provided a list of (component_name, prefix) tuples + self.component_prefix = component_prefix + + if name_case is None: + self.name_case = { + "joint": "original", + "link": "original", + } + else: + self.name_case = name_case + + # Auto-add components if provided + if components: + for comp_config in components: + if not isinstance(comp_config, dict): + logger.log_error( + f"Component configuration must be a dict, got {type(comp_config)}" + ) + continue + + # Extract required fields + component_type = comp_config.get("component_type") + urdf_path = comp_config.get("urdf_path") + + if not component_type or not urdf_path: + logger.log_error( + f"Component configuration must contain 'component_type' and 'urdf_path', got {comp_config}" + ) + continue + + # Extract optional fields + transform = comp_config.get("transform", np.eye(4)) + + # Extract additional params (exclude known keys) + params = { + k: v + for k, v in comp_config.items() + if k not in ["component_type", "urdf_path", "transform"] + } + + # Add the component + self.add_component(component_type, urdf_path, transform, **params) + + if sensors is not None: + # Accept both list and dict; serialization round-trips an empty + # dict when no sensors are configured (the field default). + if isinstance(sensors, dict) and not sensors: + self.sensors = [] + elif not isinstance(sensors, (list, dict)): + logger.log_error( + f"sensors must be a list of dicts or a dict, got {type(sensors)}" + ) + self.sensors = [] + elif isinstance(sensors, dict): + # dict keyed by sensor_name -> config + self.sensors = list(sensors.values()) + else: + # Optionally check each sensor dict + valid_sensors = [] + for sensor_config in sensors: + if not isinstance(sensor_config, dict): + logger.log_error( + f"Sensor configuration must be a dict, got {type(sensor_config)}" + ) + continue + sensor_name = sensor_config.get("sensor_name") + if not sensor_name: + logger.log_error( + f"Sensor configuration must contain 'sensor_name', got {sensor_config}" + ) + continue + valid_sensors.append(sensor_config) + self.sensors = valid_sensors + + def set_urdf(self, urdf_path: str) -> "URDFCfg": + """Directly specify a single URDF file for the robot, compatible with the single-URDF robot case. + + Args: + urdf_path (str): Path to the robot's URDF file. + + Returns: + URDFCfg: Returns self to allow method chaining. + """ + self.components.clear() + urdf_file = os.path.splitext(os.path.basename(urdf_path))[0] + self.components[urdf_file] = { + "urdf_path": urdf_path, + "transform": None, + "params": {}, + } + self.fpath = urdf_path + return self + + def add_component( + self, + component_type: str, + urdf_path: str, + transform: np.ndarray | None = None, + **params, + ) -> URDFCfg: + """Add a robot component to the assembly configuration. + + Args: + component_type (str): The type/name of the component. Should be one of SUPPORTED_COMPONENTS + (e.g., 'chassis', 'torso', 'head', 'left_arm', 'right_hand', 'arm', 'hand', etc.). + urdf_path (str): Path to the component's URDF file. + transform (np.ndarray | None): 4x4 transformation matrix for the component in the robot frame (default: None). + **params: Additional keyword parameters for the component (e.g., color, material, etc.). + + Returns: + URDFCfg: Returns self to allow method chaining. + """ + if urdf_path: + if not os.path.exists(urdf_path): + urdf_path_candidate = _get_data_path(urdf_path) + if os.path.exists(urdf_path_candidate): + urdf_path = urdf_path_candidate + else: + logger.log_error(f"URDF path '{urdf_path}' does not exist.") + raise FileNotFoundError(f"URDF path '{urdf_path}' does not exist.") + + if transform is None: + transform = np.eye(4) + + self.components[component_type] = { + "urdf_path": urdf_path, + "transform": np.array(transform), + "params": params, + } + + if self.fname: + self.fpath = f"{self.fpath_prefix}/{self.fname}/{self.fname}.urdf" + else: + # Update output_path to use all component urdf file names joined by underscores as directory + if len(self.components) == 1: + # Only one component, use its urdf file name + urdf_file = os.path.splitext(os.path.basename(urdf_path))[0] + name = urdf_file + else: + # Multiple components, join all urdf file names + urdf_files = [ + os.path.splitext(os.path.basename(v["urdf_path"]))[0] + for v in self.components.values() + ] + name = "_".join(urdf_files) + self.fpath = f"{self.fpath_prefix}/{name}/{name}.urdf" + + return self + + def add_sensor(self, sensor_name: str, **sensor_config) -> URDFCfg: + """Add a sensor to the robot configuration. + + Args: + sensor_name (str): The name of the sensor. + **sensor_config: Additional configuration parameters for the sensor. + + Returns: + URDFCfg: Returns self to allow method chaining. + """ + self.sensors.append({"sensor_name": sensor_name, **sensor_config}) + return self + + def assemble_urdf(self) -> str: + """Assemble URDF files for the robot based on the configuration. + + Returns: + str: The path to the resulting (possibly merged) URDF file. + """ + components = list(self.components.items()) + # If there is only one component, return its URDF path directly. + if len(components) == 1: + _, comp_config = components[0] + return comp_config["urdf_path"] + + from embodichain.toolkits.urdf_assembly import URDFAssemblyManager + + # If there are multiple components, merge them into a single URDF file. + manager = URDFAssemblyManager() + manager.base_link_name = self.base_link_name + + if self.component_prefix is None: + self.component_prefix = [ + ("left_arm", "left_"), + ("right_arm", "right_"), + ("left_hand", "left_"), + ("right_hand", "right_"), + ] + if isinstance(self.component_prefix, dict): + self.component_prefix = list(self.component_prefix.items()) + # Forward configured component prefixes to the assembly manager + manager.component_prefix = self.component_prefix + + if self.name_case is not None: + manager.name_case = self.name_case + + for comp_type, comp_config in components: + params = comp_config.get("params", {}) + success = manager.add_component( + comp_type, + comp_config["urdf_path"], + comp_config.get("transform"), + **params, + ) + if not success: + logger.log_error( + f"Failed to add component '{comp_type}' with config: {comp_config}" + ) + + for sensor in self.sensors: + manager.attach_sensor( + sensor_name=sensor.get("sensor_name"), + sensor_source=sensor.get("sensor_source"), + parent_component=sensor.get("parent_component"), + parent_link=sensor.get("parent_link"), + sensor_type=sensor.get("sensor_type"), + **{ + k: v + for k, v in sensor.items() + if k + not in [ + "sensor_name", + "sensor_source", + "parent_component", + "parent_link", + "sensor_type", + ] + }, + ) + + try: + # Merge all added components into a single URDF file at the specified output path. + merged_urdf_xml = manager.merge_urdfs(self.fpath, self.use_signature_check) + except Exception as e: + logger.log_error(f"URDF merge failed: {e}") + + return self.fpath + + @classmethod + def from_dict(cls, init_dict: Dict) -> "URDFCfg": + if isinstance(init_dict, cls): + return init_dict + components = init_dict.get("components", None) + if isinstance(components, dict): + components = [{"component_type": k, **v} for k, v in components.items()] + sensors = init_dict.get("sensors", None) + fpath = init_dict.get("fpath", None) + use_signature_check = init_dict.get("use_signature_check", True) + base_link_name = init_dict.get("base_link_name", "base_link") + component_prefix = init_dict.get("component_prefix", None) + name_case = init_dict.get("name_case", None) + return cls( + components=components, + sensors=sensors, + fpath=fpath, + use_signature_check=use_signature_check, + base_link_name=base_link_name, + component_prefix=component_prefix, + name_case=name_case, + ) diff --git a/embodichain/lab/sim/cfg/viewer.py b/embodichain/lab/sim/cfg/viewer.py new file mode 100644 index 000000000..35710cda4 --- /dev/null +++ b/embodichain/lab/sim/cfg/viewer.py @@ -0,0 +1,94 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Interactive viewer, marker, and recording configuration.""" + +from __future__ import annotations + +from typing import List, Literal + +import torch +from dexsim.types import AxisArrowType, AxisCornerType + +from embodichain.utils import configclass + + +@configclass +class MarkerCfg: + """Configuration for visual markers in the simulation. + + This class defines properties for creating visual markers such as coordinate frames, + lines, and points that can be used for debugging, visualization, or reference purposes + in the simulation environment. + """ + + name: str = "empty-mesh" + """Name of the marker for identification purposes.""" + + marker_type: Literal["axis", "line", "point"] = "axis" + """Type of marker to display. Can be 'axis' (3D coordinate frame), 'line', or 'point'. (only axis supported now)""" + + axis_xpos: torch.Tensor | None = None + """List of 4x4 transformation matrices defining the position and orientation of each axis marker.""" + + axis_size: float = 0.002 + """Thickness/size of the axis lines in meters.""" + + axis_len: float = 0.005 + """Length of each axis arm in meters.""" + + line_color: List[float] = [1, 1, 0, 1.0] + """RGBA color values for the marker lines. Values should be between 0.0 and 1.0.""" + + arrow_type: AxisArrowType = AxisArrowType.CONE + """Type of arrow head for axis markers (e.g., CONE, ARROW, etc.).""" + + corner_type: AxisCornerType = AxisCornerType.SPHERE + """Type of corner/joint visualization for axis markers (e.g., SPHERE, CUBE, etc.).""" + + arena_index: int = -1 + """Index of the arena where the marker should be placed. -1 means all arenas.""" + + +@configclass +class WindowRecordCfg: + """Configuration for interactive viewer window recording.""" + + enable_hotkey: bool = True + """Whether to register the ``r`` hotkey for viewer recording when the window opens.""" + + save_path: str | None = None + """Optional output path for viewer recordings. If None, use the default outputs directory.""" + + fps: int = 20 + """Frames per second for viewer recording.""" + + max_memory: int = 1024 + """Maximum buffered recording memory in MB before auto-stopping capture.""" + + video_prefix: str = "viewer_record" + """Video file prefix used when no explicit save path is provided.""" + + +@configclass +class WindowCameraPoseCfg: + """Configuration for printing the interactive viewer camera pose.""" + + enable_hotkey: bool = True + """Whether to register the ``p`` hotkey when the window opens.""" + + convert_to_look_at: bool = True + """Whether the hotkey prints a ``set_look_at`` call instead of a matrix.""" diff --git a/embodichain/lab/sim/common.py b/embodichain/lab/sim/common.py index f1380ed6b..a578fb9c7 100644 --- a/embodichain/lab/sim/common.py +++ b/embodichain/lab/sim/common.py @@ -66,8 +66,6 @@ def __init__( self._entities = entities self.device = device - self.reset() - def __str__(self) -> str: return f"{self.__class__}: managing {self.num_instances} {self._entities[0].__class__} objects | uid: {self.uid} | device: {self.device}" diff --git a/embodichain/lab/sim/diff/__init__.py b/embodichain/lab/sim/diff/__init__.py new file mode 100644 index 000000000..1aafcd511 --- /dev/null +++ b/embodichain/lab/sim/diff/__init__.py @@ -0,0 +1,33 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Differentiable Newton kinematics for EmbodiChain. + +Bridges task-defined Warp kinematics into PyTorch autograd and exposes a +:class:`tape_context` manager for advanced users composing their own kernels. +The package does not advance the Newton dynamics solver. +""" + +from __future__ import annotations + +from .bridge import ( + NewtonStepFunc, + tape_context, +) + +__all__ = [ + "NewtonStepFunc", + "tape_context", +] diff --git a/embodichain/lab/sim/diff/bridge.py b/embodichain/lab/sim/diff/bridge.py new file mode 100644 index 000000000..5e0c02c25 --- /dev/null +++ b/embodichain/lab/sim/diff/bridge.py @@ -0,0 +1,197 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Warp-tape ↔ PyTorch-autograd bridge for Newton kinematics.""" + +from __future__ import annotations + +from contextlib import contextmanager +from typing import TYPE_CHECKING, Any, Iterator + +import torch +import warp as wp + +if TYPE_CHECKING: + from embodichain.lab.sim.sim_manager import SimulationManager + +__all__ = ["NewtonStepFunc", "tape_context"] + + +def _validate_manager(manager: Any) -> None: + """Validate the Newton gradient boundary without touching its solver.""" + if not bool(getattr(manager, "is_newton_backend", False)): + raise RuntimeError( + "Differentiable kinematics require the Newton backend with " + "requires_grad=True." + ) + runtime = getattr(manager, "differentiable_runtime", None) + if runtime is not None: + # Model access validates finalization and requires_grad while remaining + # independent of the configured Newton solver. + _ = runtime.model + + +def _reset_tape(tape: wp.Tape | None) -> None: + """Release all arrays retained by a completed Warp tape.""" + if tape is not None: + tape.reset() + + +def _abort_forward(tape: wp.Tape | None) -> None: + """Best-effort cleanup that never masks the original forward failure.""" + try: + _reset_tape(tape) + except BaseException: + pass + + +@contextmanager +def tape_context(manager: "SimulationManager") -> Iterator[wp.Tape]: + """Open a Warp tape for expert Newton kinematics kernels. + + Args: + manager: Prepared Newton-backed simulation manager in gradient mode. + + Yields: + The active Warp tape. Call ``backward`` and then ``reset`` after the + context when retaining it manually. + + Raises: + RuntimeError: If the manager does not use a finalized Newton gradient + model. + """ + _validate_manager(manager) + tape = wp.Tape() + with tape: + yield tape + + +class NewtonStepFunc(torch.autograd.Function): + """Bridge one task-defined Newton kinematics step into PyTorch autograd. + + Forward records the action kernel, named kinematics callback, and output + kernels inside one Warp tape. It does not create contacts, call a Newton + solver, or advance simulation time. Backward seeds the tracked Warp output + arrays from PyTorch gradients and returns the resulting action gradient. + + ``sim_state`` must contain: + + - ``manager``: a prepared Newton-backed :class:`SimulationManager`; + - ``action_kernel``: callable ``(action_wp, tape, *kernel_args)``; + - ``kernel_args``: tuple forwarded to the action kernel; + - ``step_fn``: zero-argument task kinematics callback returning a state; + - ``obs_reward_fn``: callable that maps that state to an output dictionary. + + The output dictionary contains ``_order``, ``_grad_track``, and one torch + tensor for every name in ``_order``. ``_grad_track`` maps a name to the + backing Warp array whose gradient should be seeded, or to ``None`` for a + non-differentiable output. + """ + + @classmethod + def apply(cls, action_torch: torch.Tensor, sim_state: dict[str, Any]) -> Any: + """Capture ambient grad mode before PyTorch enters ``forward``.""" + return super().apply(action_torch, sim_state, torch.is_grad_enabled()) + + @staticmethod + def forward( + ctx: Any, + action_torch: torch.Tensor, + sim_state: dict[str, Any], + outer_grad_enabled: bool, + ) -> tuple[torch.Tensor, ...]: + """Record one kinematics step and materialize its torch outputs.""" + _validate_manager(sim_state["manager"]) + action_kernel = sim_state["action_kernel"] + kernel_args = sim_state["kernel_args"] + step_fn = sim_state["step_fn"] + obs_reward_fn = sim_state["obs_reward_fn"] + if not callable(step_fn): + raise TypeError("Differentiable kinematics require a callable step_fn.") + + ctx.saved_action_shape = action_torch.shape + action_flat = action_torch.detach().clone().reshape(-1).contiguous() + needs_action_grad = bool(outer_grad_enabled and ctx.needs_input_grad[0]) + action_wp = wp.from_torch( + action_flat, + dtype=wp.float32, + requires_grad=needs_action_grad, + ) + + tape = None + try: + tape = wp.Tape() + with tape: + action_kernel(action_wp, tape, *kernel_args) + final_state = step_fn() + outputs = obs_reward_fn(final_state) + outputs_order = tuple(outputs["_order"]) + output_values = tuple(outputs[name] for name in outputs_order) + outputs_grad_track = outputs.get("_grad_track", {}) + except BaseException: + _abort_forward(tape) + raise + + if not needs_action_grad: + _reset_tape(tape) + return output_values + + ctx.tape = tape + ctx.action_wp = action_wp + ctx.outputs_order = outputs_order + ctx.outputs_grad_track = outputs_grad_track + ctx._bridge_released = False + return output_values + + @staticmethod + def backward( + ctx: Any, + *grad_outputs: torch.Tensor | None, + ) -> tuple[torch.Tensor | None, None, None]: + """Run Warp reverse mode and return the bridged action gradient.""" + if getattr(ctx, "_bridge_released", False): + raise RuntimeError( + "NewtonStepFunc backward was already consumed; run a fresh " + "kinematics step before another backward pass." + ) + + action_grad = None + try: + for name, grad_t in zip(ctx.outputs_order, grad_outputs): + wp_arr = ctx.outputs_grad_track.get(name) + if grad_t is None or wp_arr is None: + continue + if wp_arr.grad is None: + wp_arr.grad = wp.zeros_like(wp_arr) + wp.copy( + wp_arr.grad, + wp.from_torch( + grad_t.detach().clone().contiguous(), + dtype=wp.float32, + ), + ) + ctx.tape.backward() + action_wp_grad = getattr(ctx.action_wp, "grad", None) + if action_wp_grad is not None: + action_grad = wp.to_torch(action_wp_grad).clone() + finally: + try: + _reset_tape(ctx.tape) + finally: + ctx._bridge_released = True + + if action_grad is None: + return None, None, None + return action_grad.reshape(ctx.saved_action_shape), None, None diff --git a/embodichain/lab/sim/diff/runtime.py b/embodichain/lab/sim/diff/runtime.py new file mode 100644 index 000000000..abfd66b98 --- /dev/null +++ b/embodichain/lab/sim/diff/runtime.py @@ -0,0 +1,84 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Read-only access to a Spawn-owned Newton gradient model and state.""" + +from __future__ import annotations + +from typing import Any, Callable + +__all__ = ["NewtonDifferentiableRuntime"] + + +class NewtonDifferentiableRuntime: + """Expose Newton model/state buffers required by kinematic environments. + + The backend provider is resolved for every access so a scene rebuild cannot + silently return buffers owned by a replaced Spawn backend. This facade does + not expose controls, contacts, solver stepping, or gradient rollouts. + """ + + def __init__(self, backend_provider: Callable[[], Any]) -> None: + self._backend_provider = backend_provider + + def _backend(self) -> Any: + backend = self._backend_provider() + if backend is None: + raise RuntimeError( + "The Spawn-owned Newton backend is unavailable. Call " + "SimulationManager.prepare() before using differentiable " + "kinematics." + ) + return backend + + def _validated_backend(self) -> Any: + backend = self._backend() + if backend.model is None: + raise RuntimeError( + "The Spawn-owned Newton model is not finalized. Call " + "SimulationManager.prepare() first." + ) + if not bool(backend.cfg.requires_grad): + raise RuntimeError( + "Differentiable Newton kinematics require requires_grad=True." + ) + return backend + + @staticmethod + def _spawn_runtime(backend: Any) -> Any: + """Return DexSim's runtime facade across the 0.4/0.5 API boundary.""" + runtime = getattr(backend, "runtime", None) + if runtime is None: + runtime = getattr(backend, "_runtime", None) + if runtime is None: + raise RuntimeError("The Spawn-owned Newton runtime is unavailable.") + return runtime + + @property + def model(self) -> Any: + """Return the finalized differentiable Newton model.""" + return self._validated_backend().model + + @property + def current_state(self) -> Any: + """Return the live state currently selected by the Spawn runtime.""" + backend = self._validated_backend() + return self._spawn_runtime(backend).current_state + + @property + def live_states(self) -> tuple[Any, Any]: + """Return both live ping-pong states owned by the Spawn backend.""" + backend = self._validated_backend() + return backend.state_0, backend.state_1 diff --git a/embodichain/lab/sim/motion/planners/curobo/curobo_planner.py b/embodichain/lab/sim/motion/planners/curobo/curobo_planner.py index c9869d84b..e9887aa98 100644 --- a/embodichain/lab/sim/motion/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/motion/planners/curobo/curobo_planner.py @@ -46,7 +46,7 @@ import yaml from embodichain.utils import configclass, logger -from embodichain.utils.math import pose_inv, quat_from_matrix +from embodichain.utils.math import convert_quat, pose_inv, quat_from_matrix from embodichain.lab.sim.motion.planners.base_planner import ( BasePlanner, @@ -520,7 +520,9 @@ def _matrix_to_position_quaternion( # so materialize them at the adapter boundary rather than relying on a # caller-specific layout. position = matrix[:, :3, 3].contiguous() - quaternion = quat_from_matrix(matrix[:, :3, :3]).contiguous() # wxyz + quaternion = convert_quat( + quat_from_matrix(matrix[:, :3, :3]), to="wxyz" + ).contiguous() return position, quaternion diff --git a/embodichain/lab/sim/motion/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/motion/planners/curobo/curobo_yaml.py index 55029e6f8..918a814ae 100644 --- a/embodichain/lab/sim/motion/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/motion/planners/curobo/curobo_yaml.py @@ -34,7 +34,7 @@ import torch from embodichain.utils import logger -from embodichain.utils.math import matrix_from_quat, quat_from_matrix +from embodichain.utils.math import convert_quat, matrix_from_quat, quat_from_matrix if TYPE_CHECKING: from embodichain.lab.sim.objects import RigidObject, Robot @@ -404,7 +404,8 @@ def _mesh_to_obstacle_entry( name: Obstacle name (cuRobo key under ``cuboid``/``mesh``/``sphere``). vertices: Mesh vertices ``(V, 3)`` in the object's local frame. faces: Triangle indices ``(F, 3)`` (any integer dtype). - pose: Object pose as ``(x, y, z, qw, qx, qy, qz)`` ``(7,)`` or a + pose: EmbodiChain object pose as ``(x, y, z, qx, qy, qz, qw)`` + ``(7,)`` or a homogeneous ``(4, 4)`` matrix, expressed in the cuRobo world/base frame (the same frame static collision YAMLs are authored in). representation: ``"cuboid"`` (local-frame AABB -> OBB via ``pose``, @@ -441,13 +442,16 @@ def _mesh_to_obstacle_entry( pose = torch.as_tensor(pose, dtype=torch.float32).detach().to("cpu") if pose.shape == (4, 4): position = pose[:3, 3] - quaternion = quat_from_matrix(pose[:3, :3]) # wxyz + quaternion = quat_from_matrix(pose[:3, :3]) pose = torch.cat([position, quaternion]) if pose.shape != (7,): raise ValueError( - f"pose must be (7,) [x,y,z,qw,qx,qy,qz] or (4, 4), got {tuple(pose.shape)}." + f"pose must be (7,) [x,y,z,qx,qy,qz,qw] or (4, 4), got {tuple(pose.shape)}." ) + # cuRobo world YAML stores 7D poses as xyz+wxyz. + curobo_pose = torch.cat([pose[:3], convert_quat(pose[3:7], to="wxyz")]) + if representation == "mesh": if vertices.numel() == 0 or faces.numel() == 0: raise ValueError( @@ -460,7 +464,7 @@ def _mesh_to_obstacle_entry( { "vertices": vertices.tolist(), "faces": faces.reshape(-1).to(torch.int64).tolist(), - "pose": pose.tolist(), + "pose": curobo_pose.tolist(), }, ) ] @@ -476,9 +480,9 @@ def _mesh_to_obstacle_entry( vmax = vertices.amax(dim=0) dims = vmax - vmin center_local = (vmin + vmax) / 2.0 - rotation = matrix_from_quat(pose[3:7]) # (3, 3), wxyz + rotation = matrix_from_quat(pose[3:7]) center_world = rotation @ center_local + pose[:3] - cuboid_pose = torch.cat([center_world, pose[3:7]]) + cuboid_pose = torch.cat([center_world, curobo_pose[3:7]]) return [("cuboid", name, {"dims": dims.tolist(), "pose": cuboid_pose.tolist()})] # representation == "sphere": fit spheres in the local frame, then transform diff --git a/embodichain/lab/sim/motion/planners/neural_planner.py b/embodichain/lab/sim/motion/planners/neural_planner.py index 8de69684e..f9db990f0 100644 --- a/embodichain/lab/sim/motion/planners/neural_planner.py +++ b/embodichain/lab/sim/motion/planners/neural_planner.py @@ -31,7 +31,7 @@ ) from embodichain.lab.sim.motion.planners.utils import MoveType, PlanResult, PlanState from embodichain.utils import configclass, logger -from embodichain.utils.math import convert_quat, quat_error_magnitude, quat_from_matrix +from embodichain.utils.math import quat_error_magnitude, quat_from_matrix __all__ = [ "NeuralPlanner", @@ -508,9 +508,7 @@ def _parse_waypoints( if xpos.dim() == 2: xpos = xpos.unsqueeze(0) waypoint_pos[:, idx] = xpos[:, :3, 3] - waypoint_quat[:, idx] = convert_quat( - quat_from_matrix(xpos[:, :3, :3]), to="xyzw" - ) + waypoint_quat[:, idx] = quat_from_matrix(xpos[:, :3, :3]) valid_mask[:, idx] = 1.0 return waypoint_pos, waypoint_quat, valid_mask, len(target_states) @@ -536,9 +534,7 @@ def _fk_matrix(self, qpos: torch.Tensor, control_part: str) -> torch.Tensor: def _fk_pose_xyzw(self, qpos: torch.Tensor, control_part: str) -> torch.Tensor: fk = self.robot.compute_fk(qpos=qpos, name=control_part, to_matrix=False) - pos = fk[:, :3] - quat_xyzw = convert_quat(fk[:, 3:7], to="xyzw") - return torch.cat([pos, quat_xyzw], dim=-1) + return fk def _build_obs( self, @@ -589,11 +585,9 @@ def _is_active_reached( idx = torch.arange(b, device=self.device) active_idx_clamped = torch.clamp(active_idx, max=self._num_waypoints - 1) active_pos = waypoint_pos[idx, active_idx_clamped] - active_quat_xyzw = waypoint_quat[idx, active_idx_clamped] + active_quat = waypoint_quat[idx, active_idx_clamped] pos_dist = (ee_pose[:, :3] - active_pos).norm(dim=-1) - ee_quat_wxyz = convert_quat(ee_pose[:, 3:7], to="wxyz") - active_quat_wxyz = convert_quat(active_quat_xyzw, to="wxyz") - rot_dist = quat_error_magnitude(ee_quat_wxyz, active_quat_wxyz) + rot_dist = quat_error_magnitude(ee_pose[:, 3:7], active_quat) orientation_required = self._intermediate_orientation | ( active_idx >= episode_k - 1 ) diff --git a/embodichain/lab/sim/motion/planners/toppra_planner.py b/embodichain/lab/sim/motion/planners/toppra_planner.py index 1b7783fc1..fd7d5c717 100644 --- a/embodichain/lab/sim/motion/planners/toppra_planner.py +++ b/embodichain/lab/sim/motion/planners/toppra_planner.py @@ -250,7 +250,7 @@ class ToppraPlannerCfg(BasePlannerCfg): clears the inherited atexit registry and installs ``prctl(PR_SET_PDEATHSIG)`` so workers are reaped when the parent dies (incl. the ``os._exit`` path). ``'spawn'`` is the safer choice when the parent has initialized CUDA - physics (``sim_device='cuda'``) — fork-after-CUDA-init is the officially + physics (``device='cuda'``) — fork-after-CUDA-init is the officially unsupported case — or if fork deadlocks are observed, at the cost of re-importing modules per worker. """ diff --git a/embodichain/lab/sim/motion/solvers/differential_solver.py b/embodichain/lab/sim/motion/solvers/differential_solver.py index f3925be4b..2df46023d 100644 --- a/embodichain/lab/sim/motion/solvers/differential_solver.py +++ b/embodichain/lab/sim/motion/solvers/differential_solver.py @@ -139,7 +139,7 @@ def action_dim(self) -> int: elif self.cfg.command_type == "pose" and self.cfg.use_relative_mode: return 6 # (dx, dy, dz, droll, dpitch, dyaw) else: - return 7 # (x, y, z, qw, qx, qy, qz) + return 7 # (x, y, z, qx, qy, qz, qw) def reset(self, env_ids: torch.Tensor | None = None): """Reset the internal buffers for the specified environments. @@ -151,7 +151,7 @@ def reset(self, env_ids: torch.Tensor | None = None): env_ids = torch.arange(self.num_envs, device=self.device) self.ee_pos_des[env_ids] = 0 - self.ee_quat_des[env_ids] = torch.tensor([1.0, 0, 0, 0], device=self.device) + self.ee_quat_des[env_ids] = torch.tensor([0.0, 0, 0, 1.0], device=self.device) self._command[env_ids] = 0 def set_command( @@ -412,9 +412,8 @@ def _matrix_to_pos_quat(mat): rot_matrices = mat[:, :3, :3].cpu().numpy() # Convert to NumPy for scipy quats = Rotation.from_matrix(rot_matrices).as_quat() # (N, 4), [x, y, z, w] - # Convert quaternion back to torch.Tensor and reorder to [w, x, y, z] + # SciPy's xyzw convention matches EmbodiChain's quaternion contract. quats = torch.tensor(quats, device=mat.device, dtype=mat.dtype) # (N, 4) - quats = quats[:, [3, 0, 1, 2]] # Reorder to [w, x, y, z] # Concatenate position and quaternion return torch.cat([pos, quats], dim=1) diff --git a/embodichain/lab/sim/motion/solvers/neural_ik_solver.py b/embodichain/lab/sim/motion/solvers/neural_ik_solver.py index 56159df1a..e0c9957ba 100644 --- a/embodichain/lab/sim/motion/solvers/neural_ik_solver.py +++ b/embodichain/lab/sim/motion/solvers/neural_ik_solver.py @@ -194,7 +194,7 @@ def _run_policy( for _ in range(self._max_steps): ee_xpos = self.get_fk(qpos) ee_pos = ee_xpos[:, :3, 3] - ee_quat = convert_quat(quat_from_matrix(ee_xpos[:, :3, :3]), to="xyzw") + ee_quat = quat_from_matrix(ee_xpos[:, :3, :3]) obs = self._build_obs( qpos, ee_pos, ee_quat, target_pos, target_quat, last_action @@ -212,9 +212,9 @@ def _run_policy( # Convergence check ik_xpos = self.get_fk(qpos) pos_err = (ik_xpos[:, :3, 3] - target_pos).norm(dim=-1) - ik_quat_wxyz = quat_from_matrix(ik_xpos[:, :3, :3]) - target_quat_wxyz = quat_from_matrix(target_xpos[:, :3, :3]) - rot_err = quat_error_magnitude(target_quat_wxyz, ik_quat_wxyz) + ik_quat_xyzw = quat_from_matrix(ik_xpos[:, :3, :3]) + target_quat_xyzw = quat_from_matrix(target_xpos[:, :3, :3]) + rot_err = quat_error_magnitude(target_quat_xyzw, ik_quat_xyzw) success = (pos_err < self._pos_eps) & (rot_err < self._rot_eps) return success, qpos @@ -253,7 +253,7 @@ def get_ik( B = target_xpos.shape[0] target_pos = target_xpos[:, :3, 3] - target_quat = convert_quat(quat_from_matrix(target_xpos[:, :3, :3]), to="xyzw") + target_quat = quat_from_matrix(target_xpos[:, :3, :3]) if qpos_seed is None: qpos_seed = torch.zeros(B, self.dof, device=self.device) @@ -279,9 +279,7 @@ def get_ik( ) target_xpos_repeated = sampler.repeat_target_xpos(target_xpos, n) target_pos_rep = target_xpos_repeated[:, :3, 3] - target_quat_rep = convert_quat( - quat_from_matrix(target_xpos_repeated[:, :3, :3]), to="xyzw" - ) + target_quat_rep = quat_from_matrix(target_xpos_repeated[:, :3, :3]) success_flat, ik_qpos_flat = self._run_policy( all_seeds, target_xpos_repeated, target_pos_rep, target_quat_rep diff --git a/embodichain/lab/sim/objects/__init__.py b/embodichain/lab/sim/objects/__init__.py index a6220b1d3..1695b223b 100644 --- a/embodichain/lab/sim/objects/__init__.py +++ b/embodichain/lab/sim/objects/__init__.py @@ -28,8 +28,17 @@ RigidBodyGroupData, RigidObjectGroupCfg, ) -from .soft_object import SoftObject, SoftBodyData, SoftObjectCfg -from .cloth_object import ClothObject, ClothBodyData, ClothObjectCfg +from .deformable import ( + DeformableObject, + DeformableObjectData, + SurfaceDeformableObject, + VolumeDeformableObject, +) +from ..cfg import ( + DeformableObjectCfg, + SurfaceDeformableObjectCfg, + VolumeDeformableObjectCfg, +) from .articulation import ( Articulation, ArticulationData, diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 88f70be7c..c305a7f95 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -22,18 +22,17 @@ import dexsim import numpy as np +from copy import deepcopy from dataclasses import dataclass from functools import cached_property -from typing import List, Sequence, Dict, Union, Tuple, Optional +from typing import TYPE_CHECKING, List, Sequence, Dict, Union, Tuple, Optional -from dexsim.engine import Articulation as _Articulation +from dexsim.scene import Scene from dexsim.types import ( ArticulationFlag, - ArticulationGPUAPIWriteType, - ArticulationGPUAPIReadType, DriveType, ) -from dexsim.engine import CudaArray, MaterialInst, PhysicsScene +from dexsim.engine import MaterialInst from embodichain.lab.sim import VisualMaterialInst, VisualMaterial, ReuseSegmentState from embodichain.lab.sim.material import ( @@ -43,10 +42,10 @@ _wrap_first_render_material, ) from embodichain.lab.sim.cfg import ( + _normalize_joint_target_mode, ArticulationCfg, JointDrivePropertiesCfg, - RigidBodyAttributesCfg, - RigidBodyAttributesOverrideCfg, + RigidBodyPhysicsCfg, ) from dexsim.types import PhysicalAttr from embodichain.utils.string import ( @@ -54,15 +53,23 @@ resolve_matching_names_values, ) from embodichain.lab.sim.common import BatchEntity +from embodichain.lab.sim.physics.newton import is_newton_gradient_mode +from embodichain.lab.sim.objects.backends import ( + SceneArticulationView, +) +from embodichain.lab.sim.objects.backends.base import ArticulationViewBase +from embodichain.lab.sim.objects.backends.newton import ( + _configure_newton_mimic_compliance, +) from embodichain.utils.math import ( + convert_quat, matrix_from_quat, quat_from_matrix, - convert_quat, matrix_from_euler, ) from embodichain.lab.sim.utility.sim_utils import ( + _apply_default_articulation_root_properties, get_dexsim_drive_type, - set_dexsim_articulation_cfg, ) from embodichain.lab.sim.utility.solver_utils import ( create_pk_chain, @@ -70,6 +77,19 @@ ) from embodichain.utils import logger +if TYPE_CHECKING: + from dexsim.scene import SpawnedArticulation + + +@dataclass(frozen=True, slots=True) +class _MimicInfo: + """Mimic metadata expressed in the backing state-buffer index domain.""" + + mimic_id: np.ndarray + mimic_parent: np.ndarray + mimic_multiplier: np.ndarray + mimic_offset: np.ndarray + @dataclass(frozen=True, slots=True, eq=False) class ArticulationJointKinematics: @@ -129,38 +149,37 @@ def __post_init__(self) -> None: @dataclass class ArticulationData: - """GPU data manager for articulation.""" + """Scene-batch data manager for articulations.""" def __init__( - self, entities: List[_Articulation], ps: PhysicsScene, device: torch.device + self, + entities: Sequence[SpawnedArticulation], + scene: Scene, + device: torch.device, ) -> None: """Initialize the ArticulationData. Args: - entities (List[_Articulation]): List of DexSim Articulation objects. - ps (PhysicsScene): The physics scene. - device (torch.device): The device to use for the articulation data. + entities: Articulation handles owned by ``scene``. + scene: Finalized DexSim Scene. + device: Device to use for the articulation data. """ + if not isinstance(scene, Scene): + raise TypeError("ArticulationData requires a finalized DexSim Scene.") self.entities = entities - self.ps = ps + self.scene = scene self.num_instances = len(entities) self.device = device - - # get gpu indices for the entities. - # only meaningful when using GPU physics. - self.gpu_indices = ( - torch.as_tensor( - [entity.get_gpu_index() for entity in self.entities], - dtype=torch.int32, - device=self.device, - ) - if self.device.type == "cuda" - else None + self.articulation_view: ArticulationViewBase = ( + SceneArticulationView.from_entities(scene, entities, device) ) - self.dof = self.entities[0].get_dof() - self.num_links = self.entities[0].get_links_num() - self.link_names = self.entities[0].get_link_names() + # Backward-compatible alias for callers that use GPU/articulation ids. + self.gpu_indices = self.articulation_view.articulation_ids_tensor + + self.dof = self.articulation_view.dof + self.num_links = self.articulation_view.num_links + self.link_names = self.articulation_view.link_names self._root_pose = torch.zeros( (self.num_instances, 7), dtype=torch.float32, device=self.device @@ -172,55 +191,67 @@ def __init__( (self.num_instances, 3), dtype=torch.float32, device=self.device ) - max_num_links = ( - self.ps.gpu_get_articulation_max_link_count() - if self.device.type == "cuda" - else self.num_links - ) self._body_link_pose = torch.zeros( - (self.num_instances, max_num_links, 7), + (self.num_instances, self.num_links, 7), dtype=torch.float32, device=self.device, ) self._body_link_vel = torch.zeros( - (self.num_instances, max_num_links, 6), + (self.num_instances, self.num_links, 6), dtype=torch.float32, device=self.device, ) self._body_link_lin_vel = torch.zeros( - (self.num_instances, max_num_links, 3), + (self.num_instances, self.num_links, 3), dtype=torch.float32, device=self.device, ) self._body_link_ang_vel = torch.zeros( - (self.num_instances, max_num_links, 3), + (self.num_instances, self.num_links, 3), dtype=torch.float32, device=self.device, ) - max_dof = ( - self.ps.gpu_get_articulation_max_dof() - if self.device.type == "cuda" - else self.dof + # Current link mass-property buffers use the public articulation link + # ordering. Initialization snapshots are captured after backend + # materialization and remain unchanged by runtime writes. + self._mass = torch.zeros( + (self.num_instances, self.num_links), + dtype=torch.float32, + device=self.device, + ) + self._inertia = torch.zeros( + (self.num_instances, self.num_links, 3), + dtype=torch.float32, + device=self.device, ) + self._com_pose = torch.zeros( + (self.num_instances, self.num_links, 7), + dtype=torch.float32, + device=self.device, + ) + self._default_mass: torch.Tensor | None = None + self._default_inertia: torch.Tensor | None = None + self._default_com_pose: torch.Tensor | None = None + self._target_qpos = torch.zeros( - (self.num_instances, max_dof), dtype=torch.float32, device=self.device + (self.num_instances, self.dof), dtype=torch.float32, device=self.device ) self._qpos = torch.zeros( - (self.num_instances, max_dof), dtype=torch.float32, device=self.device + (self.num_instances, self.dof), dtype=torch.float32, device=self.device ) self._target_qvel = torch.zeros( - (self.num_instances, max_dof), dtype=torch.float32, device=self.device + (self.num_instances, self.dof), dtype=torch.float32, device=self.device ) self._qvel = torch.zeros( - (self.num_instances, max_dof), dtype=torch.float32, device=self.device + (self.num_instances, self.dof), dtype=torch.float32, device=self.device ) self._qacc = torch.zeros( - (self.num_instances, max_dof), dtype=torch.float32, device=self.device + (self.num_instances, self.dof), dtype=torch.float32, device=self.device ) self._qf = torch.zeros( - (self.num_instances, max_dof), dtype=torch.float32, device=self.device + (self.num_instances, self.dof), dtype=torch.float32, device=self.device ) self._qpos_limits = torch.as_tensor( np.array([entity.get_joint_position_limits() for entity in self.entities]), @@ -238,31 +269,23 @@ def __init__( device=self.device, ) + @property + def is_newton_backend(self) -> bool: + return self.articulation_view.is_newton_backend + + @property + def is_ready(self) -> bool: + return self.articulation_view.is_ready + @property def root_pose(self) -> torch.Tensor: """Get the root pose of the articulation. Returns: - torch.Tensor: The root pose of the articulation with shape of (num_instances, 7). + torch.Tensor: Root poses with shape ``(num_instances, 7)`` in + ``(x, y, z, qx, qy, qz, qw)`` order. """ - if self.device.type == "cpu": - # Fetch pose from CPU entities - root_pose = torch.as_tensor( - np.array([entity.get_local_pose() for entity in self.entities]), - dtype=torch.float32, - device=self.device, - ) - xyzs = root_pose[:, :3, 3] - quats = quat_from_matrix(root_pose[:, :3, :3]) - return torch.cat((xyzs, quats), dim=-1) - else: - self.ps.gpu_fetch_root_data( - data=self._root_pose, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.ROOT_GLOBAL_POSE, - ) - self._root_pose[:, :4] = convert_quat(self._root_pose[:, :4], to="wxyz") - return self._root_pose[:, [4, 5, 6, 0, 1, 2, 3]] + return self.articulation_view.fetch_root_pose(self._root_pose) @property def root_lin_vel(self) -> torch.Tensor: @@ -271,22 +294,7 @@ def root_lin_vel(self) -> torch.Tensor: Returns: torch.Tensor: The linear velocity of the root link with shape of (num_instances, 3). """ - if self.device.type == "cpu": - # Fetch linear velocity from CPU entities - return torch.as_tensor( - np.array( - [entity.get_root_link_velocity()[:3] for entity in self.entities] - ), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_root_data( - data=self._root_lin_vel, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.ROOT_LINEAR_VELOCITY, - ) - return self._root_lin_vel.clone() + return self.articulation_view.fetch_root_linear_velocity(self._root_lin_vel) @property def root_ang_vel(self) -> torch.Tensor: @@ -295,22 +303,7 @@ def root_ang_vel(self) -> torch.Tensor: Returns: torch.Tensor: The angular velocity of the root link with shape of (num_instances, 3). """ - if self.device.type == "cpu": - # Fetch angular velocity from CPU entities - return torch.as_tensor( - np.array( - [entity.get_root_link_velocity()[3:] for entity in self.entities] - ), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_root_data( - data=self._root_ang_vel, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.ROOT_ANGULAR_VELOCITY, - ) - return self._root_ang_vel.clone() + return self.articulation_view.fetch_root_angular_velocity(self._root_ang_vel) @property def root_vel(self) -> torch.Tensor: @@ -328,22 +321,7 @@ def qpos(self) -> torch.Tensor: Returns: torch.Tensor: The current positions of the articulation with shape of (num_instances, dof). """ - if self.device.type == "cpu": - # Fetch qpos from CPU entities - return torch.as_tensor( - np.array( - [entity.get_current_qpos() for entity in self.entities], - ), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_joint_data( - data=self._qpos, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.JOINT_POSITION, - ) - return self._qpos[:, : self.dof].clone() + return self.articulation_view.fetch_qpos(self._qpos) @property def target_qpos(self) -> torch.Tensor: @@ -352,22 +330,7 @@ def target_qpos(self) -> torch.Tensor: Returns: torch.Tensor: The target positions of the articulation with shape of (num_instances, dof). """ - if self.device.type == "cpu": - # Fetch target_qpos from CPU entities - return torch.as_tensor( - np.array( - [entity.get_target_qpos() for entity in self.entities], - ), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_joint_data( - data=self._target_qpos, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.JOINT_TARGET_POSITION, - ) - return self._target_qpos[:, : self.dof].clone() + return self.articulation_view.fetch_target_qpos(self._target_qpos) @property def qvel(self) -> torch.Tensor: @@ -376,20 +339,7 @@ def qvel(self) -> torch.Tensor: Returns: torch.Tensor: The current velocities of the articulation with shape of (num_instances, dof). """ - if self.device.type == "cpu": - # Fetch qvel from CPU entities - return torch.as_tensor( - np.array([entity.get_current_qvel() for entity in self.entities]), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_joint_data( - data=self._qvel, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.JOINT_VELOCITY, - ) - return self._qvel[:, : self.dof].clone() + return self.articulation_view.fetch_qvel(self._qvel) @property def target_qvel(self) -> torch.Tensor: @@ -397,22 +347,7 @@ def target_qvel(self) -> torch.Tensor: Returns: torch.Tensor: The target velocities of the articulation with shape of (num_instances, dof). """ - if self.device.type == "cpu": - # Fetch target_qvel from CPU entities - return torch.as_tensor( - np.array( - [entity.get_target_qvel() for entity in self.entities], - ), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_joint_data( - data=self._target_qvel, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.JOINT_TARGET_VELOCITY, - ) - return self._target_qvel[:, : self.dof].clone() + return self.articulation_view.fetch_target_qvel(self._target_qvel) @property def qacc(self) -> torch.Tensor: @@ -421,20 +356,7 @@ def qacc(self) -> torch.Tensor: Returns: torch.Tensor: The current accelerations of the articulation with shape of (num_instances, dof). """ - if self.device.type == "cpu": - # Fetch qacc from CPU entities - return torch.as_tensor( - np.array([entity.get_current_qacc() for entity in self.entities]), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_joint_data( - data=self._qacc, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.JOINT_ACCELERATION, - ) - return self._qacc[:, : self.dof].clone() + return self.articulation_view.fetch_qacc(self._qacc) @property def qf(self) -> torch.Tensor: @@ -443,56 +365,17 @@ def qf(self) -> torch.Tensor: Returns: torch.Tensor: The current forces of the articulation with shape of (num_instances, dof). """ - if self.device.type == "cpu": - # Fetch qf from CPU entities - return torch.as_tensor( - np.array([entity.get_current_qf() for entity in self.entities]), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_joint_data( - data=self._qf, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.JOINT_FORCE, - ) - return self._qf[:, : self.dof].clone() + return self.articulation_view.fetch_qf(self._qf) @property def body_link_pose(self) -> torch.Tensor: """Get the pose of all links in the articulation. Returns: - torch.Tensor: The poses of the links in the articulation with shape (N, num_links, 7). + torch.Tensor: Link poses with shape ``(N, num_links, 7)`` in + ``(x, y, z, qx, qy, qz, qw)`` order. """ - if self.device.type == "cpu": - from embodichain.lab.sim.utility import get_dexsim_arenas - - arenas = get_dexsim_arenas() - for j, entity in enumerate(self.entities): - - link_pose = np.zeros((self.num_links, 4, 4), dtype=np.float32) - for i, link_name in enumerate(self.link_names): - pose = entity.get_link_pose(link_name) - arena_pose = arenas[j].get_root_node().get_local_pose() - pose[:2, 3] -= arena_pose[:2, 3] - link_pose[i] = pose - - link_pose = torch.from_numpy(link_pose) - xyz = link_pose[:, :3, 3] - quat = quat_from_matrix(link_pose[:, :3, :3]) - self._body_link_pose[j][: self.num_links, :] = torch.cat( - (xyz, quat), dim=-1 - ) - return self._body_link_pose[:, : self.num_links, :] - else: - self.ps.gpu_fetch_link_data( - data=self._body_link_pose, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.LINK_GLOBAL_POSE, - ) - quat = convert_quat(self._body_link_pose[..., :4], to="wxyz") - return torch.cat((self._body_link_pose[..., 4:], quat), dim=-1) + return self.articulation_view.fetch_link_pose(self._body_link_pose) @property def body_link_vel(self) -> torch.Tensor: @@ -501,26 +384,161 @@ def body_link_vel(self) -> torch.Tensor: Returns: torch.Tensor: The poses of the links in the articulation with shape (N, num_links, 6). """ - if self.device.type == "cpu": - for i, entity in enumerate(self.entities): - self._body_link_vel[i][: self.num_links] = torch.from_numpy( - entity.get_link_general_velocities() + return self.articulation_view.fetch_link_velocity( + self._body_link_vel, + self._body_link_lin_vel, + self._body_link_ang_vel, + ) + + def _entity_drive_properties(self, entity: object) -> tuple[object, ...]: + """Read drive values without conflating backend target semantics.""" + if ( + isinstance(self.articulation_view, SceneArticulationView) + and self.is_newton_backend + ): + return tuple(entity.get_newton_drive()) + return tuple(entity.get_drive()) + + def _entity_link_properties(self, entity: object, link_name: str) -> object: + """Read native mass properties through the active backend contract.""" + if ( + isinstance(self.articulation_view, SceneArticulationView) + and self.is_newton_backend + ): + return entity.get_newton_link_properties(link_name) + return entity.get_physical_attr(link_name) + + def read_physical_properties( + self, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Refresh current mass, inertia diagonal, and local COM pose buffers. + + COM poses use the EmbodiChain convention ``xyz + xyzw`` and all + tensors use the public link ordering. DexSim physical-property + descriptors use ``wxyz`` and are converted at this boundary. + """ + masses: list[list[float]] = [] + inertias: list[list[np.ndarray]] = [] + com_poses: list[list[np.ndarray]] = [] + for entity in self.entities: + mass_row: list[float] = [] + inertia_row: list[np.ndarray] = [] + com_row: list[np.ndarray] = [] + for link_name in self.link_names: + attr = self._entity_link_properties(entity, link_name) + mass_row.append(float(attr.mass)) + inertia_row.append(np.asarray(attr.inertia, dtype=np.float32)) + com_row.append( + np.concatenate( + ( + np.asarray(attr.com_position, dtype=np.float32), + convert_quat( + np.asarray(attr.com_quaternion, dtype=np.float32), + to="xyzw", + ), + ) + ) ) - return self._body_link_vel[:, : self.num_links, :] - else: - self.ps.gpu_fetch_link_data( - data=self._body_link_lin_vel, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.LINK_LINEAR_VELOCITY, + masses.append(mass_row) + inertias.append(inertia_row) + com_poses.append(com_row) + + self._mass.copy_( + torch.as_tensor( + np.asarray(masses, dtype=np.float32), + dtype=torch.float32, + device=self.device, ) - self.ps.gpu_fetch_link_data( - data=self._body_link_ang_vel, - gpu_indices=self.gpu_indices, - data_type=ArticulationGPUAPIReadType.LINK_ANGULAR_VELOCITY, + ) + self._inertia.copy_( + torch.as_tensor( + np.asarray(inertias, dtype=np.float32), + dtype=torch.float32, + device=self.device, + ) + ) + self._com_pose.copy_( + torch.as_tensor( + np.asarray(com_poses, dtype=np.float32), + dtype=torch.float32, + device=self.device, + ) + ) + return self._mass, self._inertia, self._com_pose + + @property + def mass(self) -> torch.Tensor: + """Current link masses with shape ``(N, num_links)``.""" + return self.read_physical_properties()[0] + + @property + def inertia(self) -> torch.Tensor: + """Current link inertia diagonals with shape ``(N, num_links, 3)``.""" + return self.read_physical_properties()[1] + + @property + def com_pose(self) -> torch.Tensor: + """Current local link COM poses as ``xyz + xyzw`` tensors.""" + return self.read_physical_properties()[2] + + @property + def default_physical_properties_initialized(self) -> bool: + """Whether initialization-time link mass properties are available.""" + return ( + self._default_mass is not None + and self._default_inertia is not None + and self._default_com_pose is not None + ) + + @property + def default_mass(self) -> torch.Tensor: + """Initialization-time link masses with shape ``(N, num_links)``.""" + if self._default_mass is None: + raise RuntimeError("Default articulation link masses are unavailable.") + return self._default_mass + + @property + def default_inertia(self) -> torch.Tensor: + """Initialization-time link inertia diagonals.""" + if self._default_inertia is None: + raise RuntimeError("Default articulation link inertias are unavailable.") + return self._default_inertia + + @property + def default_com_pose(self) -> torch.Tensor: + """Initialization-time local link COM poses in ``xyz + xyzw`` order.""" + if self._default_com_pose is None: + raise RuntimeError("Default articulation link COM poses are unavailable.") + return self._default_com_pose + + def capture_default_physical_properties( + self, + *, + mass: torch.Tensor, + inertia: torch.Tensor, + com_pose: torch.Tensor, + ) -> None: + """Capture backend-resolved link mass properties exactly once.""" + expected_shapes = { + "mass": (self.num_instances, self.num_links), + "inertia": (self.num_instances, self.num_links, 3), + "com_pose": (self.num_instances, self.num_links, 7), + } + values = {"mass": mass, "inertia": inertia, "com_pose": com_pose} + for name, value in values.items(): + if tuple(value.shape) != expected_shapes[name]: + raise ValueError( + f"Expected {name} shape {expected_shapes[name]}, " + f"got {tuple(value.shape)}." + ) + if self.default_physical_properties_initialized: + raise RuntimeError( + "Default articulation link mass properties are already captured." ) - self._body_link_vel[..., :3] = self._body_link_lin_vel - self._body_link_vel[..., 3:] = self._body_link_ang_vel - return self._body_link_vel[:, : self.num_links, :] + + self._default_mass = mass.to(self.device, dtype=torch.float32).clone() + self._default_inertia = inertia.to(self.device, dtype=torch.float32).clone() + self._default_com_pose = com_pose.to(self.device, dtype=torch.float32).clone() @property def joint_stiffness(self) -> torch.Tensor: @@ -530,7 +548,9 @@ def joint_stiffness(self) -> torch.Tensor: torch.Tensor: The joint stiffness of the articulation with shape (N, dof). """ return torch.as_tensor( - np.array([entity.get_drive()[0] for entity in self.entities]), + np.array( + [self._entity_drive_properties(entity)[0] for entity in self.entities] + ), dtype=torch.float32, device=self.device, ) @@ -543,7 +563,9 @@ def joint_damping(self) -> torch.Tensor: torch.Tensor: The joint damping of the articulation with shape (N, dof). """ return torch.as_tensor( - np.array([entity.get_drive()[1] for entity in self.entities]), + np.array( + [self._entity_drive_properties(entity)[1] for entity in self.entities] + ), dtype=torch.float32, device=self.device, ) @@ -556,7 +578,9 @@ def joint_friction(self) -> torch.Tensor: torch.Tensor: The joint friction of the articulation with shape (N, dof). """ return torch.as_tensor( - np.array([entity.get_drive()[4] for entity in self.entities]), + np.array( + [self._entity_drive_properties(entity)[4] for entity in self.entities] + ), dtype=torch.float32, device=self.device, ) @@ -569,7 +593,9 @@ def joint_armature(self) -> torch.Tensor: torch.Tensor: The joint armature of the articulation with shape (N, dof). """ return torch.as_tensor( - np.array([entity.get_drive()[5] for entity in self.entities]), + np.array( + [self._entity_drive_properties(entity)[5] for entity in self.entities] + ), dtype=torch.float32, device=self.device, ) @@ -632,178 +658,300 @@ class Articulation(BatchEntity): For floating-base articulation, it can be a humanoid, drawer, etc. Args: - cfg (ArticulationCfg): Configuration for the articulation. - entities (List[_Articulation], optional): List of articulation entities. - device (torch.device, optional): Device to use (CPU or CUDA). + cfg: Configuration for the articulation. + device: Device to use (CPU or CUDA). """ def __init__( self, cfg: ArticulationCfg, - entities: List[_Articulation] = None, device: torch.device = torch.device("cpu"), ) -> None: - # Initialize world and physics scene - self._world = dexsim.default_world() - self._ps = self._world.get_physics_scene() + """Create an unregistered articulation facade. - self.cfg = cfg - self._entities = entities + ``SpawnScene`` owns the replicated instance count and the finalized + ``Scene``. It injects them at declaration and binding time instead of + exposing either lifecycle dependency through this constructor. + """ + self._newton_mimic_compliance_configured = False + self._prepared_default_root_topology_revision = -1 + self.cfg = deepcopy(cfg) + self.uid = self.cfg.uid self.device = device + self._entities: list[SpawnedArticulation] = [] + self._declared_num_instances: int | None = None + self._spawn_result: Scene | None = None + self._world = None + self._ps = None + self._data: ArticulationData | None = None + self._all_indices = torch.empty(0, dtype=torch.int32) + self._visual_material: List[Dict[str, VisualMaterialInst]] = [] + self.is_shared_visual_material = False + self._has_collision_visible_node_dict: dict[str, bool] = {} + + def _initialize_spawn_declaration(self, num_instances: int) -> None: + """Initialize instance-dependent declaration state from ``SpawnScene``.""" + if num_instances <= 0: + raise ValueError("A declared Articulation requires num_instances > 0.") + if self._declared_num_instances is not None: + if self._declared_num_instances != num_instances: + raise RuntimeError( + f"Articulation {self.uid!r} is already declared for " + f"{self._declared_num_instances} instances." + ) + return - # Store all indices for batch operations - self._all_indices = torch.arange(len(entities), dtype=torch.int32) - - # Apply gravity before the first physics update. Unlike asset-backed - # physical properties, this config field is an explicit runtime flag. - self.set_gravity(self.cfg.enable_gravity) + self._declared_num_instances = num_instances + self._all_indices = torch.arange(num_instances, dtype=torch.int32) + self._visual_material = [{} for _ in range(num_instances)] - if device.type == "cuda": - self._world.update(0.001) + def _require_declared_num_instances(self) -> int: + """Return the Spawn-provided instance count or raise a lifecycle error.""" + if self._declared_num_instances is None: + raise RuntimeError( + f"Articulation {self.uid!r} must be registered through SpawnScene " + "before it can be used." + ) + return self._declared_num_instances - self._data = ArticulationData(entities=entities, ps=self._ps, device=device) + @property + def is_spawn_bound(self) -> bool: + """Whether this facade is bound to one finalized Scene.""" + return self._spawn_result is not None - self.cfg: ArticulationCfg - if self.cfg.init_qpos is None: - self.cfg.init_qpos = torch.zeros(self.dof, dtype=torch.float32) + @property + def is_declared(self) -> bool: + """Whether this facade is waiting for its Scene binding.""" + return self._world is None - # Get default masses. - self.default_link_masses = self.get_mass() + @property + def num_instances(self) -> int: + if self._entities: + return len(self._entities) + return self._require_declared_num_instances() - # Determine if we should use USD properties or cfg properties. - if not self.cfg.use_usd_properties: - # Set articulation configuration in DexSim - set_dexsim_articulation_cfg(entities, self.cfg) + def attach_spawn_handles( + self, + entities: Sequence[SpawnedArticulation], + ) -> None: + """Store handles and expose metadata without initializing Batch data. - num_entities = len(entities) - dof = self._data.dof - default_cfg = JointDrivePropertiesCfg() - self.default_joint_damping = torch.full( - (num_entities, dof), - default_cfg.damping, - dtype=torch.float32, - device=device, - ) - self.default_joint_stiffness = torch.full( - (num_entities, dof), - default_cfg.stiffness, - dtype=torch.float32, - device=device, - ) - self.default_joint_max_effort = torch.full( - (num_entities, dof), - default_cfg.max_effort, - dtype=torch.float32, - device=device, - ) - self.default_joint_max_velocity = torch.full( - (num_entities, dof), - default_cfg.max_velocity, - dtype=torch.float32, - device=device, - ) - self.default_joint_friction = torch.full( - (num_entities, dof), - default_cfg.friction, - dtype=torch.float32, - device=device, - ) - self.default_joint_armature = torch.full( - (num_entities, dof), - default_cfg.armature, - dtype=torch.float32, - device=device, + This pre-finalize step supports eager Default loading and only reads + articulation metadata. ``bind_spawn()`` performs result-dependent + Batch/Data initialization after finalization. + """ + handles = list(entities) + expected = self._require_declared_num_instances() + if len(handles) != expected: + raise ValueError( + f"Articulation {self.uid!r} expected " + f"{expected} Spawn handles, got {len(handles)}." ) - self._set_default_joint_drive() - else: - # Read current properties from USD-loaded entities - self.default_joint_stiffness = self._data.joint_stiffness.clone() - self.default_joint_damping = self._data.joint_damping.clone() - self.default_joint_friction = self._data.joint_friction.clone() - self.default_joint_armature = self._data.joint_armature.clone() - self.default_joint_max_effort = self._data.qf_limits.clone() - self.default_joint_max_velocity = self._data.qvel_limits.clone() + self._entities = handles + self._mimic_info = self._state_mimic_info() + self.active_joint_ids = [ + index for index in range(self.dof) if index not in self.mimic_ids + ] - # Write the USD properties back to cfg - usd_drive_pros = self.cfg.drive_pros - usd_drive_pros.stiffness = ( - self.default_joint_stiffness[0].cpu().numpy().tolist() - ) - usd_drive_pros.damping = ( - self.default_joint_damping[0].cpu().numpy().tolist() - ) - usd_drive_pros.friction = ( - self.default_joint_friction[0].cpu().numpy().tolist() - ) - usd_drive_pros.armature = ( - self.default_joint_armature[0].cpu().numpy().tolist() - ) - usd_drive_pros.max_effort = ( - self.default_joint_max_effort[0].cpu().numpy().tolist() + def _clear_spawn_cached_properties(self) -> None: + """Drop metadata cached against declaration-time handles.""" + for name in ( + "dof", + "active_dof", + "num_links", + "link_names", + "user_ids", + "root_link_name", + "joint_names", + "active_joint_names", + "all_joint_names", + ): + self.__dict__.pop(name, None) + + def _initialize_spawn_bound(self, result: Scene) -> None: + """Create result-dependent runtime state on this declared facade.""" + if not isinstance(result, Scene): + raise TypeError( + "Articulation binding requires a finalized DexSim Scene; use " + "SimulationManager.prepare()." ) - usd_drive_pros.max_velocity = ( - self.default_joint_max_velocity[0].cpu().numpy().tolist() + + entities = list(self._entities) + expected = self._require_declared_num_instances() + if len(entities) != expected: + raise ValueError( + f"Articulation {self.uid!r} expected {expected} Spawn handles, " + f"got {len(entities)}." ) - # Apply configured qpos limits if provided. This replaces the asset - # limits as the baseline and allows expanding the allowed range. - if self.cfg.qpos_limits is not None: - if isinstance(self.cfg.qpos_limits, dict): - indices, _, values = resolve_matching_names_values( - self.cfg.qpos_limits, self.joint_names - ) - local_joint_ids = torch.as_tensor( - indices, dtype=torch.long, device=self.device - ) - values_tensor = torch.as_tensor( - values, dtype=torch.float32, device=self.device - ).unsqueeze(0) - values_tensor = values_tensor.expand(self.num_instances, -1, -1) - self.set_qpos_limits(values_tensor, joint_ids=local_joint_ids) - else: - qpos_limits = torch.as_tensor( - self.cfg.qpos_limits, dtype=torch.float32, device=self.device - ) - if qpos_limits.dim() == 2: - qpos_limits = qpos_limits.unsqueeze(0).expand( - self.num_instances, -1, -1 - ) - self.set_qpos_limits(qpos_limits) + cfg = deepcopy(self.cfg) + self._clear_spawn_cached_properties() + self._newton_mimic_compliance_configured = False + self._spawn_result = result + self._world = result.world + self._ps = None + self.cfg = cfg + self._entities = entities + self._all_indices = torch.arange(len(entities), dtype=torch.int32) + self._data = ArticulationData( + entities=entities, + scene=result, + device=self.device, + ) + + if self.cfg.init_qpos is None: + self.cfg.init_qpos = torch.zeros(self.dof, dtype=torch.float32) + + self._capture_default_physical_properties() + self.default_joint_stiffness = self._data.joint_stiffness.clone() + self.default_joint_damping = self._data.joint_damping.clone() + self.default_joint_friction = self._data.joint_friction.clone() + self.default_joint_armature = self._data.joint_armature.clone() + self.default_joint_max_effort = self._data.qf_limits.clone() + self.default_joint_max_velocity = self._data.qvel_limits.clone() + is_usd_source = str(self.cfg.fpath).lower().endswith((".usd", ".usda", ".usdc")) self.pk_chain = None - if self.cfg.build_pk_chain: + if self.cfg.build_pk_chain and not is_usd_source: self.pk_chain = create_pk_chain( urdf_path=self.cfg.fpath, device=self.device ) + elif self.cfg.build_pk_chain: + logger.log_warning( + f"Articulation {self.uid!r} uses USD for simulation; skipping " + "the URDF-only pk_chain. Configure a solver with its matching " + "URDF when kinematics are required." + ) - # For rendering purposes, each articulation can have multiple material instances associated with its links. - self._visual_material: List[Dict[str, VisualMaterialInst]] = [ - {} for _ in range(len(entities)) - ] + self._visual_material = [{} for _ in range(len(entities))] self.is_shared_visual_material = False + self._mimic_info = self._state_mimic_info() + self.active_joint_ids = [i for i in range(self.dof) if i not in self.mimic_ids] - # Stores mimic information for joints. - self._mimic_info = entities[0].get_mimic_info() + super().__init__(cfg, entities, self.device) + self._initialize_existing_visual_material() + self._has_collision_visible_node_dict = { + link_name: False for link_name in self.link_names + } + self._initialize_spawn_bound_extension() - self.active_joint_ids = [i for i in range(self.dof) if i not in self.mimic_ids] + def _initialize_spawn_bound_extension(self) -> None: + """Initialize subclass state after the Scene batch becomes available.""" - # TODO: very weird that we must call update here to make sure the GPU indices are valid. - if device.type == "cuda": - self._world.update(0.001) + def bind_spawn( + self, + result: Scene, + ) -> None: + """Initialize this declared facade from Spawn articulation handles.""" + if self.is_spawn_bound: + raise RuntimeError(f"Articulation {self.uid!r} is already Spawn-bound.") + if not self.is_declared: + raise RuntimeError( + f"Articulation {self.uid!r} was not created as a Spawn declaration." + ) + + declared_state = self.__dict__.copy() + try: + self._initialize_spawn_bound(result) + self._apply_spawn_config() + if is_newton_gradient_mode(result): + initial_qpos = torch.as_tensor(self.cfg.init_qpos).reshape(-1) + if initial_qpos.numel() != self.dof: + raise ValueError( + f"Articulation {self.uid!r} expected {self.dof} initial " + f"joint positions, got {initial_qpos.numel()}." + ) + if torch.any(initial_qpos != 0.0): + raise NotImplementedError( + "Newton gradient mode cannot apply non-zero init_qpos after " + "Spawn finalization. Author the initial coordinates in the " + "source asset or initialize them in a differentiable task " + "before opening a Warp tape." + ) + # Spawn already authored the root pose and zero joint/dynamics + # state during model construction. Its Batch mutation APIs are + # intentionally fenced once the model requires gradients. + else: + self.reset() + except Exception: + self.__dict__.clear() + self.__dict__.update(declared_state) + raise + + def _apply_spawn_config(self) -> None: + """Apply configuration that requires finalized backend resources. + + Link physics and joint-drive regex selection is resolved by + EmbodiChain against the source descriptor before finalization. Default + articulation-root properties are normally handled by the pre-runtime + hook; calling it here keeps direct facade binding safe. Render + operations also require materialized native resources. + """ + spawn_result = getattr(self, "_spawn_result", None) + self._prepare_spawn_runtime_config(spawn_result) + + self._newton_mimic_compliance_configured = _configure_newton_mimic_compliance( + result=spawn_result, + entities=self._entities, + state_joint_names=self._state_joint_names(), + mimic_ids=self.mimic_ids, + mimic_parents=self.mimic_parents, + ) - super().__init__(cfg, entities, device) + if not self.cfg.compute_uv: + return - self._initialize_existing_visual_material() + for entity in self._entities: + for link_name in self.link_names: + render_body = entity.get_render_body(link_name) + if render_body is not None: + render_body.set_projective_uv() - # set default collision filter - self._set_default_collision_filter() + def _prepare_spawn_runtime_config(self, result: Scene | None) -> None: + """Apply Default root properties before Direct GPU initialization. - # flag for collision visible node existence - self._has_collision_visible_node_dict = dict() - for link_name in self.link_names: - self._has_collision_visible_node_dict[link_name] = False + PhysX snapshots articulation solver iteration counts when the Direct + GPU runtime is initialized. Applying these values only during facade + binding is too late because ``World.init_gpu_physics()`` has already + performed its warm-up steps. CPU simulation accepts the late write, + which otherwise makes identical hand mimic constraints substantially + softer on CUDA. + """ + if result is None or getattr(result, "backend", None) != "dexsim": + return + + topology_revision = int(result.topology_revision) + if self._prepared_default_root_topology_revision == topology_revision: + return + + root_props = getattr(self.cfg, "root_props", None) + default_root_values_configured = root_props is not None and ( + root_props.sleep_threshold is not None + or root_props.min_position_iters is not None + or root_props.min_velocity_iters is not None + ) + if default_root_values_configured: + for entity in self._entities: + # SpawnedArticulation deliberately fences these setters, while + # its Default-native binding exposes the articulation-root API. + native_articulation = getattr(entity, "_physics_binding", None) + if native_articulation is None: + raise RuntimeError( + "Default Spawn articulation has no native physics binding." + ) + _apply_default_articulation_root_properties( + native_articulation, + root_props, + ) + self._prepared_default_root_topology_revision = topology_revision def __str__(self) -> str: + if self.is_declared: + parent_str = ( + f"{self.__class__}: declared {self.num_instances} Spawn " + f"articulations | uid: {self.uid} | device: {self.device}" + ) + return parent_str parent_str = super().__str__() return parent_str + f" | dof: {self.dof} | num_links: {self.num_links}" @@ -814,7 +962,9 @@ def dof(self) -> int: Returns: int: The degree of freedom of the articulation. """ - return self._data.dof + if self._data is not None: + return self._data.dof + return self._entities[0].get_dof() @cached_property def active_dof(self) -> int: @@ -832,7 +982,9 @@ def num_links(self) -> int: Returns: int: The number of links in the articulation. """ - return self._data.num_links + if self._data is not None: + return self._data.num_links + return len(self._entities[0].get_link_names()) @cached_property def link_names(self) -> List[str]: @@ -841,7 +993,9 @@ def link_names(self) -> List[str]: Returns: List[str]: The names of the links in the articulation. """ - return self._data.link_names + if self._data is not None: + return self._data.link_names + return self._entities[0].get_link_names() @cached_property def user_ids(self) -> torch.Tensor: @@ -872,12 +1026,202 @@ def root_link_name(self) -> str: @cached_property def joint_names(self) -> List[str]: - """Get the names of the joints in the articulation. + """Get active joint names in public qpos-buffer order. Returns: - List[str]: The names of the actived joints in the articulation. + List[str]: Active joint names aligned with qpos, qvel, and qf. + """ + if getattr(self, "_data", None) is not None: + return list(self._data.articulation_view.joint_names) + return self._state_joint_names() + + def _state_joint_names(self) -> List[str]: + """Return active joint names in the backing qpos-buffer order. + + The Scene's Newton batch layout may differ from its source articulation + order. Joint IDs sent to the batch must therefore use the layout + order. :attr:`joint_names` exposes this same order; query the Spawn + handle directly only for source-topology resolution. """ - return self._entities[0].get_actived_joint_names() + if not self._entities: + return [] + entity = self._entities[0] + try: + layout = entity.joint_dof_layout + except (AttributeError, RuntimeError): + return entity.get_actived_joint_names() + return [joint.name for joint in layout] + + def _source_qpos_to_state_order(self, qpos: torch.Tensor) -> torch.Tensor: + """Map source-ordered initial qpos values to the runtime state order.""" + if not self.is_spawn_bound: + return qpos + + source_joint_names = self._entities[0].get_actived_joint_names() + state_joint_names = self._state_joint_names() + if source_joint_names == state_joint_names: + return qpos + + source_indices = {name: index for index, name in enumerate(source_joint_names)} + try: + state_order = [source_indices[name] for name in state_joint_names] + except KeyError as error: + raise RuntimeError( + "Spawn articulation state layout contains a joint absent from " + "the source articulation layout." + ) from error + return qpos[..., state_order] + + def _state_mimic_info(self) -> _MimicInfo: + """Map source-articulation mimic indices to state-buffer indices.""" + entity = self._entities[0] + source_info = entity.get_mimic_info() + source_mimic_ids = np.asarray(source_info.mimic_id, dtype=np.int32).reshape(-1) + source_parent_ids = np.asarray( + source_info.mimic_parent, dtype=np.int32 + ).reshape(-1) + multipliers = np.asarray( + source_info.mimic_multiplier, dtype=np.float32 + ).reshape(-1) + offsets = np.asarray(source_info.mimic_offset, dtype=np.float32).reshape(-1) + relation_count = len(source_mimic_ids) + if not all( + len(values) == relation_count + for values in (source_parent_ids, multipliers, offsets) + ): + raise RuntimeError("Articulation mimic metadata has inconsistent lengths.") + if relation_count == 0: + return _MimicInfo( + mimic_id=source_mimic_ids, + mimic_parent=source_parent_ids, + mimic_multiplier=multipliers, + mimic_offset=offsets, + ) + + source_joint_names = entity.get_actived_joint_names() + try: + state_joint_ids = { + joint.name: int(joint.dof_start) for joint in entity.joint_dof_layout + } + except (AttributeError, RuntimeError): + state_joint_ids = { + name: index for index, name in enumerate(source_joint_names) + } + + try: + mimic_ids = np.asarray( + [ + state_joint_ids[source_joint_names[int(source_id)]] + for source_id in source_mimic_ids + ], + dtype=np.int32, + ) + parent_ids = np.asarray( + [ + state_joint_ids[source_joint_names[int(source_id)]] + for source_id in source_parent_ids + ], + dtype=np.int32, + ) + except (IndexError, KeyError) as error: + raise RuntimeError( + "Articulation mimic metadata references a joint absent from " + "the backing state layout." + ) from error + + return _MimicInfo( + mimic_id=mimic_ids, + mimic_parent=parent_ids, + mimic_multiplier=multipliers, + mimic_offset=offsets, + ) + + def _project_mimic_qpos(self, qpos: torch.Tensor) -> torch.Tensor: + """Return qpos with every mimic child projected from its parent.""" + if not self.mimic_ids: + return qpos + + projected = qpos.clone() + mimic_ids = torch.as_tensor( + self.mimic_ids, dtype=torch.long, device=qpos.device + ) + parent_ids = torch.as_tensor( + self.mimic_parents, dtype=torch.long, device=qpos.device + ) + multipliers = torch.as_tensor( + self.mimic_multipliers, dtype=qpos.dtype, device=qpos.device + ) + offsets = torch.as_tensor( + self.mimic_offsets, dtype=qpos.dtype, device=qpos.device + ) + projected[..., mimic_ids] = projected[..., parent_ids] * multipliers + offsets + return projected + + def _stabilize_newton_mimic_target_write( + self, + values: torch.Tensor, + env_ids: torch.Tensor, + joint_ids: torch.Tensor, + *, + velocity: bool, + ) -> None: + """Update weak follower-drive targets for written mimic leaders. + + The native Newton equality remains the physical coupling. This only + keeps its low-gain follower stabilizer pointed at the same commanded + relation; it never copies measured qpos or qvel into follower state. + """ + if not self._newton_mimic_compliance_configured: + return + + selected_columns = { + int(joint_id): column + for column, joint_id in enumerate(joint_ids.detach().cpu().tolist()) + } + follower_ids: list[int] = [] + follower_targets: list[torch.Tensor] = [] + for child_id, parent_id, multiplier, offset in zip( + self.mimic_ids, + self.mimic_parents, + self.mimic_multipliers, + self.mimic_offsets, + strict=True, + ): + parent_column = selected_columns.get(int(parent_id)) + if parent_column is None: + continue + target = values[:, parent_column] * float(multiplier) + if not velocity: + target = target + float(offset) + follower_ids.append(int(child_id)) + follower_targets.append(target) + + if not follower_ids: + return + + targets = torch.stack(follower_targets, dim=1) + follower_ids_tensor = torch.as_tensor( + follower_ids, dtype=torch.int32, device=self.device + ) + if velocity: + limits = self.body_data.qvel_limits[env_ids][:, follower_ids_tensor] + targets = targets.clamp(-limits, limits) + self._data.articulation_view.apply_qvel( + targets, + env_ids, + follower_ids_tensor, + target=True, + ) + return + + limits = self.body_data.qpos_limits[env_ids][:, follower_ids_tensor, :] + targets = targets.clamp(limits[..., 0], limits[..., 1]) + self._data.articulation_view.apply_qpos( + targets, + env_ids, + follower_ids_tensor, + target=True, + ) @cached_property def active_joint_names(self) -> List[str]: @@ -886,7 +1230,8 @@ def active_joint_names(self) -> List[str]: Returns: List[str]: The names of the active joints in the articulation. """ - return [self.joint_names[i] for i in self.active_joint_ids] + state_joint_names = self._state_joint_names() + return [state_joint_names[i] for i in self.active_joint_ids] @cached_property def all_joint_names(self) -> List[str]: @@ -903,9 +1248,10 @@ def get_parent_joint_chain( ) -> tuple[ArticulationJointKinematics, ...]: """Return the joints from a link toward the articulation root. - The immediate parent joint is first. Native simulator joint-info values - are copied into :class:`ArticulationJointKinematics`, keeping callers - independent of DexSim objects and the private entity collection. + The immediate parent joint is first. Backend-native joint-info values + or Newton's backend-neutral joint descriptors are copied into + :class:`ArticulationJointKinematics`, keeping callers independent of + DexSim objects and the private entity collection. Args: link_name: Link whose parent chain should be queried. @@ -932,11 +1278,36 @@ def get_parent_joint_chain( entity = self._entities[0] joints_by_child: dict[str, ArticulationJointKinematics] = {} for joint_name in entity.get_joint_names(): - native = entity.get_joint_info(joint_name) - if native is None: - raise ValueError( - f"Native articulation has no joint info for {joint_name!r}." - ) + if getattr(self._data, "is_newton_backend", False): + get_joint_desc = getattr(entity, "get_joint_desc", None) + if not callable(get_joint_desc): + raise ValueError( + "Native articulation has no joint topology for " + f"{joint_name!r}." + ) + try: + native = get_joint_desc(joint_name) + except (KeyError, StopIteration) as exc: + raise ValueError( + "Native articulation has no joint topology for " + f"{joint_name!r}." + ) from exc + else: + native = entity.get_joint_info(joint_name) + if native is None: + get_joint_desc = getattr(entity, "get_joint_desc", None) + if not callable(get_joint_desc): + raise ValueError( + "Native articulation has no joint topology for " + f"{joint_name!r}." + ) + try: + native = get_joint_desc(joint_name) + except (KeyError, StopIteration) as exc: + raise ValueError( + "Native articulation has no joint topology for " + f"{joint_name!r}." + ) from exc native_joint_type = getattr( native.joint_type, "name", @@ -944,11 +1315,19 @@ def get_parent_joint_chain( ) lower_limit = getattr(native, "lower_limit", None) upper_limit = getattr(native, "upper_limit", None) - joint_limits = ( - None - if lower_limit is None or upper_limit is None - else (float(lower_limit), float(upper_limit)) - ) + if lower_limit is None or upper_limit is None: + joint_limits = None + else: + lower_values = np.asarray(lower_limit, dtype=np.float32).reshape(-1) + upper_values = np.asarray(upper_limit, dtype=np.float32).reshape(-1) + if lower_values.size != 1 or upper_values.size != 1: + raise ValueError( + "Articulation topology requires scalar limits for " + f"joint {joint_name!r}; got lower shape " + f"{tuple(lower_values.shape)} and upper shape " + f"{tuple(upper_values.shape)}." + ) + joint_limits = (float(lower_values[0]), float(upper_values[0])) joint = ArticulationJointKinematics( name=native.name, joint_type=str(native_joint_type), @@ -988,6 +1367,77 @@ def body_data(self) -> ArticulationData: """ return self._data + @property + def default_link_masses(self) -> torch.Tensor: + """Initialization-time link masses retained for compatibility.""" + return self.body_data.default_mass + + def _capture_default_physical_properties(self) -> None: + """Capture materialized link mass properties as reset defaults.""" + if self._data.default_physical_properties_initialized: + return + mass, inertia, com_pose = self._data.read_physical_properties() + self._data.capture_default_physical_properties( + mass=mass, + inertia=inertia, + com_pose=com_pose, + ) + + def _resolve_link_names( + self, link_names: str | Sequence[str] | None + ) -> tuple[list[str], torch.Tensor]: + """Validate link names and return their public data-column indices.""" + names = ( + list(self.link_names) + if link_names is None + else [link_names] if isinstance(link_names, str) else list(link_names) + ) + unknown = [name for name in names if name not in self.link_names] + if unknown: + raise ValueError( + f"Unknown articulation links {unknown}; available links: " + f"{self.link_names}." + ) + indices = torch.as_tensor( + [self.link_names.index(name) for name in names], + dtype=torch.long, + device=self.device, + ) + return names, indices + + def _restore_default_physical_properties( + self, env_ids: Sequence[int] | torch.Tensor + ) -> None: + """Restore initialization-time link mass properties for selected rows.""" + if not self._data.default_physical_properties_initialized or len(env_ids) == 0: + return + + env_index = self._resolve_env_ids(env_ids) + env_list = env_index.detach().cpu().tolist() + default_mass = self._data.default_mass[env_index] + default_inertia = self._data.default_inertia[env_index] + default_com_pose = self._data.default_com_pose[env_index] + current_mass, current_inertia, current_com_pose = ( + value[env_index] for value in self._data.read_physical_properties() + ) + + mass_changed = not torch.allclose(current_mass, default_mass) + inertia_changed = not torch.allclose(current_inertia, default_inertia) + if mass_changed: + self.set_mass(default_mass, link_names=self.link_names, env_ids=env_list) + if mass_changed or inertia_changed: + self.set_inertia( + default_inertia, + link_names=self.link_names, + env_ids=env_list, + ) + if not torch.allclose(current_com_pose, default_com_pose): + self.set_com_pose( + default_com_pose, + link_names=self.link_names, + env_ids=env_list, + ) + @property def root_state(self) -> torch.Tensor: """Get the root state of the articulation. @@ -1118,53 +1568,29 @@ def set_local_pose( f"Length of env_ids {len(local_env_ids)} does not match pose length {len(pose)}." ) - if self.device.type == "cpu": - pose = pose.cpu() - if pose.dim() == 2 and pose.shape[1] == 7: - pose_matrix = torch.eye(4).unsqueeze(0).repeat(pose.shape[0], 1, 1) - pose_matrix[:, :3, 3] = pose[:, :3] - pose_matrix[:, :3, :3] = matrix_from_quat(pose[:, 3:7]) - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].set_local_pose(pose_matrix[i]) - elif pose.dim() == 3 and pose.shape[1:] == (4, 4): - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].set_local_pose(pose[i]) - else: - logger.log_error( - f"Invalid pose shape {pose.shape}. Expected (N, 7) or (N, 4, 4)." - ) - # TODO: in manual physics mode, the update should be explicitly called after - # setting the pose to synchronize the state to renderer. - + if pose.dim() == 2 and pose.shape[1] == 7: + target_pose = pose.to(device=self.device, dtype=torch.float32) + elif pose.dim() == 3 and pose.shape[1:] == (4, 4): + xyz = pose[:, :3, 3] + quat = quat_from_matrix(pose[:, :3, :3]) + target_pose = torch.cat((xyz, quat), dim=-1).to( + device=self.device, dtype=torch.float32 + ) else: - if pose.dim() == 2 and pose.shape[1] == 7: - xyz = pose[:, :3] - quat = convert_quat(pose[:, 3:7], to="xyzw") - elif pose.dim() == 3 and pose.shape[1:] == (4, 4): - xyz = pose[:, :3, 3] - quat = quat_from_matrix(pose[:, :3, :3]) - quat = convert_quat(quat, to="xyzw") - else: - logger.log_error( - f"Invalid pose shape {pose.shape}. Expected (N, 7) or (N, 4, 4)." - ) - - # we should keep `pose_` life cycle to the end of the function. - pose_ = torch.cat((quat, xyz), dim=-1) - indices = self.body_data.gpu_indices[local_env_ids] - self._ps.gpu_apply_root_data( - data=pose_, - gpu_indices=indices, - data_type=ArticulationGPUAPIWriteType.ROOT_GLOBAL_POSE, + logger.log_error( + f"Invalid pose shape {pose.shape}. Expected (N, 7) or (N, 4, 4)." ) - self._ps.gpu_compute_articulation_kinematic(gpu_indices=indices) - self._world.update(0.001) + return + + self._data.articulation_view.apply_root_pose(target_pose, local_env_ids) + if self.device.type == "cpu" and not self._data.is_newton_backend: + self._world.update(0.001) def get_local_pose(self, to_matrix=False) -> torch.Tensor: """Get local pose (root link pose) of the articulation. Args: - to_matrix (bool, optional): If True, return the pose as a 4x4 matrix. If False, return as (x, y, z, qw, qx, qy, qz). Defaults to False. + to_matrix (bool, optional): If True, return the pose as a 4x4 matrix. If False, return as (x, y, z, qx, qy, qz, qw). Defaults to False. Returns: torch.Tensor: The local pose of the articulation with shape (N, 7) or (N, 4, 4) depending on `to_matrix`. @@ -1246,7 +1672,7 @@ def get_link_pose( Args: link_name (str): The name of the link. env_ids (Sequence[int] | None, optional): Environment indices. If None, then all indices are used. - to_matrix (bool, optional): If True, return the pose as a 4x4 matrix. If False, return as (x, y, z, qw, qx, qy, qz). Defaults to False. + to_matrix (bool, optional): If True, return the pose as a 4x4 matrix. If False, return as (x, y, z, qx, qy, qz, qw). Defaults to False. Returns: torch.Tensor: The pose of the specified link with shape (N, 7) or (N, 4, 4) depending on `to_matrix`. @@ -1500,34 +1926,18 @@ def set_qpos( :, local_joint_ids, : ] qpos = qpos.clamp(selected_limits[..., 0], selected_limits[..., 1]) - - if self.device.type == "cpu": - local_joint_ids_np = ( - local_joint_ids.detach().cpu().numpy().astype(np.int32, copy=False) - ) - for i, env_idx in enumerate(local_env_ids.detach().cpu().tolist()): - setter = ( - self._entities[env_idx].set_target_qpos - if target - else self._entities[env_idx].set_current_qpos - ) - setter(qpos[i].detach().cpu().numpy(), local_joint_ids_np) - else: - data_type = ( - ArticulationGPUAPIWriteType.JOINT_TARGET_POSITION - if target - else ArticulationGPUAPIWriteType.JOINT_POSITION - ) - - # Always fetch the latest data to avoid stale values - qpos_set = self.body_data._target_qpos if target else self.body_data._qpos - - indices = self.body_data.gpu_indices[local_env_ids] - qpos_set[local_env_ids[:, None], local_joint_ids] = qpos - self._ps.gpu_apply_joint_data( - data=qpos_set, - gpu_indices=indices, - data_type=data_type, + self._data.articulation_view.apply_qpos( + qpos, + local_env_ids, + local_joint_ids, + target=target, + ) + if target: + self._stabilize_newton_mimic_target_write( + qpos, + local_env_ids, + local_joint_ids, + velocity=False, ) def get_qvel(self, target: bool = False) -> torch.Tensor: @@ -1578,55 +1988,35 @@ def set_qvel( Raises: ValueError: If the length of `env_ids` does not match the length of `qvel`. """ - local_env_ids = self._all_indices if env_ids is None else env_ids + local_env_ids = self._resolve_env_ids(env_ids) + + if not isinstance(qvel, torch.Tensor): + qvel = torch.as_tensor(qvel, dtype=torch.float32, device=self.device) + else: + qvel = qvel.to(device=self.device, dtype=torch.float32) + + if qvel.dim() == 1: + qvel = qvel.unsqueeze(0) if len(local_env_ids) != len(qvel): logger.log_error( f"Length of env_ids {len(local_env_ids)} does not match qvel length {len(qvel)}." ) - if joint_ids is None: - local_joint_ids = torch.arange( - self.dof, device=self.device, dtype=torch.int32 - ) - elif not isinstance(joint_ids, torch.Tensor): - local_joint_ids = torch.as_tensor( - joint_ids, dtype=torch.int32, device=self.device - ) - else: - local_joint_ids = joint_ids - - if self.device.type == "cpu": - for i, env_idx in enumerate(local_env_ids): - setter = ( - self._entities[env_idx].set_target_qvel - if target - else self._entities[env_idx].set_current_qvel - ) - setter(qvel[i].numpy(), local_joint_ids) - else: - data_type = ( - ArticulationGPUAPIWriteType.JOINT_TARGET_VELOCITY - if target - else ArticulationGPUAPIWriteType.JOINT_VELOCITY - ) - - # Always fetch the latest data to avoid stale values - if target: - qvel_set = self.body_data._target_qvel - else: - qvel_set = self.body_data._qvel + local_joint_ids = self._resolve_joint_ids(joint_ids) - if not isinstance(local_env_ids, torch.Tensor): - local_env_ids = torch.as_tensor( - local_env_ids, dtype=torch.long, device=self.device - ) - indices = self.body_data.gpu_indices[local_env_ids] - qvel_set[local_env_ids[:, None], local_joint_ids] = qvel - self._ps.gpu_apply_joint_data( - data=qvel_set, - gpu_indices=indices, - data_type=data_type, + self._data.articulation_view.apply_qvel( + qvel, + local_env_ids, + local_joint_ids, + target=target, + ) + if target: + self._stabilize_newton_mimic_target_write( + qvel, + local_env_ids, + local_joint_ids, + velocity=True, ) def set_qf( @@ -1644,30 +2034,31 @@ def set_qf( """ local_env_ids = self._all_indices if env_ids is None else env_ids + if not isinstance(qf, torch.Tensor): + qf = torch.as_tensor(qf, dtype=torch.float32, device=self.device) + else: + qf = qf.to(device=self.device, dtype=torch.float32) + + if qf.dim() == 1: + qf = qf.unsqueeze(0) + if len(local_env_ids) != len(qf): logger.log_error( f"Length of env_ids {len(local_env_ids)} does not match qf length {len(qf)}." ) - if self.device.type == "cpu": - local_joint_ids = np.arange(self.dof) if joint_ids is None else joint_ids - for i, env_idx in enumerate(local_env_ids): - setter = self._entities[env_idx].set_current_qf - setter(qf[i].numpy(), local_joint_ids) - else: - indices = self.body_data.gpu_indices[local_env_ids] - if joint_ids is None: - qf_set = self.body_data._qf[local_env_ids] - qf_set[:, : self.dof] = qf - else: - self.body_data.qf - qf_set = self.body_data._qf[local_env_ids] - qf_set[:, joint_ids] = qf - self._ps.gpu_apply_joint_data( - data=qf_set, - gpu_indices=indices, - data_type=ArticulationGPUAPIWriteType.JOINT_FORCE, + if joint_ids is None: + local_joint_ids = torch.arange( + self.dof, device=self.device, dtype=torch.int32 + ) + elif not isinstance(joint_ids, torch.Tensor): + local_joint_ids = torch.as_tensor( + joint_ids, dtype=torch.int32, device=self.device ) + else: + local_joint_ids = joint_ids.to(device=self.device, dtype=torch.int32) + + self._data.articulation_view.apply_qf(qf, local_env_ids, local_joint_ids) def get_qf(self) -> torch.Tensor: """Get the current generalized efforts (qf) of the articulation. @@ -1700,76 +2091,178 @@ def get_qf_limits( def set_mass( self, mass: torch.Tensor, - link_names: Sequence[str], - env_ids: Sequence[int] | None = None, + link_names: str | Sequence[str] | None = None, + env_ids: Sequence[int] | torch.Tensor | None = None, ) -> None: """Set the mass of specific links in the articulation. Args: - mass (torch.Tensor): The mass values to set with shape (N, len(link_names)). - link_names (Sequence[str]): The names of the links to set the mass for. - env_ids (Sequence[int] | None, optional): Environment indices to apply the mass change. If None, applies to all environments. Defaults to None. - """ - local_env_ids = self._all_indices if env_ids is None else env_ids - - if len(local_env_ids) != len(mass): - logger.log_error( - f"Length of env_ids {len(local_env_ids)} does not match mass length {len(mass)}." + mass: Mass values with shape ``(num_envs, num_links)``. + link_names: Link names to update. If None, all links are updated. + env_ids: Environment indices. If None, all rows are updated. + """ + env_index = self._resolve_env_ids(env_ids) + env_list = env_index.detach().cpu().tolist() + names, _ = self._resolve_link_names(link_names) + mass = torch.as_tensor(mass, dtype=torch.float32, device=self.device) + expected_shape = (len(env_list), len(names)) + if tuple(mass.shape) != expected_shape: + raise ValueError( + f"Expected mass shape {expected_shape}, got {tuple(mass.shape)}." ) - for link_name in link_names: - if link_name not in self.link_names: - logger.log_error( - f"Link name {link_name} not found in {self.__class__.__name__}. Available links: {self.link_names}" - ) - - for i, env_idx in enumerate(local_env_ids): - for j, name in enumerate(link_names): - self._entities[env_idx].set_mass(name, mass[i, j].item()) + for i, env_idx in enumerate(env_list): + entity = self._entities[env_idx] + for j, name in enumerate(names): + if self.is_spawn_bound or self._data.is_newton_backend: + entity.set_link_mass(name, mass[i, j].item()) + else: + entity.set_mass(name, mass[i, j].item()) def get_mass( self, - link_names: Sequence[str] | None = None, - env_ids: Sequence[int] | None = None, + link_names: str | Sequence[str] | None = None, + env_ids: Sequence[int] | torch.Tensor | None = None, ) -> torch.Tensor: """Get the mass of specific links in the articulation. Args: - link_names (Sequence[str] | None, optional): The names of the links to get the mass for. If None, gets mass for all links. Defaults to None. - env_ids (Sequence[int] | None, optional): Environment indices to get the mass from. If None, gets from all environments. Defaults to None. + link_names: Link names to query. If None, all links are returned. + env_ids: Environment indices. If None, all rows are returned. Returns: - torch.Tensor: The mass of the specified links with shape (N, len(link_names)). + Selected link masses with shape ``(num_envs, num_links)``. """ - local_env_ids = self._all_indices if env_ids is None else env_ids + env_index = self._resolve_env_ids(env_ids) + _, link_index = self._resolve_link_names(link_names) + return self.body_data.mass[ + env_index[:, None], + link_index[None, :], + ] - if link_names is None: - link_names = self.link_names - else: - for link_name in link_names: - if link_name not in self.link_names: - logger.log_error( - f"Link name {link_name} not found in {self.__class__.__name__}. Available links: {self.link_names}" + def set_inertia( + self, + inertia: torch.Tensor, + link_names: str | Sequence[str] | None = None, + env_ids: Sequence[int] | torch.Tensor | None = None, + ) -> None: + """Set principal moments of inertia for selected links.""" + env_index = self._resolve_env_ids(env_ids) + env_list = env_index.detach().cpu().tolist() + names, _ = self._resolve_link_names(link_names) + inertia = torch.as_tensor(inertia, dtype=torch.float32, device=self.device) + expected_shape = (len(env_list), len(names), 3) + if tuple(inertia.shape) != expected_shape: + raise ValueError( + f"Expected inertia shape {expected_shape}, " + f"got {tuple(inertia.shape)}." + ) + + values = inertia.detach().cpu().numpy() + for i, env_idx in enumerate(env_list): + entity = self._entities[env_idx] + for j, name in enumerate(names): + value = np.asarray(values[i, j], dtype=np.float32) + if self.is_spawn_bound and self._data.is_newton_backend: + entity.set_newton_link_properties( + name, + rigid_body=dexsim.spawn.RigidBodyPhysicsDesc.dynamic( + inertia=value + ), + ) + elif not self._data.is_newton_backend: + entity.get_physical_body(name).set_mass_space_inertia_tensor(value) + else: + attr = entity.get_physical_attr(name) + attr.inertia = value + entity.set_physical_attr( + attr, + name, + is_replace_inertial=False, ) - mass_tensor = torch.zeros( - (len(local_env_ids), len(link_names)), - dtype=torch.float32, - device=self.device, - ) - for i, env_idx in enumerate(local_env_ids): - for j, name in enumerate(link_names): - mass_tensor[i, j] = ( - self._entities[env_idx].get_physical_body(name).get_mass() + def get_inertia( + self, + link_names: str | Sequence[str] | None = None, + env_ids: Sequence[int] | torch.Tensor | None = None, + ) -> torch.Tensor: + """Get principal moments of inertia for selected links.""" + env_index = self._resolve_env_ids(env_ids) + _, link_index = self._resolve_link_names(link_names) + return self.body_data.inertia[ + env_index[:, None], + link_index[None, :], + ] + + def set_com_pose( + self, + com_pose: torch.Tensor, + link_names: str | Sequence[str] | None = None, + env_ids: Sequence[int] | torch.Tensor | None = None, + ) -> None: + """Set local COM poses in EmbodiChain ``xyz + xyzw`` convention.""" + env_index = self._resolve_env_ids(env_ids) + env_list = env_index.detach().cpu().tolist() + names, _ = self._resolve_link_names(link_names) + com_pose = torch.as_tensor(com_pose, dtype=torch.float32, device=self.device) + expected_shape = (len(env_list), len(names), 7) + if tuple(com_pose.shape) != expected_shape: + raise ValueError( + f"Expected COM pose shape {expected_shape}, " + f"got {tuple(com_pose.shape)}." + ) + + values = com_pose.detach().cpu().numpy() + for i, env_idx in enumerate(env_list): + entity = self._entities[env_idx] + for j, name in enumerate(names): + position = np.asarray(values[i, j, :3], dtype=np.float32) + quaternion = np.asarray( + convert_quat(values[i, j, 3:7], to="wxyz"), + dtype=np.float32, ) - return mass_tensor + if self.is_spawn_bound and self._data.is_newton_backend: + entity.set_newton_link_properties( + name, + rigid_body=dexsim.spawn.RigidBodyPhysicsDesc.dynamic( + com_position=position, + com_quaternion=quaternion, + ), + ) + elif not self._data.is_newton_backend: + entity.get_physical_body(name).set_cmass_local_pose( + position, + quaternion, + ) + else: + attr = entity.get_physical_attr(name) + attr.com_position = position + attr.com_quaternion = quaternion + entity.set_physical_attr( + attr, + name, + is_replace_inertial=False, + ) + + def get_com_pose( + self, + link_names: str | Sequence[str] | None = None, + env_ids: Sequence[int] | torch.Tensor | None = None, + ) -> torch.Tensor: + """Get local COM poses in EmbodiChain ``xyz + xyzw`` convention.""" + env_index = self._resolve_env_ids(env_ids) + _, link_index = self._resolve_link_names(link_names) + return self.body_data.com_pose[ + env_index[:, None], + link_index[None, :], + ] def get_link_physical_attr( self, link_names: str | Sequence[str] | None = None, env_ids: Sequence[int] | None = None, ) -> list[PhysicalAttr]: - """Get physical attributes for articulation links. + """Get DexSim-native physical attributes for articulation links. Args: link_names: Link names or regex patterns. If None, all links are returned. @@ -1779,6 +2272,11 @@ def get_link_physical_attr( List of :class:`~dexsim.types.PhysicalAttr`, one per (env, link) pair in row-major order (env-major). """ + if self._data is not None and self._data.is_newton_backend: + raise RuntimeError( + "get_link_physical_attr() exposes DexSim PhysicalAttr semantics; " + "use get_newton_link_properties() for Newton." + ) if link_names is None: matched_link_names = self.link_names elif isinstance(link_names, str): @@ -1793,28 +2291,82 @@ def get_link_physical_attr( local_env_ids = [0] if env_ids is None else list(env_ids) attrs: list[PhysicalAttr] = [] for env_idx in local_env_ids: + entity = self._entities[env_idx] for name in matched_link_names: - attrs.append(self._entities[env_idx].get_physical_attr(name)) + attrs.append(entity.get_physical_attr(name)) return attrs + def get_newton_link_properties( + self, + link_names: str | Sequence[str] | None = None, + env_ids: Sequence[int] | None = None, + ) -> list[dexsim.spawn.RigidBodyPhysicsDesc]: + """Get Newton model mass properties as typed Spawn descriptors. + + Args: + link_names: Link names or regex patterns. If None, all links are + returned. + env_ids: Environment indices. If None, only environment 0 is + queried. + + Returns: + One typed descriptor per selected ``(environment, link)`` pair in + environment-major order. + """ + if not ( + self.is_spawn_bound + and self._data is not None + and self._data.is_newton_backend + ): + raise RuntimeError( + "get_newton_link_properties() requires a Spawn-bound Newton " + "articulation." + ) + if link_names is None: + matched_link_names = self.link_names + else: + _, matched_link_names = resolve_matching_names( + keys=link_names, + list_of_strings=self.link_names, + ) + + local_env_ids = [0] if env_ids is None else list(env_ids) + properties = [] + for env_idx in local_env_ids: + entity = self._entities[env_idx] + for name in matched_link_names: + properties.append(entity.get_newton_link_properties(name)) + return properties + def set_link_physical_attr( self, - attrs: RigidBodyAttributesCfg | RigidBodyAttributesOverrideCfg | PhysicalAttr, + attrs: RigidBodyPhysicsCfg | PhysicalAttr, link_names: str | Sequence[str] | None = None, env_ids: Sequence[int] | None = None, *, - base_attrs: RigidBodyAttributesCfg | None = None, + base_attrs: RigidBodyPhysicsCfg | None = None, replace_inertial: bool = False, ) -> None: """Set physical attributes for selected articulation links. Args: - attrs: Full, partial, or DexSim physical attributes to apply. + attrs: Grouped or DexSim physical attributes to apply. link_names: Link names or regex patterns. If None, all links are updated. env_ids: Environment indices. If None, all environments are updated. base_attrs: Base config used when ``attrs`` is a partial override. replace_inertial: Recompute inertia when mass changes. + + .. attention:: + This compatibility API exposes DexSim ``PhysicalAttr`` semantics. + Newton properties must use typed Spawn descriptors. """ + is_newton = self._data is not None and self._data.is_newton_backend + if is_newton: + raise TypeError( + "set_link_physical_attr() is DexSim-only; use typed Newton " + "link properties or set_mass()/set_inertia()/set_com_pose()." + ) + if link_names is None: matched_link_names = self.link_names elif isinstance(link_names, str): @@ -1826,22 +2378,26 @@ def set_link_physical_attr( keys=link_names, list_of_strings=self.link_names ) - if isinstance(attrs, RigidBodyAttributesOverrideCfg): + if isinstance(attrs, RigidBodyPhysicsCfg): if base_attrs is None: base_attrs = self.cfg.attrs - physical_attr = attrs.merge_with(base_attrs) - if attrs.mass is not None: - replace_inertial = True - elif isinstance(attrs, RigidBodyAttributesCfg): - physical_attr = attrs.attr() + physical_attr = attrs.to_dexsim_physical_attr( + base=base_attrs.to_dexsim_physical_attr() + ) + mass_props = attrs.mass_props + if mass_props is not None and mass_props.recompute_inertia is not None: + replace_inertial = bool(mass_props.recompute_inertia) else: physical_attr = attrs local_env_ids = self._all_indices if env_ids is None else env_ids for env_idx in local_env_ids: + entity = self._entities[env_idx] for name in matched_link_names: - self._entities[env_idx].set_physical_attr( - physical_attr, name, is_replace_inertial=replace_inertial + entity.set_physical_attr( + physical_attr, + name, + is_replace_inertial=replace_inertial, ) def set_joint_drive( @@ -1852,9 +2408,11 @@ def set_joint_drive( max_velocity: torch.Tensor | None = None, friction: torch.Tensor | None = None, armature: torch.Tensor | None = None, - drive_type: str = "none", + drive_type: str | None = None, joint_ids: Sequence[int] | None = None, env_ids: Sequence[int] | None = None, + *, + target_mode: str | int | None = None, ) -> None: """Set the drive properties for the articulation. @@ -1865,24 +2423,88 @@ def set_joint_drive( max_velocity (torch.Tensor): The maximum velocity of the joint drive with shape (len(env_ids), len(joint_ids)). friction (torch.Tensor): The joint friction coefficient with shape (len(env_ids), len(joint_ids)). armature (torch.Tensor): The joint armature with shape (len(env_ids), len(joint_ids)). - drive_type (str, optional): The type of drive to apply. Defaults to "none". + drive_type: ``force``, ``acceleration``, or ``none``. ``None`` + preserves the current mode unless a target mode activates a + force drive. joint_ids (Sequence[int] | None, optional): The joint indices to apply the drive to. If None, applies to all joints. Defaults to None. env_ids (Sequence[int] | None, optional): The environment indices to apply the drive to. If None, applies to all environments. Defaults to None. + target_mode: Portable target mode: ``none``, ``position``, + ``velocity``, ``position_velocity``, ``effort``, or integer + value 0 through 4. """ local_env_ids = self._all_indices if env_ids is None else env_ids local_joint_ids = np.arange(self.dof) if joint_ids is None else joint_ids cache_env_ids = self._resolve_env_ids(env_ids) cache_joint_ids = self._resolve_joint_ids(joint_ids) + mode_cfg = JointDrivePropertiesCfg( + target_mode=target_mode, + drive_type=drive_type, + ) + resolved_target_mode, resolved_drive_type = mode_cfg._resolve_modes() + if isinstance(resolved_target_mode, dict): + raise TypeError( + "set_joint_drive() accepts one scalar target_mode; configure " + "per-joint mappings through JointDrivePropertiesCfg." + ) + target_mode_value = ( + None + if resolved_target_mode is None + else _normalize_joint_target_mode(resolved_target_mode) + ) + if target_mode_value in {1, 2, 3} and resolved_drive_type == "none": + raise ValueError( + "drive_type='none' conflicts with an active target_mode; use " + "target_mode='none' or 'effort'." + ) + def _drive_arg(value: torch.Tensor, index: int) -> float | np.ndarray: result = value[index].detach().cpu().numpy() return result.item() if result.size == 1 else result for i, env_idx in enumerate(local_env_ids): - drive_args = { - "drive_type": get_dexsim_drive_type(drive_type), - "joint_ids": local_joint_ids, - } + if self.is_spawn_bound and self.body_data.is_newton_backend: + if resolved_drive_type == "acceleration" and target_mode_value in { + 1, + 2, + 3, + }: + raise NotImplementedError( + "Newton Spawn does not have an exact equivalent of " + "the Default acceleration drive. Use " + "drive_type='force' or disable the drive." + ) + drive_args = {"joint_ids": local_joint_ids} + if target_mode_value is not None: + drive_args["target_mode"] = target_mode_value + if stiffness is not None: + drive_args["target_ke"] = _drive_arg(stiffness, i) + if damping is not None: + drive_args["target_kd"] = _drive_arg(damping, i) + if max_effort is not None: + drive_args["effort_limit"] = _drive_arg(max_effort, i) + if max_velocity is not None: + drive_args["velocity_limit"] = _drive_arg(max_velocity, i) + if friction is not None: + drive_args["friction"] = _drive_arg(friction, i) + if armature is not None: + drive_args["armature"] = _drive_arg(armature, i) + if target_mode_value in {0, 4}: + drive_args["target_ke"] = 0.0 + drive_args["target_kd"] = 0.0 + elif target_mode_value == 2: + drive_args["target_ke"] = 0.0 + self._entities[env_idx].set_newton_drive(**drive_args) + continue + + drive_args = {"joint_ids": local_joint_ids} + default_drive_type = resolved_drive_type + if target_mode_value in {0, 4}: + default_drive_type = "none" + elif target_mode_value in {1, 2, 3} and default_drive_type is None: + default_drive_type = "force" + if default_drive_type is not None: + drive_args["drive_type"] = get_dexsim_drive_type(default_drive_type) if stiffness is not None: drive_args["stiffness"] = _drive_arg(stiffness, i) if damping is not None: @@ -1895,6 +2517,11 @@ def _drive_arg(value: torch.Tensor, index: int) -> float | np.ndarray: drive_args["joint_friction"] = _drive_arg(friction, i) if armature is not None: drive_args["armature"] = _drive_arg(armature, i) + if target_mode_value in {0, 4}: + drive_args["stiffness"] = 0.0 + drive_args["damping"] = 0.0 + elif target_mode_value == 2: + drive_args["stiffness"] = 0.0 self._entities[env_idx].set_drive(**drive_args) if max_velocity is not None: @@ -1987,7 +2614,7 @@ def get_joint_drive( friction_i, armature_i, *_, - ) = self._entities[env_idx].get_drive() + ) = self._data._entity_drive_properties(self._entities[env_idx]) stiffness[i] = torch.as_tensor( stiffness_i, dtype=torch.float32, device=self.device )[local_joint_ids_tensor] @@ -2013,15 +2640,20 @@ def get_joint_drive_type( joint_ids: Sequence[int] | None = None, env_ids: Sequence[int] | None = None, ) -> list[list[DriveType]]: - """Get the backend drive type for the selected joints. + """Get the portable drive type for the selected joints. Args: joint_ids: Joint indices to query. If None, queries all joints. env_ids: Environment indices to query. If None, queries all environments. Returns: - Backend drive types grouped by environment, with one + Drive types grouped by environment, with one :class:`~dexsim.types.DriveType` per selected joint. + + Newton has no acceleration-drive equivalent. Its passive and + direct-effort target modes map to :attr:`DriveType.NONE` because + neither installs a PD drive; position and velocity target modes + map to :attr:`DriveType.FORCE`. """ local_env_ids = self._all_indices if env_ids is None else env_ids if joint_ids is None: @@ -2035,12 +2667,63 @@ def get_joint_drive_type( drive_types: list[list[DriveType]] = [] for env_idx in local_env_ids: - entity_drive_types = self._entities[int(env_idx)].get_drive( - local_joint_ids - )[-1] - drive_types.append(list(entity_drive_types)) + entity = self._entities[int(env_idx)] + if self._data is not None and self._data.is_newton_backend: + target_modes = np.asarray(entity.get_newton_drive()[-1])[ + local_joint_ids + ] + drive_types.append( + [ + (DriveType.NONE if int(mode) in {0, 4} else DriveType.FORCE) + for mode in target_modes + ] + ) + else: + entity_drive_types = np.asarray(entity.get_drive()[-1])[local_joint_ids] + drive_types.append(list(entity_drive_types)) return drive_types + def get_joint_target_mode( + self, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | None = None, + ) -> list[list[int]]: + """Get Newton ``JointTargetMode`` integer values by environment. + + Args: + joint_ids: Flattened DOF indices. If None, all DOFs are queried. + env_ids: Environment indices. If None, all environments are + queried. + + Returns: + Integer target modes grouped by selected environment. + """ + if not ( + self.is_spawn_bound + and self._data is not None + and self._data.is_newton_backend + ): + raise RuntimeError( + "get_joint_target_mode() requires a Spawn-bound Newton " "articulation." + ) + local_env_ids = self._all_indices if env_ids is None else env_ids + if joint_ids is None: + local_joint_ids = np.arange(self.dof, dtype=np.int32) + elif isinstance(joint_ids, torch.Tensor): + local_joint_ids = ( + joint_ids.detach().cpu().numpy().astype(np.int32, copy=False) + ) + else: + local_joint_ids = np.asarray(joint_ids, dtype=np.int32) + + target_modes = [] + for env_idx in local_env_ids: + modes = self._entities[int(env_idx)].get_newton_drive()[-1] + target_modes.append( + [int(value) for value in np.asarray(modes)[local_joint_ids]] + ) + return target_modes + def get_user_ids( self, link_name: str | None = None, env_ids: Sequence[int] | None = None ) -> torch.Tensor: @@ -2073,10 +2756,7 @@ def clear_dynamics(self, env_ids: Sequence[int] | None = None) -> None: env_ids (Sequence[int] | None): Environment indices. If None, then all indices are used. """ local_env_ids = self._all_indices if env_ids is None else env_ids - zeros = torch.zeros((len(local_env_ids), self.dof), device=self.device) - self.set_qvel(zeros, env_ids=local_env_ids) - self.set_qvel(zeros, env_ids=local_env_ids, target=True) - self.set_qf(zeros, env_ids=local_env_ids) + self._data.articulation_view.clear_dynamics(local_env_ids) def reallocate_body_data(self) -> None: """Reallocate body data tensors to match the current articulation state in the GPU physics scene.""" @@ -2133,49 +2813,76 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: self.cfg: ArticulationCfg self.restore_visual_material(env_ids=local_env_ids) + self._restore_default_physical_properties(local_env_ids) - pos = torch.as_tensor( - self.cfg.init_pos, dtype=torch.float32, device=self.device - ) - rot = ( - torch.as_tensor(self.cfg.init_rot, dtype=torch.float32, device=self.device) - * torch.pi - / 180.0 - ) - pos = pos.unsqueeze(0).repeat(num_instances, 1) - rot = rot.unsqueeze(0).repeat(num_instances, 1) - mat = matrix_from_euler(rot, "XYZ") - pose = ( - torch.eye(4, dtype=torch.float32, device=self.device) - .unsqueeze(0) - .repeat(num_instances, 1, 1) - ) - pose[:, :3, 3] = pos - pose[:, :3, :3] = mat + if self.cfg.init_local_pose is not None: + pose = ( + torch.as_tensor( + self.cfg.init_local_pose, + dtype=torch.float32, + device=self.device, + ) + .reshape(1, 4, 4) + .repeat(num_instances, 1, 1) + ) + else: + pos = torch.as_tensor( + self.cfg.init_pos, dtype=torch.float32, device=self.device + ) + rot = ( + torch.as_tensor( + self.cfg.init_rot, dtype=torch.float32, device=self.device + ) + * torch.pi + / 180.0 + ) + pos = pos.unsqueeze(0).repeat(num_instances, 1) + rot = rot.unsqueeze(0).repeat(num_instances, 1) + pose = ( + torch.eye(4, dtype=torch.float32, device=self.device) + .unsqueeze(0) + .repeat(num_instances, 1, 1) + ) + pose[:, :3, 3] = pos + pose[:, :3, :3] = matrix_from_euler(rot, "XYZ") self.set_local_pose(pose, env_ids=local_env_ids) qpos = torch.as_tensor( self.cfg.init_qpos, dtype=torch.float32, device=self.device ) qpos = qpos.unsqueeze(0).repeat(num_instances, 1) + qpos = self._source_qpos_to_state_order(qpos) + if ( + self.body_data.is_newton_backend + and not self._newton_mimic_compliance_configured + ): + # Native Newton mimic constraints can generate a large corrective + # impulse when initialized away from their equality manifold. + qpos = self._project_mimic_qpos(qpos) self.set_qpos(qpos, target=False, env_ids=local_env_ids) # Set drive target to hold position. self.set_qpos(qpos, target=True, env_ids=local_env_ids) self.clear_dynamics(env_ids=local_env_ids) - if self.device.type == "cuda": - self._ps.gpu_compute_articulation_kinematic( - gpu_indices=self.body_data.gpu_indices[local_env_ids] - ) - self._world.update(0.001) + self._data.articulation_view.compute_kinematics(local_env_ids) + if self.device.type == "cpu" and not self._data.is_newton_backend: + self._world.update(0.001) - def _set_default_joint_drive(self) -> None: + def _set_default_joint_drive( + self, + joint_drive_props: JointDrivePropertiesCfg | dict | None = None, + ) -> None: """Set default joint drive parameters based on the configuration.""" import numbers from embodichain.utils.string import resolve_matching_names_values - drive_props = [ + if joint_drive_props is None: + joint_drive_props = self.cfg.joint_drive_props + if joint_drive_props is None: + return + + joint_property_targets = [ ("damping", self.default_joint_damping), ("stiffness", self.default_joint_stiffness), ("max_effort", self.default_joint_max_effort), @@ -2184,8 +2891,12 @@ def _set_default_joint_drive(self) -> None: ("armature", self.default_joint_armature), ] - for prop_name, default_array in drive_props: - value = getattr(self.cfg.drive_pros, prop_name, None) + for prop_name, default_array in joint_property_targets: + value = ( + joint_drive_props.get(prop_name) + if isinstance(joint_drive_props, dict) + else getattr(joint_drive_props, prop_name, None) + ) if value is None: continue if isinstance(value, numbers.Number): @@ -2201,11 +2912,19 @@ def _set_default_joint_drive(self) -> None: except Exception as e: logger.log_error(f"Failed to set {prop_name}: {e}") - drive_pros = self.cfg.drive_pros - if isinstance(drive_pros, dict): - drive_type = drive_pros.get("drive_type", "none") + if isinstance(joint_drive_props, dict): + drive_type = joint_drive_props.get("drive_type") + target_mode = joint_drive_props.get("target_mode") else: - drive_type = getattr(drive_pros, "drive_type", "none") + drive_type = getattr(joint_drive_props, "drive_type", None) + target_mode = getattr(joint_drive_props, "target_mode", None) + if isinstance(target_mode, dict): + logger.log_warning( + "Per-joint target_mode mappings require a Spawn-bound " + "articulation; the retained raw-articulation path preserves " + "its current target modes." + ) + target_mode = None # Apply drive parameters to all articulations in the batch self.set_joint_drive( @@ -2216,6 +2935,7 @@ def _set_default_joint_drive(self) -> None: friction=self.default_joint_friction, armature=self.default_joint_armature, drive_type=drive_type, + target_mode=target_mode, ) def compute_fk( @@ -2476,7 +3196,12 @@ def set_visual_material( for link_name in link_names: mat_inst = mat.create_instance(f"{mat.uid}_{self.uid}_{link_name}") for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].set_material(link_name, mat_inst.mat) + if self.is_spawn_bound: + self._entities[env_idx].set_material_inst( + link_name, mat_inst.mat + ) + else: + self._entities[env_idx].set_material(link_name, mat_inst.mat) self._visual_material[env_idx][link_name] = mat_inst if update_default: self._original_visual_material[env_idx][link_name] = ( @@ -2494,7 +3219,12 @@ def set_visual_material( mat_inst = mat.create_instance( f"{mat.uid}_{self.uid}_{link_name}_{env_idx}" ) - self._entities[env_idx].set_material(link_name, mat_inst.mat) + if self.is_spawn_bound: + self._entities[env_idx].set_material_inst( + link_name, mat_inst.mat + ) + else: + self._entities[env_idx].set_material(link_name, mat_inst.mat) self._visual_material[env_idx][link_name] = mat_inst if update_default: self._original_visual_material[env_idx][link_name] = ( @@ -2719,6 +3449,17 @@ def set_physical_visible( ) link_names = self.link_names if link_names is None else link_names + if self.is_spawn_bound: + for env_idx in self._all_indices: + entity = self._entities[env_idx] + for link_name in link_names: + self._spawn_result.set_physical_visible( + (entity, link_name), rgba, visible + ) + for link_name in link_names: + self._has_collision_visible_node_dict[link_name] = True + return + # create collision visible node if not exist if visible: for i, env_idx in enumerate(self._all_indices): @@ -2784,12 +3525,18 @@ def set_gravity( self._entities[env_idx].enable_gravity(bool(enable)) def destroy(self) -> None: + if self.is_declared or self.is_spawn_bound: + # The finalized Scene is the sole owner of native lifetime. + return env = self._world.get_env() arenas = env.get_all_arenas() if len(arenas) == 0: arenas = [env] for i, entity in enumerate(self._entities): - arenas[i].remove_articulation(entity) + if self._data.is_newton_backend: + arenas[i].remove_skeleton(entity) + else: + arenas[i].remove_articulation(entity) __all__ = ["ArticulationData", "Articulation", "ArticulationJointKinematics"] diff --git a/embodichain/lab/sim/objects/backends/__init__.py b/embodichain/lab/sim/objects/backends/__init__.py new file mode 100644 index 000000000..e4a98aafd --- /dev/null +++ b/embodichain/lab/sim/objects/backends/__init__.py @@ -0,0 +1,27 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from .base import ArticulationViewBase, RigidBodyViewBase +from .newton import is_newton_scene +from .scene import SceneArticulationView, SceneRigidBodyView + +__all__ = [ + "ArticulationViewBase", + "RigidBodyViewBase", + "is_newton_scene", + "SceneArticulationView", + "SceneRigidBodyView", +] diff --git a/embodichain/lab/sim/objects/backends/base.py b/embodichain/lab/sim/objects/backends/base.py new file mode 100644 index 000000000..81a475ec3 --- /dev/null +++ b/embodichain/lab/sim/objects/backends/base.py @@ -0,0 +1,403 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import Sequence +from functools import cached_property + +import torch + +__all__ = ["RigidBodyViewBase", "ArticulationViewBase"] + + +class RigidBodyViewBase(ABC): + """Abstract interface for physics-backend rigid body data access. + + All pose/velocity/acceleration data uses EmbodiChain convention: + ``(x, y, z, qx, qy, qz, qw)``. + """ + + # -- Lifecycle & State -------------------------------------------------- + + @property + @abstractmethod + def is_ready(self) -> bool: + """Whether the backend simulation is finalized and data can be accessed.""" + ... + + @property + def can_apply_pose(self) -> bool: + """Whether world poses can be written through the backend view.""" + return self.is_ready + + @property + def can_fetch_pose(self) -> bool: + """Whether world poses can be read through the backend view.""" + return self.is_ready + + @property + def is_newton_backend(self) -> bool: + """Whether this view targets the DexSim Newton backend.""" + return False + + # -- Body ID Management ------------------------------------------------- + + @cached_property + @abstractmethod + def body_ids(self) -> list[int]: + """Backend body IDs for all managed entities.""" + ... + + @cached_property + @abstractmethod + def body_ids_tensor(self) -> torch.Tensor: + """Body IDs as an int32 tensor on ``device``.""" + ... + + @abstractmethod + def select_body_ids(self, indices: Sequence[int] | torch.Tensor) -> torch.Tensor: + """Return body IDs for the given entity indices.""" + ... + + # -- Pose --------------------------------------------------------------- + + @abstractmethod + def fetch_pose( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch poses into ``data`` as ``(N, 7)`` in ``(x, y, z, qx, qy, qz, qw)``.""" + ... + + @abstractmethod + def apply_pose(self, pose: torch.Tensor, body_ids: torch.Tensor) -> None: + """Apply poses from ``(N, 7)`` tensor in ``(x, y, z, qx, qy, qz, qw)``.""" + ... + + # -- Center of Mass (local) --------------------------------------------- + + @abstractmethod + def fetch_com_local_pose( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch COM-local poses as ``(x, y, z, qx, qy, qz, qw)``.""" + ... + + @abstractmethod + def apply_com_local_pose(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + """Apply COM-local poses from ``(x, y, z, qx, qy, qz, qw)``.""" + ... + + # -- Velocity ----------------------------------------------------------- + + @abstractmethod + def fetch_linear_velocity( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch linear velocities into ``data`` as ``(N, 3)``.""" + ... + + @abstractmethod + def fetch_angular_velocity( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch angular velocities into ``data`` as ``(N, 3)``.""" + ... + + @abstractmethod + def apply_linear_velocity(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + """Set linear velocities from ``(N, 3)`` tensor.""" + ... + + @abstractmethod + def apply_angular_velocity( + self, data: torch.Tensor, body_ids: torch.Tensor + ) -> None: + """Set angular velocities from ``(N, 3)`` tensor.""" + ... + + # -- Acceleration ------------------------------------------------------- + + @abstractmethod + def fetch_linear_acceleration( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch linear accelerations into ``data`` as ``(N, 3)``.""" + ... + + @abstractmethod + def fetch_angular_acceleration( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch angular accelerations into ``data`` as ``(N, 3)``.""" + ... + + # -- Force & Torque ----------------------------------------------------- + + @abstractmethod + def apply_force(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + """Apply external forces ``(N, 3)``. One-shot — consumed on next step.""" + ... + + @abstractmethod + def apply_torque(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + """Apply external torques ``(N, 3)``. One-shot — consumed on next step.""" + ... + + # -- Physical Properties ------------------------------------------------- + + @abstractmethod + def fetch_mass( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch masses into ``data`` as ``(N, 1)``.""" + ... + + @abstractmethod + def apply_mass(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + """Apply masses from ``(N, 1)`` tensor.""" + ... + + @abstractmethod + def fetch_inertia_diagonal( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch inertia diagonals into ``data`` as ``(N, 3)``.""" + ... + + @abstractmethod + def apply_inertia_diagonal( + self, data: torch.Tensor, body_ids: torch.Tensor + ) -> None: + """Apply inertia diagonals from ``(N, 3)`` tensor.""" + ... + + @abstractmethod + def fetch_friction( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch friction coefficients into ``data`` as ``(N, 1)``.""" + ... + + @abstractmethod + def apply_friction(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + """Apply friction coefficients from ``(N, 1)`` tensor.""" + ... + + @abstractmethod + def fetch_restitution( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch restitution coefficients into ``data`` as ``(N, 1)``.""" + ... + + @abstractmethod + def apply_restitution(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + """Apply restitution coefficients from ``(N, 1)`` tensor.""" + ... + + @abstractmethod + def fetch_contact_offset( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch contact offsets into ``data`` as ``(N, 1)``.""" + ... + + @abstractmethod + def apply_contact_offset(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + """Apply contact offsets from ``(N, 1)`` tensor.""" + ... + + def fetch_damping( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch linear/angular damping into ``data`` as ``(N, 2)``.""" + raise NotImplementedError("This backend view does not expose damping.") + + def apply_damping(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + """Apply linear/angular damping from an ``(N, 2)`` tensor.""" + raise NotImplementedError("This backend view does not expose damping.") + + def fetch_collision_filter( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + """Fetch collision-filter rows into ``data`` as ``(N, 4)``.""" + raise NotImplementedError( + "This backend view does not expose collision filters." + ) + + def apply_collision_filter( + self, data: torch.Tensor, body_ids: torch.Tensor + ) -> None: + """Apply collision-filter rows from an ``(N, 4)`` tensor.""" + raise NotImplementedError( + "This backend view does not expose collision filters." + ) + + +class ArticulationViewBase(ABC): + """Abstract interface for physics-backend articulation data access. + + Public root/link poses use EmbodiChain convention: + ``(x, y, z, qx, qy, qz, qw)``. + """ + + dof: int + """Scalar degree-of-freedom width exposed by the view.""" + + num_links: int + """Link width exposed by the view.""" + + joint_names: list[str] + """Active joint names in public flattened-DOF order.""" + + link_names: list[str] + """Link names in public link-buffer order.""" + + @property + @abstractmethod + def is_ready(self) -> bool: + """Whether backend runtime data can be accessed through batch APIs.""" + ... + + @property + def is_newton_backend(self) -> bool: + """Whether this view targets the DexSim Newton backend.""" + return False + + @property + @abstractmethod + def articulation_ids_tensor(self) -> torch.Tensor | None: + """Backend articulation ids as an int32 tensor, if the backend uses ids.""" + ... + + @abstractmethod + def select_articulation_ids( + self, env_ids: Sequence[int] | torch.Tensor + ) -> torch.Tensor: + """Return backend articulation ids for the given environment ids.""" + ... + + @abstractmethod + def fetch_root_pose(self, data: torch.Tensor) -> torch.Tensor: + """Fetch root poses into ``data`` and return a view/result tensor.""" + ... + + @abstractmethod + def fetch_root_linear_velocity(self, data: torch.Tensor) -> torch.Tensor: + """Fetch root linear velocities into ``data`` and return a tensor.""" + ... + + @abstractmethod + def fetch_root_angular_velocity(self, data: torch.Tensor) -> torch.Tensor: + """Fetch root angular velocities into ``data`` and return a tensor.""" + ... + + @abstractmethod + def fetch_qpos(self, data: torch.Tensor) -> torch.Tensor: + """Fetch current joint positions into ``data``.""" + ... + + @abstractmethod + def fetch_target_qpos(self, data: torch.Tensor) -> torch.Tensor: + """Fetch target joint positions into ``data``.""" + ... + + @abstractmethod + def fetch_qvel(self, data: torch.Tensor) -> torch.Tensor: + """Fetch current joint velocities into ``data``.""" + ... + + @abstractmethod + def fetch_target_qvel(self, data: torch.Tensor) -> torch.Tensor: + """Fetch target joint velocities into ``data``.""" + ... + + @abstractmethod + def fetch_qacc(self, data: torch.Tensor) -> torch.Tensor: + """Fetch current joint accelerations into ``data``.""" + ... + + @abstractmethod + def fetch_qf(self, data: torch.Tensor) -> torch.Tensor: + """Fetch current joint forces into ``data``.""" + ... + + @abstractmethod + def fetch_link_pose(self, data: torch.Tensor) -> torch.Tensor: + """Fetch link poses into ``data``.""" + ... + + @abstractmethod + def fetch_link_velocity( + self, + data: torch.Tensor, + linear_data: torch.Tensor, + angular_data: torch.Tensor, + ) -> torch.Tensor: + """Fetch link velocities into ``data`` using provided scratch buffers.""" + ... + + @abstractmethod + def apply_root_pose( + self, pose: torch.Tensor, env_ids: Sequence[int] | torch.Tensor + ) -> None: + """Apply root poses from EmbodiChain ``xyz + xyzw`` tensors.""" + ... + + @abstractmethod + def apply_qpos( + self, + qpos: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + joint_ids: Sequence[int] | torch.Tensor, + *, + target: bool, + ) -> None: + """Apply joint positions for selected envs and joints.""" + ... + + @abstractmethod + def apply_qvel( + self, + qvel: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + joint_ids: Sequence[int] | torch.Tensor, + *, + target: bool, + ) -> None: + """Apply joint velocities for selected envs and joints.""" + ... + + @abstractmethod + def apply_qf( + self, + qf: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + joint_ids: Sequence[int] | torch.Tensor, + ) -> None: + """Apply joint forces for selected envs and joints.""" + ... + + @abstractmethod + def clear_dynamics(self, env_ids: Sequence[int] | torch.Tensor) -> None: + """Clear joint velocities, target velocities, and forces.""" + ... + + @abstractmethod + def compute_kinematics(self, env_ids: Sequence[int] | torch.Tensor) -> None: + """Refresh articulation kinematics if required by the backend.""" + ... diff --git a/embodichain/lab/sim/objects/backends/newton.py b/embodichain/lab/sim/objects/backends/newton.py new file mode 100644 index 000000000..739ce3c5d --- /dev/null +++ b/embodichain/lab/sim/objects/backends/newton.py @@ -0,0 +1,238 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Newton-specific hooks used by the backend-neutral Scene batch views.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any, Sequence + +import numpy as np +from dexsim.scene import Scene + +if TYPE_CHECKING: + from dexsim.scene import RigidBodyBatch + +__all__ = ["is_newton_scene"] + + +def is_newton_scene(scene: object) -> bool: + """Return whether an object is a current DexSim Newton Scene.""" + return isinstance(scene, Scene) and scene.backend == "newton" + + +def _create_newton_standalone_state_sync( + model: Any, + body_ids: Sequence[int], +) -> Any: + """Create DexSim's reusable FREE-joint synchronization selection.""" + from dexsim.engine.newton_physics.rigid_body.state_sync import ( + StandaloneRigidStateSync, + ) + + return StandaloneRigidStateSync.from_body_ids(model, body_ids) + + +def _synchronize_standalone_rigid_body_state( + scene: Scene, + batch: RigidBodyBatch, + cached: tuple[int, Any, Any] | None, +) -> tuple[int, Any, Any]: + """Synchronize Newton FREE-joint state after a Scene batch write.""" + topology_revision = int(scene.topology_revision) + if cached is None or cached[0] != topology_revision: + # Accessing ``_binding`` refreshes a stale stable batch. DexSim + # currently exposes neither the Newton runtime nor this required + # synchronization through the public Batch API. + binding = batch._binding + runtime = getattr(binding, "_runtime", None) + indices = getattr(binding, "_indices", None) + if runtime is None or indices is None: + raise RuntimeError( + "Newton rigid-body batch has no finalized runtime selection." + ) + state_sync = _create_newton_standalone_state_sync( + runtime.model, + indices.detach().cpu().tolist(), + ) + cached = (topology_revision, runtime, state_sync) + + _, runtime, state_sync = cached + state_sync.synchronize((runtime.current_state, runtime.other_state)) + return cached + + +_DEFAULT_MIMIC_NATURAL_FREQUENCY = 1.0e3 +_DEFAULT_MIMIC_DAMPING_RATIO = 1.0e1 +_MIMIC_FOLLOWER_TARGET_GAIN_RATIO = 1.0e-2 + + +def _default_mujoco_mimic_solref(physics_dt: float, num_substeps: int) -> np.ndarray: + """Approximate Default's mimic compliance with MuJoCo solref. + + Positive MuJoCo solref uses (timeconst, dampratio) and therefore retains + the effective-mass scaling of PhysX articulation mimic joints. MuJoCo's + reference-safety rule clamps timeconst to twice the solver timestep, so + apply the same bound explicitly. + """ + if not np.isfinite(physics_dt) or physics_dt <= 0.0: + raise ValueError("Newton physics_dt must be finite and positive.") + if num_substeps <= 0: + raise ValueError("Newton num_substeps must be positive.") + + solver_dt = physics_dt / num_substeps + natural_time_constant = 1.0 / ( + _DEFAULT_MIMIC_NATURAL_FREQUENCY * _DEFAULT_MIMIC_DAMPING_RATIO + ) + return np.asarray( + ( + max(natural_time_constant, 2.0 * solver_dt), + _DEFAULT_MIMIC_DAMPING_RATIO, + ), + dtype=np.float32, + ) + + +def _configure_newton_mimic_compliance( + *, + result: Scene | None, + entities: Sequence[object], + state_joint_names: Sequence[str], + mimic_ids: Sequence[int], + mimic_parents: Sequence[int], +) -> bool: + """Tune native MuJoCo-Warp mimic constraints toward Default behavior. + + MuJoCo's default equality solref is underdamped relative to Default's + articulation mimic. Map Default's natural-frequency and damping-ratio + parameters to MuJoCo's mass-scaled positive convention as a stable + approximation. A follower drive with one percent of its leader's gains also + tracks the leader's target relation between solver updates. Keeping the + native equality rows enabled preserves mechanical force coupling; the drive + is only a stabilizer and never mirrors measured follower state. + """ + if result is None or result.backend != "newton" or not mimic_ids: + return False + + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(result.world) + if ( + backend is None + or backend.solver_type != "mujoco_warp" + or backend.cfg.requires_grad + or (backend.model is not None and backend.model.requires_grad) + ): + return False + + relation_names = [ + (state_joint_names[child_id], state_joint_names[parent_id]) + for child_id, parent_id in zip(mimic_ids, mimic_parents, strict=True) + ] + first_binding = getattr(entities[0], "_physics_binding", None) + runtime = getattr(first_binding, "_runtime", None) + if runtime is None: + raise RuntimeError("Newton Scene articulation has no finalized runtime.") + + model = runtime.model + target_ke = np.asarray(model.joint_target_ke.numpy()).reshape(-1) + target_kd = np.asarray(model.joint_target_kd.numpy()).reshape(-1) + target_mode = np.asarray(model.joint_target_mode.numpy()).reshape(-1) + expected_pairs: set[tuple[int, int]] = set() + for entity in entities: + binding = getattr(entity, "_physics_binding", None) + if binding is None or getattr(binding, "_runtime", None) is not runtime: + raise RuntimeError( + "Newton mimic configuration requires one shared finalized runtime." + ) + runtime_joints = {joint.name: joint for joint in binding.joints} + follower_ke: list[float] = [] + follower_kd: list[float] = [] + follower_mode: list[int] = [] + for child_name, parent_name in relation_names: + try: + child = runtime_joints[child_name] + parent = runtime_joints[parent_name] + except KeyError as error: + raise RuntimeError( + "Newton mimic metadata references a missing runtime joint." + ) from error + if int(child.qd_size) != 1 or int(parent.qd_size) != 1: + raise NotImplementedError( + "MuJoCo-Warp mimic compliance requires scalar joints." + ) + expected_pairs.add((int(child.joint_id), int(parent.joint_id))) + parent_dof = int(parent.qd_start) + follower_ke.append( + float(target_ke[parent_dof]) * _MIMIC_FOLLOWER_TARGET_GAIN_RATIO + ) + follower_kd.append( + float(target_kd[parent_dof]) * _MIMIC_FOLLOWER_TARGET_GAIN_RATIO + ) + follower_mode.append(int(target_mode[parent_dof])) + + configured = entity.set_newton_drive( + joint_ids=np.asarray(mimic_ids, dtype=np.int32), + target_ke=np.asarray(follower_ke, dtype=np.float32), + target_kd=np.asarray(follower_kd, dtype=np.float32), + target_mode=np.asarray(follower_mode, dtype=np.int32), + ) + if configured != len(mimic_ids): + raise RuntimeError( + "Newton failed to configure every mimic follower stabilizer." + ) + + mimic_joint0 = np.asarray(model.constraint_mimic_joint0.numpy()).reshape(-1) + mimic_joint1 = np.asarray(model.constraint_mimic_joint1.numpy()).reshape(-1) + row_by_pair = { + (int(child), int(parent)): row + for row, (child, parent) in enumerate( + zip(mimic_joint0, mimic_joint1, strict=True) + ) + } + try: + constraint_rows = np.asarray( + [row_by_pair[pair] for pair in expected_pairs], dtype=np.int32 + ) + except KeyError as error: + raise RuntimeError( + f"Newton model has no mimic constraint for joint pair {error.args[0]}." + ) from error + + solver = runtime.solver + mapping = getattr(solver, "mjc_eq_to_newton_mimic", None) + mjw_model = getattr(solver, "mjw_model", None) + if mapping is None or mjw_model is None: + raise RuntimeError("MuJoCo-Warp did not materialize Newton mimic rows.") + + mapping_values = np.asarray(mapping.numpy()) + selected = np.isin(mapping_values, constraint_rows) + if int(selected.sum()) != len(constraint_rows): + raise RuntimeError( + "MuJoCo-Warp mimic row mapping does not match the articulation." + ) + eq_solref = np.asarray(mjw_model.eq_solref.numpy()).copy() + mimic_solref = _default_mujoco_mimic_solref( + float(backend.cfg.dt), + int(backend.cfg.num_substeps), + ) + eq_solref[selected] = mimic_solref + mjw_model.eq_solref.assign(eq_solref) + + # Keep the optional CPU mirror coherent for debugging and CPU execution. + mj_model = getattr(solver, "mj_model", None) + if mj_model is not None and len(eq_solref) > 0: + mj_model.eq_solref[:] = eq_solref[0] + return True diff --git a/embodichain/lab/sim/objects/backends/scene.py b/embodichain/lab/sim/objects/backends/scene.py new file mode 100644 index 000000000..560da5f7c --- /dev/null +++ b/embodichain/lab/sim/objects/backends/scene.py @@ -0,0 +1,681 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""EmbodiChain views over backend-neutral :mod:`dexsim.scene` batches. + +Backend selection, handle rebinding, and topology revision tracking remain +owned by DexSim's ``Scene`` and batch classes. EmbodiChain adapts logical row +selections and its public pose convention ``(x, y, z, qx, qy, qz, qw)``; +backend-specific native compatibility work is delegated to narrow hooks. + +Row and DOF selection is delegated to DexSim's public batches. +""" + +from __future__ import annotations + +from numbers import Integral +from typing import TYPE_CHECKING, Any, Sequence + +import torch + +from .base import ArticulationViewBase, RigidBodyViewBase + +if TYPE_CHECKING: + from dexsim.scene import ( + ArticulationBatch, + RigidBodyBatch, + Scene, + SpawnedArticulation, + SpawnedRigidBody, + ) + +__all__ = ["SceneArticulationView", "SceneRigidBodyView"] + +_NEWTON_ROOT_POSE_ATOL = 1.0e-6 + + +def _checked_batch_call( + batch: Any, + method_name: str, + /, + *args: Any, + **kwargs: Any, +) -> Any: + """Call one Scene batch operation and reject native failure statuses.""" + status = getattr(batch, method_name)(*args, **kwargs) + if isinstance(status, Integral) and status < 0: + raise RuntimeError( + f"DexSim Scene batch operation {method_name!r} failed with " + f"status {status}." + ) + return status + + +def _rows( + selection: Sequence[int] | torch.Tensor | None, + count: int, + device: torch.device, +) -> torch.Tensor: + if selection is None: + return torch.arange(count, dtype=torch.long, device=device) + result = torch.as_tensor(selection, dtype=torch.long, device=device).reshape(-1) + if torch.any(result < 0) or torch.any(result >= count): + raise IndexError(f"Batch row selection is outside [0, {count}).") + return result + + +def _batch_pose(data: torch.Tensor) -> torch.Tensor: + """Convert rigid-body ``xyz+xyzw`` poses to batch ``xyzw+xyz``.""" + result = torch.empty_like(data, dtype=torch.float32) + result[..., 0:4] = data[..., 3:7] + result[..., 4:7] = data[..., 0:3] + return result + + +def _embodichain_pose(data: torch.Tensor) -> torch.Tensor: + """Convert batch ``xyzw+xyz`` poses to rigid-body ``xyz+xyzw``.""" + result = torch.empty_like(data, dtype=torch.float32) + result[..., 0:3] = data[..., 4:7] + result[..., 3:7] = data[..., 0:4] + return result + + +class _SceneBatchSelectionAdapter: + """Shared row-selection support for fixed-size Scene batches.""" + + def __init__(self, batch: Any, device: torch.device, row_count: int) -> None: + self.batch = batch + self.device = device + self._row_count = row_count + + def _fetch_rows( + self, + method_name: str, + out: torch.Tensor, + selection: Sequence[int] | torch.Tensor | None, + tail_shape: tuple[int, ...], + ) -> torch.Tensor: + if selection is None: + expected_shape = (self._row_count, *tail_shape) + if tuple(out.shape) != expected_shape: + raise ValueError( + f"Expected batch output shape {expected_shape}, got " + f"{tuple(out.shape)}." + ) + _checked_batch_call(self.batch, method_name, out) + return out + + rows = _rows(selection, self._row_count, self.device) + expected_shape = (len(rows), *tail_shape) + if tuple(out.shape) != expected_shape: + raise ValueError( + f"Expected selected output shape {expected_shape}, got " + f"{tuple(out.shape)}." + ) + if len(rows): + _checked_batch_call(self.batch.select(rows), method_name, out) + return out + + def _apply_rows( + self, + method_name: str, + values: torch.Tensor, + selection: Sequence[int] | torch.Tensor, + tail_shape: tuple[int, ...], + ) -> None: + rows = _rows(selection, self._row_count, self.device) + expected_shape = (len(rows), *tail_shape) + if tuple(values.shape) != expected_shape: + raise ValueError( + f"Expected selected data shape {expected_shape}, got " + f"{tuple(values.shape)}." + ) + if len(rows): + _checked_batch_call(self.batch.select(rows), method_name, values) + + +class SceneRigidBodyView(_SceneBatchSelectionAdapter, RigidBodyViewBase): + """Backend-neutral rigid-body view backed by ``RigidBodyBatch``.""" + + @classmethod + def from_entities( + cls, + scene: Scene, + entities: Sequence[SpawnedRigidBody], + device: torch.device, + ) -> SceneRigidBodyView: + """Create a view from rigid-body handles owned by ``scene``.""" + return cls( + scene, + scene.create_rigid_body_batch(list(entities)), + device, + ) + + def __init__( + self, + scene: Scene, + batch: RigidBodyBatch, + device: torch.device, + ) -> None: + super().__init__(batch, device, len(batch)) + self.scene = scene + self._body_ids_tensor = torch.arange( + len(batch), dtype=torch.int32, device=device + ) + self._newton_state_sync: tuple[int, Any, Any] | None = None + + @property + def is_ready(self) -> bool: + return True + + @property + def is_newton_backend(self) -> bool: + return self.scene.backend == "newton" + + @property + def body_ids(self) -> list[int]: + return list(range(self._row_count)) + + @property + def body_ids_tensor(self) -> torch.Tensor: + return self._body_ids_tensor + + def select_body_ids(self, indices: Sequence[int] | torch.Tensor) -> torch.Tensor: + return self._body_ids_tensor[indices] + + def fetch_pose( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + batch_pose = torch.empty( + (len(data), 7), dtype=torch.float32, device=self.device + ) + self._fetch_rows("fetch_pose", batch_pose, body_ids, (7,)) + data.copy_(_embodichain_pose(batch_pose).to(data.device, data.dtype)) + + def apply_pose(self, pose: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows( + "apply_pose", + _batch_pose(pose.to(self.device, torch.float32)), + body_ids, + (7,), + ) + if self.is_newton_backend and len(body_ids): + self._synchronize_newton_standalone_state() + + def _synchronize_newton_standalone_state(self) -> None: + """Keep Newton standalone-body FREE joints coherent after state writes. + + DexSim 0.4.3's device ``RigidBodyBatch`` state writes update maximal + ``body_q`` or ``body_qd`` state, while MuJoCo-Warp advances standalone + rigid bodies from their reduced FREE-joint state. Cache one selection + for this stable batch and project both state buffers after each write. + """ + from .newton import _synchronize_standalone_rigid_body_state + + self._newton_state_sync = _synchronize_standalone_rigid_body_state( + self.scene, + self.batch, + self._newton_state_sync, + ) + + def fetch_com_local_pose( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + batch_pose = torch.empty( + (len(data), 7), dtype=torch.float32, device=self.device + ) + self._fetch_rows("fetch_com_local_pose", batch_pose, body_ids, (7,)) + data.copy_(_embodichain_pose(batch_pose).to(data.device, data.dtype)) + + def apply_com_local_pose(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows( + "apply_com_local_pose", + _batch_pose(data.to(self.device, torch.float32)), + body_ids, + (7,), + ) + + def fetch_linear_velocity( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_linear_velocity", data, body_ids, (3,)) + + def fetch_angular_velocity( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_angular_velocity", data, body_ids, (3,)) + + def apply_linear_velocity(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows( + "apply_linear_velocity", + data, + body_ids, + (3,), + ) + if self.is_newton_backend and len(body_ids): + self._synchronize_newton_standalone_state() + + def apply_angular_velocity( + self, data: torch.Tensor, body_ids: torch.Tensor + ) -> None: + self._apply_rows( + "apply_angular_velocity", + data, + body_ids, + (3,), + ) + if self.is_newton_backend and len(body_ids): + self._synchronize_newton_standalone_state() + + def fetch_linear_acceleration( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_linear_acceleration", data, body_ids, (3,)) + + def fetch_angular_acceleration( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_angular_acceleration", data, body_ids, (3,)) + + def apply_force(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows("apply_force", data, body_ids, (3,)) + + def apply_torque(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows("apply_torque", data, body_ids, (3,)) + + def fetch_mass( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_mass", data, body_ids, (1,)) + + def apply_mass(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows("apply_mass", data, body_ids, (1,)) + + def fetch_inertia_diagonal( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_inertia_diagonal", data, body_ids, (3,)) + + def apply_inertia_diagonal( + self, data: torch.Tensor, body_ids: torch.Tensor + ) -> None: + self._apply_rows( + "apply_inertia_diagonal", + data, + body_ids, + (3,), + ) + + def fetch_friction( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_friction", data, body_ids, (1,)) + + def apply_friction(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows("apply_friction", data, body_ids, (1,)) + + def fetch_restitution( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_restitution", data, body_ids, (1,)) + + def apply_restitution(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows("apply_restitution", data, body_ids, (1,)) + + def fetch_contact_offset( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_contact_offset", data, body_ids, (1,)) + + def apply_contact_offset(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows("apply_contact_offset", data, body_ids, (1,)) + + def fetch_damping( + self, data: torch.Tensor, body_ids: torch.Tensor | None = None + ) -> None: + self._fetch_rows("fetch_damping", data, body_ids, (2,)) + + def apply_damping(self, data: torch.Tensor, body_ids: torch.Tensor) -> None: + self._apply_rows("apply_damping", data, body_ids, (2,)) + + def fetch_collision_filter( + self, + data: torch.Tensor, + body_ids: torch.Tensor | None = None, + ) -> None: + self._fetch_rows("fetch_collision_filter", data, body_ids, (4,)) + + def apply_collision_filter( + self, + data: torch.Tensor, + body_ids: torch.Tensor, + ) -> None: + self._apply_rows("apply_collision_filter", data, body_ids, (4,)) + + +class SceneArticulationView(_SceneBatchSelectionAdapter, ArticulationViewBase): + """Backend-neutral articulation state view backed by ``ArticulationBatch``. + + Joint selections currently require one scalar DOF per selected joint. The + public DexSim layout already describes multi-DOF joints; supporting them + without ambiguity requires a DOF-selection API in DexSim and is therefore + kept as an explicit boundary rather than guessed here. + """ + + @classmethod + def from_entities( + cls, + scene: Scene, + entities: Sequence[SpawnedArticulation], + device: torch.device, + ) -> SceneArticulationView: + """Create a view from articulation handles owned by ``scene``.""" + return cls( + scene, + scene.create_articulation_batch(list(entities)), + device, + ) + + def __init__( + self, + scene: Scene, + batch: ArticulationBatch, + device: torch.device, + ) -> None: + super().__init__(batch, device, len(batch)) + self.scene = scene + self._validate_homogeneous_layout() + self._articulation_ids = torch.arange( + len(batch), dtype=torch.int32, device=device + ) + + def _validate_homogeneous_layout(self) -> None: + """Require the uniform topology promised by one EC Articulation.""" + dof_counts = tuple(self.batch.dof_counts) + link_counts = tuple(self.batch.link_counts) + joint_names = tuple(self.batch.joint_names_per_articulation) + link_names = tuple(self.batch.link_names_per_articulation) + if dof_counts and len(set(dof_counts)) != 1: + raise ValueError( + "One EmbodiChain Articulation cannot bind heterogeneous Scene " + f"DOF counts: {dof_counts}." + ) + if link_counts and len(set(link_counts)) != 1: + raise ValueError( + "One EmbodiChain Articulation cannot bind heterogeneous Scene " + f"link counts: {link_counts}." + ) + if joint_names and any(names != joint_names[0] for names in joint_names[1:]): + raise ValueError( + "One EmbodiChain Articulation requires identical active-joint " + "ordering in every Scene row." + ) + if link_names and any(names != link_names[0] for names in link_names[1:]): + raise ValueError( + "One EmbodiChain Articulation requires identical link ordering " + "in every Scene row." + ) + layouts = tuple(self.batch.joint_layouts_per_articulation) + if layouts and any(layout.dof_count != 1 for layout in layouts[0]): + raise NotImplementedError( + "EmbodiChain's Articulation API currently indexes joints and " + "scalar DOFs interchangeably. Scene multi-DOF joints require " + "an explicit DOF-selection API before they can be bound safely." + ) + + @property + def dof(self) -> int: + """Scalar DOF width shared by every articulation row.""" + return self.batch.dof_width + + @property + def num_links(self) -> int: + """Link count shared by every articulation row.""" + return self.batch.link_width + + @property + def joint_names(self) -> list[str]: + """Active joints in public flattened-DOF order.""" + rows = self.batch.joint_names_per_articulation + return [] if not rows else list(rows[0]) + + @property + def link_names(self) -> list[str]: + """Links in public link-buffer order.""" + rows = self.batch.link_names_per_articulation + return [] if not rows else list(rows[0]) + + @property + def is_ready(self) -> bool: + return True + + @property + def is_newton_backend(self) -> bool: + return self.scene.backend == "newton" + + @property + def articulation_ids_tensor(self) -> torch.Tensor: + return self._articulation_ids + + def select_articulation_ids( + self, env_ids: Sequence[int] | torch.Tensor + ) -> torch.Tensor: + return self._articulation_ids[env_ids] + + def fetch_root_pose(self, data: torch.Tensor) -> torch.Tensor: + batch_pose = torch.empty_like(data, dtype=torch.float32, device=self.device) + _checked_batch_call(self.batch, "fetch_root_pose", batch_pose) + data.copy_(_embodichain_pose(batch_pose).to(data.device, data.dtype)) + return data + + def fetch_root_linear_velocity(self, data: torch.Tensor) -> torch.Tensor: + _checked_batch_call(self.batch, "fetch_root_linear_velocity", data) + return data + + def fetch_root_angular_velocity(self, data: torch.Tensor) -> torch.Tensor: + _checked_batch_call(self.batch, "fetch_root_angular_velocity", data) + return data + + def fetch_qpos(self, data: torch.Tensor) -> torch.Tensor: + _checked_batch_call(self.batch, "fetch_joint_position", data) + return data + + def fetch_target_qpos(self, data: torch.Tensor) -> torch.Tensor: + _checked_batch_call(self.batch, "fetch_joint_target_position", data) + return data + + def fetch_qvel(self, data: torch.Tensor) -> torch.Tensor: + _checked_batch_call(self.batch, "fetch_joint_velocity", data) + return data + + def fetch_target_qvel(self, data: torch.Tensor) -> torch.Tensor: + _checked_batch_call(self.batch, "fetch_joint_target_velocity", data) + return data + + def fetch_qacc(self, data: torch.Tensor) -> torch.Tensor: + _checked_batch_call(self.batch, "fetch_joint_acceleration", data) + return data + + def fetch_qf(self, data: torch.Tensor) -> torch.Tensor: + _checked_batch_call(self.batch, "fetch_joint_force", data) + return data + + def fetch_link_pose(self, data: torch.Tensor) -> torch.Tensor: + batch_pose = torch.empty_like(data, dtype=torch.float32, device=self.device) + _checked_batch_call(self.batch, "fetch_link_pose", batch_pose) + data.copy_(_embodichain_pose(batch_pose).to(data.device, data.dtype)) + return data + + def fetch_link_velocity( + self, + data: torch.Tensor, + linear_data: torch.Tensor, + angular_data: torch.Tensor, + ) -> torch.Tensor: + _checked_batch_call(self.batch, "fetch_link_linear_velocity", linear_data) + _checked_batch_call(self.batch, "fetch_link_angular_velocity", angular_data) + data[..., 0:3] = linear_data + data[..., 3:6] = angular_data + return data + + def apply_root_pose( + self, pose: torch.Tensor, env_ids: Sequence[int] | torch.Tensor + ) -> None: + rows = _rows(env_ids, self._row_count, self.device) + batch_pose = _batch_pose(pose.to(self.device, torch.float32)) + expected_shape = (len(rows), 7) + if tuple(batch_pose.shape) != expected_shape: + raise ValueError( + f"Expected selected data shape {expected_shape}, got " + f"{tuple(batch_pose.shape)}." + ) + if self.is_newton_backend and len(rows): + current_pose = torch.empty_like(batch_pose) + self._fetch_rows( + "fetch_root_pose", + current_pose, + rows, + (7,), + ) + translation_matches = torch.all( + torch.abs(current_pose[:, 4:7] - batch_pose[:, 4:7]) + <= _NEWTON_ROOT_POSE_ATOL, + dim=1, + ) + quaternion_delta = torch.minimum( + torch.amax(torch.abs(current_pose[:, 0:4] - batch_pose[:, 0:4]), dim=1), + torch.amax(torch.abs(current_pose[:, 0:4] + batch_pose[:, 0:4]), dim=1), + ) + changed = ~( + translation_matches & (quaternion_delta <= _NEWTON_ROOT_POSE_ATOL) + ) + rows = rows[changed] + batch_pose = batch_pose[changed] + + self._apply_rows( + "apply_root_pose", + batch_pose, + rows, + (7,), + ) + + def _joint_columns(self, joint_ids: Sequence[int] | torch.Tensor) -> torch.Tensor: + layouts = self.batch.joint_layouts_per_articulation + if not layouts: + return _rows(joint_ids, self.dof, self.device) + reference = layouts[0] + ids = _rows(joint_ids, len(reference), self.device) + columns: list[int] = [] + for joint_id in ids.detach().cpu().tolist(): + layout = reference[joint_id] + if layout.dof_count != 1: + raise NotImplementedError( + "SceneArticulationView needs DexSim DOF selection for " + f"multi-DOF joint {layout.name!r}." + ) + columns.append(layout.dof_start) + return torch.as_tensor(columns, dtype=torch.long, device=self.device) + + def _apply_joint_selection( + self, + values: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + joint_ids: Sequence[int] | torch.Tensor, + *, + apply_method: str, + ) -> None: + rows = _rows(env_ids, self._row_count, self.device) + columns = self._joint_columns(joint_ids) + values = values.to(device=self.device, dtype=torch.float32) + expected = (len(rows), len(columns)) + if tuple(values.shape) != expected: + raise ValueError( + f"Expected selected joint data shape {expected}, got " + f"{tuple(values.shape)}." + ) + if len(rows) and len(columns): + _checked_batch_call( + self.batch.select(rows), + apply_method, + values, + dof_ids=columns, + ) + + def apply_qpos( + self, + qpos: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + joint_ids: Sequence[int] | torch.Tensor, + *, + target: bool, + ) -> None: + self._apply_joint_selection( + qpos, + env_ids, + joint_ids, + apply_method=( + "apply_joint_target_position" if target else "apply_joint_position" + ), + ) + + def apply_qvel( + self, + qvel: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + joint_ids: Sequence[int] | torch.Tensor, + *, + target: bool, + ) -> None: + self._apply_joint_selection( + qvel, + env_ids, + joint_ids, + apply_method=( + "apply_joint_target_velocity" if target else "apply_joint_velocity" + ), + ) + + def apply_qf( + self, + qf: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + joint_ids: Sequence[int] | torch.Tensor, + ) -> None: + self._apply_joint_selection( + qf, + env_ids, + joint_ids, + apply_method="apply_joint_force", + ) + + def clear_dynamics(self, env_ids: Sequence[int] | torch.Tensor) -> None: + rows = _rows(env_ids, self._row_count, self.device) + if not len(rows): + return + zeros = torch.zeros( + (len(rows), self.batch.dof_width), + dtype=torch.float32, + device=self.device, + ) + selected = self.batch.select(rows) + _checked_batch_call(selected, "apply_joint_velocity", zeros) + _checked_batch_call(selected, "apply_joint_target_velocity", zeros) + _checked_batch_call(selected, "apply_joint_force", zeros) + + def compute_kinematics(self, env_ids: Sequence[int] | torch.Tensor) -> None: + rows = _rows(env_ids, self._row_count, self.device) + if not len(rows): + return + _checked_batch_call(self.batch.select(rows), "compute_kinematics") diff --git a/embodichain/lab/sim/objects/cloth_object.py b/embodichain/lab/sim/objects/cloth_object.py deleted file mode 100644 index ef8419080..000000000 --- a/embodichain/lab/sim/objects/cloth_object.py +++ /dev/null @@ -1,454 +0,0 @@ -# ---------------------------------------------------------------------------- -# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# ---------------------------------------------------------------------------- - -from __future__ import annotations - -import torch -import dexsim -import numpy as np -from functools import cached_property - -from dataclasses import dataclass -from typing import List, Sequence, Union - -from dexsim.models import MeshObject -from dexsim.engine import ClothBody, PhysicsScene -from dexsim.types import ClothBodyGPUAPIReadWriteType -from scipy.spatial import cKDTree -from embodichain.lab.sim.common import ( - BatchEntity, -) -from embodichain.lab.sim.material import ( - VisualMaterial, - VisualMaterialInst, - _capture_render_materials, - _restore_render_materials, - _wrap_first_render_material, -) -from embodichain.utils.math import ( - matrix_from_euler, -) -from embodichain.utils import logger -from embodichain.lab.sim.cfg import ( - ClothObjectCfg, -) -from embodichain.utils.math import xyz_quat_to_4x4_matrix - -__all__ = ["ClothBodyData", "ClothObject", "ClothObjectCfg"] - - -@dataclass -class ClothBodyData: - """Data manager for cloth. - - Note: - 1. The pose data managed by dexsim is in the format of (qx, qy, qz, qw, x, y, z), but in EmbodiChain, we use (x, y, z, qw, qx, qy, qz) format. - """ - - def __init__( - self, entities: List[MeshObject], ps: PhysicsScene, device: torch.device - ) -> None: - """Initialize the ClothBodyData. - - Args: - entities (List[MeshObject]): List of MeshObjects representing the cloth bodies. - ps (PhysicsScene): The physics scene. - device (torch.device): The device to use for the cloth body data. - """ - self.entities = entities - # TODO: cloth body data can only be stored in cuda device for now. - self.device = device - # TODO: inorder to retrieve arena position, we need to access the node of each entity. - self.ps = ps - self.num_instances = len(entities) - - self.cloth_bodies: Sequence[ClothBody] = [ - self.entities[i].get_physical_body() for i in range(self.num_instances) - ] - self.n_vertices = self.cloth_bodies[0].get_num_vertices() - - self._rest_position_buffer = torch.empty( - (self.num_instances, self.n_vertices, 4), - device=self.device, - dtype=torch.float32, - ) - for i, cloth_body in enumerate(self.cloth_bodies): - self._rest_position_buffer[i] = cloth_body.get_position_inv_mass_buffer() - - self._vertex_position = torch.zeros( - (self.num_instances, self.n_vertices, 3), - device=self.device, - dtype=torch.float32, - ) - - self._vertex_velocity = torch.zeros( - (self.num_instances, self.n_vertices, 3), - device=self.device, - dtype=torch.float32, - ) - - @property - def rest_vertices(self): - """Get the rest position buffer of the cloth bodies.""" - return self._rest_position_buffer[:, :, :3].clone() - - @property - def vertex_position(self): - """Get the current vertex position buffer of the cloth bodies.""" - for i, clothbody in enumerate(self.cloth_bodies): - self._vertex_position[i] = clothbody.get_position_inv_mass_buffer()[:, :3] - return self._vertex_position.clone() - - @property - def vertex_velocity(self): - """Get the current vertex velocity buffer of the cloth bodies.""" - for i, clothbody in enumerate(self.cloth_bodies): - self._vertex_velocity[i] = clothbody.get_velocity_buffer()[:, 3:] - return self._vertex_velocity.clone() - - -class ClothObject(BatchEntity): - """ClothObject represents a batch of cloth body in the simulation.""" - - def __init__( - self, - cfg: ClothObjectCfg, - entities: List[MeshObject] = None, - device: torch.device = torch.device("cpu"), - ) -> None: - self._world: dexsim.World = dexsim.default_world() - self._ps = self._world.get_physics_scene() - self._all_indices = torch.arange(len(entities), dtype=torch.int32).tolist() - - self._data = ClothBodyData(entities=entities, ps=self._ps, device=device) - - self._world.update(0.001) - self._surface_triangles = self._build_surface_triangles( - entities[0], - self._data.rest_vertices[0].detach().cpu().numpy(), - ) - - self._visual_material: List[VisualMaterialInst | None] = [None] * len(entities) - self.is_shared_visual_material = False - - super().__init__(cfg=cfg, entities=entities, device=device) - - self._initialize_existing_visual_material() - - self._set_default_collision_filter() - - @staticmethod - def _build_surface_triangles( - entity: MeshObject, - rest_vertices: np.ndarray, - ) -> np.ndarray: - """Map render triangles onto DexSim's welded cloth vertex buffer.""" - render_body = entity.get_render_body() - render_vertices: list[np.ndarray] = [] - render_triangles: list[np.ndarray] = [] - vertex_offset = 0 - for mesh_id in range(render_body.get_mesh_count()): - vertices = np.asarray( - render_body.get_vertices(mesh_id), - dtype=np.float32, - ) - triangles = np.asarray( - render_body.get_triangles(mesh_id), - dtype=np.int64, - ) - render_vertices.append(vertices) - render_triangles.append(triangles + vertex_offset) - vertex_offset += len(vertices) - - vertices = np.concatenate(render_vertices, axis=0) - triangles = np.concatenate(render_triangles, axis=0) - distances, cloth_vertex_ids = cKDTree(rest_vertices).query(vertices) - scale = max(float(np.ptp(rest_vertices, axis=0).max()), 1.0) - if float(distances.max(initial=0.0)) > scale * 1.0e-5: - raise RuntimeError( - "Could not map cloth render vertices onto the physical vertex buffer." - ) - return np.asarray(cloth_vertex_ids[triangles], dtype=np.int32) - - def _initialize_existing_visual_material(self) -> None: - """Wrap asset-parsed materials during cloth-object construction. - - For a multi-segment render body, the first segment with a valid - material is registered as the environment's representative material. - """ - self._original_visual_material = [[] for _ in self._entities] - self._original_visual_material_inst = [None] * len(self._entities) - for env_idx, entity in enumerate(self._entities): - render_body = entity.get_render_body() - if render_body is None: - continue - original_materials = _capture_render_materials(render_body) - self._original_visual_material[env_idx] = original_materials - wrapped = _wrap_first_render_material(original_materials) - if wrapped is not None: - self._visual_material[env_idx] = wrapped - self._original_visual_material_inst[env_idx] = wrapped - - def set_visual_material( - self, - mat: VisualMaterial, - env_ids: Sequence[int] | None = None, - shared: bool = False, - ) -> None: - """Set visual material for the cloth object. - - Args: - mat: The material template to assign. - env_ids: Environment indices. If None, all instances are used. - shared: Whether selected environments share one material instance. - """ - local_env_ids = self._all_indices if env_ids is None else env_ids - if shared: - if len(local_env_ids) != self.num_instances: - logger.log_error("Cannot share material instance for partial env_ids.") - mat_inst = mat.create_instance(f"{mat.uid}_{self.uid}") - for env_idx in local_env_ids: - self._entities[env_idx].set_material(mat_inst.mat) - self._visual_material[env_idx] = mat_inst - self.is_shared_visual_material = True - else: - for env_idx in local_env_ids: - mat_inst = mat.create_instance(f"{mat.uid}_{self.uid}_{env_idx}") - self._entities[env_idx].set_material(mat_inst.mat) - self._visual_material[env_idx] = mat_inst - self.is_shared_visual_material = False - - def restore_visual_material(self, env_ids: Sequence[int] | None = None) -> None: - """Restore visual materials captured when the cloth object was created. - - Args: - env_ids: Environment indices. If None, all instances are restored. - """ - if not hasattr(self, "_original_visual_material"): - return - local_env_ids = self._all_indices if env_ids is None else env_ids - for env_idx in local_env_ids: - render_body = self._entities[env_idx].get_render_body() - if render_body is None: - continue - _restore_render_materials( - render_body, self._original_visual_material[env_idx] - ) - self._visual_material[env_idx] = self._original_visual_material_inst[ - env_idx - ] - self.is_shared_visual_material = False - - def get_visual_material_inst( - self, env_ids: Sequence[int] | None = None - ) -> List[VisualMaterialInst | None]: - """Get the material instance registered for each selected environment. - - Args: - env_ids: Environment indices. If None, all instances are returned. - - Returns: - The existing material wrappers, or None where an asset has no material. - """ - ids = env_ids if env_ids is not None else range(self.num_instances) - return [self._visual_material[i] for i in ids] - - def _set_default_collision_filter(self) -> None: - collision_filter_data = torch.zeros( - size=(self.num_instances, 4), dtype=torch.int32 - ) - for i in range(self.num_instances): - collision_filter_data[i, 0] = i - collision_filter_data[i, 1] = 1 - self.set_collision_filter(collision_filter_data) - - def set_collision_filter( - self, filter_data: torch.Tensor, env_ids: Sequence[int] | None = None - ) -> None: - """Set collision filter data for the cloth object. - - Args: - filter_data (torch.Tensor): [N, 4] of int. - First element of each object is arena id. - If 2nd element is 0, the object will collision with all other objects in world. - 3rd and 4th elements are not used currently. - - env_ids (Sequence[int] | None): Environment indices. If None, then all indices are used. - """ - local_env_ids = self._all_indices if env_ids is None else env_ids - - if len(local_env_ids) != len(filter_data): - logger.log_error( - f"Length of env_ids {len(local_env_ids)} does not match pose length {len(filter_data)}." - ) - - filter_data_np = filter_data.cpu().numpy().astype(np.uint32) - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].get_physical_body().set_collision_filter_data( - filter_data_np[i] - ) - - @property - def body_data(self) -> ClothBodyData | None: - """Get the cloth body data manager for this cloth object. - - Returns: - ClothBodyData | None: The cloth body data manager. - """ - return self._data - - def get_rest_vertex_position(self) -> torch.Tensor: - """Get the rest vertex position of the cloth bodies. - - Returns: - torch.Tensor: The rest vertex position of the cloth bodies, shape (num_instances, n_vertices, 3). - """ - return self._data.rest_vertices - - def get_current_vertex_position(self) -> torch.Tensor: - """Get the current vertex position of the cloth bodies. - - Returns: - torch.Tensor: The current vertex position of the cloth bodies, shape (num_instances, n_vertices, 3). - """ - return self._data.vertex_position - - def get_current_vertex_velocity(self) -> torch.Tensor: - """Get the current vertex velocity of the cloth bodies. - - Returns: - torch.Tensor: The current vertex velocity of the cloth bodies, shape (num_instances, n_vertices, 3). - """ - return self._data.vertex_velocity - - def get_triangles(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: - """Get surface triangle indices for selected cloth instances. - - Args: - env_ids: Environment indices. If ``None``, returns all instances. - - Returns: - Triangle indices with shape ``(N, num_triangles, 3)``. - """ - ids = self._all_indices if env_ids is None else env_ids - triangles = torch.as_tensor( - self._surface_triangles, - dtype=torch.int32, - device=self.device, - ) - return triangles.unsqueeze(0).expand(len(ids), -1, -1).clone() - - def set_local_pose( - self, pose: torch.Tensor, env_ids: Sequence[int] | None = None - ) -> None: - """Set local pose of the cloth object. - - Args: - pose (torch.Tensor): The local pose of the cloth object with shape (N, 7) or (N, 4, 4). - env_ids (Sequence[int] | None): Environment indices. If None, then all indices are used. - """ - from embodichain.lab.sim import SimulationManager - - sim = SimulationManager.get_instance() - - local_env_ids = self._all_indices if env_ids is None else env_ids - - if len(local_env_ids) != len(pose): - logger.log_error( - f"Length of env_ids {len(local_env_ids)} does not match pose length {len(pose)}." - ) - - if pose.dim() == 2 and pose.shape[1] == 7: - pose4x4 = xyz_quat_to_4x4_matrix(pose) - elif pose.dim() == 3 and pose.shape[1:3] == (4, 4): - pose4x4 = pose - else: - logger.log_error( - f"Invalid pose shape {pose.shape}. Expected (N, 7) or (N, 4, 4)." - ) - - arena_offsets = sim.arena_offsets - for i, env_idx in enumerate(local_env_ids): - # TODO: cloth body cannot directly set by `set_local_pose` currently. - rest_vertices = self.body_data.rest_vertices[i] - rotation = pose4x4[i][:3, :3] - translation = pose4x4[i][:3, 3] - - # apply transformation to local rest vertices and back - rest_vertices_local = rest_vertices - arena_offsets[i] - transformed_vertices = rest_vertices_local @ rotation.T + translation - transformed_vertices = transformed_vertices + arena_offsets[i] - - cloth_body: ClothBody = self._entities[env_idx].get_physical_body() - position_buffer = cloth_body.get_position_inv_mass_buffer() - velocity_buffer = cloth_body.get_velocity_buffer() - position_buffer[:, :3] = transformed_vertices - velocity_buffer[:, 3:] = 0.0 - - cloth_body.mark_dirty(ClothBodyGPUAPIReadWriteType.ALL) - # TODO: currently cloth body has no wake up interface, use set_wake_counter and pass in a positive value to wake it up - cloth_body.set_wake_counter(0.4) - - def get_local_pose(self, to_matrix=False): - """Get local pose of the cloth object. - - Args: - to_matrix (bool, optional): If True, return the pose as a 4x4 matrix. If False, return as (x, y, z, qw, qx, qy, qz). Defaults to False. - - Returns: - torch.Tensor: The local pose of the cloth object with shape (N, 7) or (N, 4, 4) depending on `to_matrix`. - """ - raise NotImplementedError( - "Getting local pose for ClothObject is not supported." - ) - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - local_env_ids = self._all_indices if env_ids is None else env_ids - num_instances = len(local_env_ids) - - self.restore_visual_material(env_ids=local_env_ids) - - # TODO: set attr for cloth body after loading in physics scene. - - # rest cloth body to init_pos - pos = torch.as_tensor( - self.cfg.init_pos, dtype=torch.float32, device=self.device - ) - rot = ( - torch.as_tensor(self.cfg.init_rot, dtype=torch.float32, device=self.device) - * torch.pi - / 180.0 - ) - pos = pos.unsqueeze(0).repeat(num_instances, 1) - rot = rot.unsqueeze(0).repeat(num_instances, 1) - mat = matrix_from_euler(rot, "XYZ") - pose = ( - torch.eye(4, dtype=torch.float32, device=self.device) - .unsqueeze(0) - .repeat(num_instances, 1, 1) - ) - pose[:, :3, 3] = pos - pose[:, :3, :3] = mat - self.set_local_pose(pose, env_ids=local_env_ids) - - def destroy(self) -> None: - # TODO: not tested yet - env = self._world.get_env() - arenas = env.get_all_arenas() - if len(arenas) == 0: - arenas = [env] - for i, entity in enumerate(self._entities): - arenas[i].remove_actor(entity) diff --git a/embodichain/lab/sim/objects/deformable/__init__.py b/embodichain/lab/sim/objects/deformable/__init__.py new file mode 100644 index 000000000..4df6cc8ae --- /dev/null +++ b/embodichain/lab/sim/objects/deformable/__init__.py @@ -0,0 +1,35 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Unified deformable-object API over Newton particle-set specializations.""" + +from __future__ import annotations + +from .base import DeformableObject +from .data import DeformableObjectData +from .surface import ( + SurfaceDeformableObject, +) +from .volume import ( + VolumeDeformableObject, +) + +__all__ = [ + "DeformableObject", + "DeformableObjectData", + "SurfaceDeformableObject", + "VolumeDeformableObject", +] diff --git a/embodichain/lab/sim/objects/deformable/base.py b/embodichain/lab/sim/objects/deformable/base.py new file mode 100644 index 000000000..231baf321 --- /dev/null +++ b/embodichain/lab/sim/objects/deformable/base.py @@ -0,0 +1,517 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Common Newton facade for volume and surface deformable objects.""" + +from __future__ import annotations + +from abc import abstractmethod +from copy import deepcopy +from typing import TYPE_CHECKING, Any, Literal, Sequence + +import numpy as np +import torch +from dexsim.scene import Scene + +from embodichain.lab.sim.cfg import DeformableObjectCfg +from embodichain.lab.sim.common import BatchEntity +from embodichain.lab.sim.material import ( + VisualMaterial, + VisualMaterialInst, + _capture_render_materials, + _restore_render_materials, + _wrap_first_render_material, +) +from embodichain.utils import logger +from embodichain.utils.math import matrix_from_euler, xyz_quat_to_4x4_matrix + +from .data import DeformableObjectData + +if TYPE_CHECKING: + from dexsim.scene import SpawnedParticleSet + +__all__ = ["DeformableObject"] + + +class DeformableObject(BatchEntity): + """Common facade over a batch of Newton particle-set deformables. + + Volume and surface objects retain EmbodiChain's public nodal contract, but + their runtime ownership is exclusively DexSim Spawn's Newton scene. The + Default backend and direct native soft/cloth body buffers are unsupported. + """ + + @property + @abstractmethod + def deformable_type(self) -> Literal["volume", "surface"]: + """Physical topology supplied by the concrete object class.""" + + def __init__( + self, + cfg: DeformableObjectCfg, + device: torch.device = torch.device("cpu"), + ) -> None: + """Create an unregistered deformable facade. + + ``SpawnScene`` supplies the replicated instance count when declaring + this facade and the finalized ``Scene`` when binding it. + """ + if cfg.deformable_type != self.deformable_type: + raise ValueError( + f"{type(self).__name__} requires deformable_type=" + f"{self.deformable_type!r}, got {cfg.deformable_type!r}." + ) + + self._initialize_unregistered(cfg, device) + + def _initialize_unregistered( + self, + cfg: DeformableObjectCfg, + device: torch.device, + ) -> None: + """Initialize state that is independent of Spawn replication.""" + self.cfg = deepcopy(cfg) + self.uid = self.cfg.uid + self.device = device + self._entities: list[Any] = [] + self._declared_num_instances: int | None = None + self._spawn_result: Scene | None = None + self._world = None + self._data = None + self._all_indices: list[int] = [] + self._visual_material: list[VisualMaterialInst | None] = [] + self.is_shared_visual_material = False + + def _initialize_spawn_declaration(self, num_instances: int) -> None: + """Initialize instance-dependent declaration state from ``SpawnScene``.""" + if num_instances <= 0: + raise ValueError( + f"A declared {type(self).__name__} requires num_instances > 0." + ) + if self._declared_num_instances is not None: + if self._declared_num_instances != num_instances: + raise RuntimeError( + f"{type(self).__name__} {self.uid!r} is already declared for " + f"{self._declared_num_instances} instances." + ) + return + + self._declared_num_instances = num_instances + self._all_indices = list(range(num_instances)) + self._visual_material = [None] * num_instances + + def _require_declared_num_instances(self) -> int: + """Return the Spawn-provided instance count or raise a lifecycle error.""" + if self._declared_num_instances is None: + raise RuntimeError( + f"{type(self).__name__} {self.uid!r} must be registered through " + "SpawnScene before it can be used." + ) + return self._declared_num_instances + + def _initialize_topology(self, entities: Sequence[SpawnedParticleSet]) -> None: + """Capture per-instance render topology with a stable batch shape.""" + vertex_counts = tuple( + np.asarray(entity.get_render_vertices(), dtype=np.float32) + .reshape(-1, 3) + .shape[0] + for entity in entities + ) + if len(set(vertex_counts)) != 1: + raise RuntimeError( + "Replicated Newton deformable render meshes must share one " + "vertex count, but DexSim materialized counts " + f"{vertex_counts}. This indicates a render-clone topology " + "mismatch; use a compatible source mesh or one environment " + "until the DexSim clone path is corrected." + ) + triangles = [ + np.asarray(entity.get_render_triangles(), dtype=np.int32).reshape(-1, 3) + for entity in entities + ] + triangle_counts = {len(item) for item in triangles} + if len(triangle_counts) != 1: + raise ValueError( + "All instances of one deformable asset must share render " + f"triangle count, got {sorted(triangle_counts)}." + ) + for instance, (instance_triangles, vertex_count) in enumerate( + zip(triangles, vertex_counts, strict=True) + ): + if instance_triangles.size and ( + int(instance_triangles.min()) < 0 + or int(instance_triangles.max()) >= vertex_count + ): + raise ValueError( + "Deformable render topology contains an out-of-range " + f"vertex index for instance {instance}." + ) + self._surface_triangles = torch.as_tensor( + np.stack(triangles), + dtype=torch.int32, + device=self.device, + ).clone() + + @staticmethod + def _resolve_arena_offsets( + scene: Scene, + entities: Sequence[SpawnedParticleSet], + ) -> torch.Tensor: + if not scene.arenas: + offsets = np.zeros((len(entities), 3), dtype=np.float32) + else: + arena_indices = [ + scene.arenas.index(entity.arena_name) for entity in entities + ] + offsets = scene.arenas.root_offsets[arena_indices] + return torch.as_tensor(offsets, dtype=torch.float32) + + def _configured_initial_pose(self) -> torch.Tensor: + if self.cfg.init_local_pose is not None: + pose = torch.as_tensor( + self.cfg.init_local_pose, + dtype=torch.float32, + device=self.device, + ).reshape(4, 4) + return pose.clone() + + pose = torch.eye(4, dtype=torch.float32, device=self.device) + pose[:3, 3] = torch.as_tensor( + self.cfg.init_pos, + dtype=torch.float32, + device=self.device, + ) + rotation = ( + torch.as_tensor( + self.cfg.init_rot, + dtype=torch.float32, + device=self.device, + ) + * torch.pi + / 180.0 + ) + pose[:3, :3] = matrix_from_euler(rotation.unsqueeze(0), "XYZ")[0] + return pose + + def _capture_local_rest_positions(self) -> torch.Tensor: + self._require_data() + initial_pose = self._configured_initial_pose() + initial_positions = self.data.default_nodal_state_w[..., :3] + arena_offsets = self._arena_offsets.to(self.device).unsqueeze(1) + translated = initial_positions - initial_pose[:3, 3] - arena_offsets + return translated @ initial_pose[:3, :3] + + @property + def is_spawn_bound(self) -> bool: + """Whether this facade is bound to one finalized Spawn scene.""" + return self._spawn_result is not None + + @property + def is_declared(self) -> bool: + """Whether this facade is waiting for its Spawn scene binding.""" + return self._world is None + + @property + def num_instances(self) -> int: + """Return the materialized or declared instance count.""" + return ( + len(self._entities) + if self._entities + else self._require_declared_num_instances() + ) + + @property + def data(self) -> DeformableObjectData | None: + """Return the common deformable data view after Spawn binding.""" + return self._data + + def attach_spawn_handles(self, entities: Sequence[Any]) -> None: + """Store materialized handles before final Spawn binding.""" + handles = list(entities) + expected = self._require_declared_num_instances() + if len(handles) != expected: + raise ValueError( + f"{type(self).__name__} {self.uid!r} expected {expected} Spawn " + f"handles, got {len(handles)}." + ) + self._entities = handles + + def _initialize_spawn_bound( + self, + result: Scene, + entities: Sequence[Any], + ) -> None: + """Create result-dependent runtime state on this declared facade.""" + if not isinstance(result, Scene): + raise TypeError( + "DeformableObject binding requires a finalized DexSim Scene; use " + "SimulationManager.prepare()." + ) + + if result.backend != "newton": + raise NotImplementedError( + "EmbodiChain deformable objects require the Newton backend; " + "the Default backend is no longer supported." + ) + + cfg = deepcopy(self.cfg) + self._spawn_result = result + self._world = result.world + self._all_indices = list(range(len(entities))) + self._arena_offsets = self._resolve_arena_offsets(result, entities) + self._data = DeformableObjectData(entities, result, self.device) + self._local_rest_positions = self._capture_local_rest_positions() + self._initialize_topology(entities) + self._visual_material = [None] * len(entities) + self.is_shared_visual_material = False + + super().__init__(cfg=cfg, entities=list(entities), device=self.device) + self._initialize_existing_visual_material() + self.reset() + + def bind_spawn(self, result: Scene) -> None: + """Bind a declared facade to finalized native handles in place.""" + if self.is_spawn_bound: + raise RuntimeError( + f"{type(self).__name__} {self.uid!r} is already Spawn-bound." + ) + if not self.is_declared: + raise RuntimeError( + f"{type(self).__name__} {self.uid!r} was not created as a Spawn declaration." + ) + + entities = list(self._entities) + expected = self._require_declared_num_instances() + if len(entities) != expected: + raise ValueError( + f"{type(self).__name__} {self.uid!r} expected {expected} Spawn " + f"handles, got {len(entities)}." + ) + + declared_state = self.__dict__.copy() + try: + if self.cfg.shape.compute_uv: + for entity in entities: + render_body = entity.get_render_body() + project_uv = getattr(render_body, "set_projective_uv", None) + if project_uv is None: + raise NotImplementedError( + "compute_uv requires a deformable render body with " + "set_projective_uv()." + ) + project_uv(np.asarray(self.cfg.shape.project_direction)) + self._initialize_spawn_bound(result, entities) + except Exception: + self.__dict__.clear() + self.__dict__.update(declared_state) + raise + + def __str__(self) -> str: + if self.is_declared: + return ( + f"{self.__class__}: declared {self.num_instances} Spawn " + f"{self.deformable_type} deformable objects | uid: {self.uid} | " + f"device: {self.device}" + ) + return super().__str__() + + def _initialize_existing_visual_material(self) -> None: + """Capture and wrap materials parsed from the source asset.""" + self._original_visual_material = [[] for _ in self._entities] + self._original_visual_material_inst = [None] * len(self._entities) + for env_idx, entity in enumerate(self._entities): + render_body = entity.get_render_body() + if render_body is None: + continue + original_materials = _capture_render_materials(render_body) + self._original_visual_material[env_idx] = original_materials + wrapped = _wrap_first_render_material(original_materials) + if wrapped is not None: + self._visual_material[env_idx] = wrapped + self._original_visual_material_inst[env_idx] = wrapped + + def set_visual_material( + self, + mat: VisualMaterial, + env_ids: Sequence[int] | None = None, + shared: bool = False, + ) -> None: + """Assign visual material instances to selected environments.""" + local_env_ids = self._resolve_env_ids(env_ids) + if shared: + if len(local_env_ids) != self.num_instances: + logger.log_error("Cannot share material instance for partial env_ids.") + mat_inst = mat.create_instance(f"{mat.uid}_{self.uid}") + for env_idx in local_env_ids: + self._entities[env_idx].set_material(mat_inst.mat) + self._visual_material[env_idx] = mat_inst + self.is_shared_visual_material = True + return + + for env_idx in local_env_ids: + mat_inst = mat.create_instance(f"{mat.uid}_{self.uid}_{env_idx}") + self._entities[env_idx].set_material(mat_inst.mat) + self._visual_material[env_idx] = mat_inst + self.is_shared_visual_material = False + + def restore_visual_material(self, env_ids: Sequence[int] | None = None) -> None: + """Restore materials captured when the deformable was created.""" + if not hasattr(self, "_original_visual_material"): + return + for env_idx in self._resolve_env_ids(env_ids): + render_body = self._entities[env_idx].get_render_body() + if render_body is None: + continue + _restore_render_materials( + render_body, self._original_visual_material[env_idx] + ) + self._visual_material[env_idx] = self._original_visual_material_inst[ + env_idx + ] + self.is_shared_visual_material = False + + def get_visual_material_inst( + self, env_ids: Sequence[int] | None = None + ) -> list[VisualMaterialInst | None]: + """Return registered material wrappers for selected environments.""" + return [self._visual_material[i] for i in self._resolve_env_ids(env_ids)] + + def _resolve_env_ids(self, env_ids: Sequence[int] | None) -> list[int]: + if env_ids is None: + return list(self._all_indices) + if isinstance(env_ids, torch.Tensor): + ids = env_ids.detach().cpu().reshape(-1).tolist() + else: + ids = list(env_ids) + resolved = [int(env_id) for env_id in ids] + if any(env_id < 0 or env_id >= self.num_instances for env_id in resolved): + raise IndexError( + f"Environment IDs {resolved!r} are outside [0, {self.num_instances})." + ) + return resolved + + def set_local_pose( + self, pose: torch.Tensor, env_ids: Sequence[int] | None = None + ) -> None: + """Set a deformable pose by transforming its captured rest particles.""" + local_env_ids = self._resolve_env_ids(env_ids) + if len(local_env_ids) != len(pose): + raise ValueError( + f"Length of env_ids {len(local_env_ids)} does not match pose " + f"length {len(pose)}." + ) + if pose.dim() == 2 and pose.shape[1] == 7: + pose4x4 = xyz_quat_to_4x4_matrix(pose) + elif pose.dim() == 3 and pose.shape[1:] == (4, 4): + pose4x4 = pose + else: + raise ValueError( + f"Invalid pose shape {tuple(pose.shape)}. Expected (N, 7) or " + "(N, 4, 4)." + ) + self._apply_local_pose( + pose4x4.to(device=self.device, dtype=torch.float32), + local_env_ids, + ) + + def _apply_local_pose( + self, + pose: torch.Tensor, + env_ids: Sequence[int], + ) -> None: + """Apply rest-particle transforms through the Spawn particle batch.""" + self._require_data() + if not env_ids: + return + index = torch.as_tensor(env_ids, dtype=torch.long, device=self.device) + local_positions = self._local_rest_positions.index_select(0, index) + rotations = pose[:, :3, :3] + translations = pose[:, :3, 3].unsqueeze(1) + arena_offsets = self._arena_offsets.to(self.device).index_select(0, index) + positions = ( + torch.bmm(local_positions, rotations.transpose(1, 2)) + + translations + + arena_offsets.unsqueeze(1) + ) + self._data._apply_nodal_state( + positions, + torch.zeros_like(positions), + env_ids, + ) + + def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: + """Reject root-pose reads because deformables have no rigid root pose.""" + del to_matrix + raise NotImplementedError( + f"Getting local pose for {type(self).__name__} is not supported." + ) + + def _require_data(self) -> None: + if self.data is None: + raise RuntimeError( + f"{type(self).__name__} data is unavailable before Spawn finalization." + ) + + def get_surface_vertices(self) -> torch.Tensor: + """Return live render-surface vertices in world frame.""" + vertices_per_instance: list[torch.Tensor] = [] + render_pose = self._configured_initial_pose() + render_rotation = render_pose[:3, :3] + render_translation = render_pose[:3, 3] + arena_offsets = self._arena_offsets.to(self.device) + for env_idx, entity in enumerate(self._entities): + vertices_warp = entity.get_render_vertices_warp() + if vertices_warp is None: + vertices = torch.as_tensor( + entity.get_render_vertices(), + dtype=torch.float32, + device=self.device, + ).reshape(-1, 3) + else: + import warp as wp + + vertices = wp.to_torch(vertices_warp).reshape(-1, 3).to(self.device) + vertices_per_instance.append( + vertices @ render_rotation.T + + render_translation + + arena_offsets[env_idx] + ) + + vertex_counts = {len(vertices) for vertices in vertices_per_instance} + if len(vertex_counts) != 1: + raise ValueError( + "All instances of one deformable asset must share render vertex count." + ) + return torch.stack(vertices_per_instance).clone() + + def get_surface_triangles( + self, env_ids: Sequence[int] | None = None + ) -> torch.Tensor: + """Return render-surface triangle indices for selected environments.""" + ids = self._resolve_env_ids(env_ids) + index = torch.as_tensor(ids, dtype=torch.long, device=self.device) + return self._surface_triangles.index_select(0, index).clone() + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Restore the configured pose, zero velocity, and source materials.""" + local_env_ids = self._resolve_env_ids(env_ids) + self.restore_visual_material(env_ids=local_env_ids) + initial_pose = self._configured_initial_pose() + pose = initial_pose.unsqueeze(0).repeat(len(local_env_ids), 1, 1) + self.set_local_pose(pose, env_ids=local_env_ids) + + def destroy(self) -> None: + """Leave particle lifetime ownership with the finalized Spawn scene.""" diff --git a/embodichain/lab/sim/objects/deformable/data.py b/embodichain/lab/sim/objects/deformable/data.py new file mode 100644 index 000000000..4c20e314d --- /dev/null +++ b/embodichain/lab/sim/objects/deformable/data.py @@ -0,0 +1,176 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Shared nodal data for Newton volume and surface particle sets.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Sequence + +import torch + +if TYPE_CHECKING: + from dexsim.scene import Scene, SpawnedParticleSet + +__all__ = ["DeformableObjectData"] + + +class DeformableObjectData: + """Common nodal-state view for volume and surface deformables. + + Positions and velocities use the simulation world frame. Consumers can + rely on a stable ``(num_instances, n_nodes, 3)`` contract. State properties + return independent snapshots; the default state is captured at Spawn binding. + + Args: + entities: Replicated particle sets with equal node counts. + scene: Finalized DexSim scene that owns the particle sets. + device: Device used for state tensors and batch transfers. + """ + + def __init__( + self, + entities: Sequence[SpawnedParticleSet], + scene: Scene, + device: torch.device, + ) -> None: + self.entities = list(entities) + if not self.entities: + raise ValueError("A deformable particle-set batch cannot be empty.") + + self.scene = scene + self.device = device + self.num_instances = len(self.entities) + particle_counts = tuple(int(entity.particle_count) for entity in self.entities) + if any(count <= 0 for count in particle_counts): + raise ValueError("Deformable particle sets must contain particles.") + if len(set(particle_counts)) != 1: + raise ValueError( + "All instances of one deformable asset must have the same " + f"particle count, got {particle_counts}." + ) + + self.n_nodes = particle_counts[0] + self.batch = scene.create_particle_set_batch(self.entities) + self._position_buffer = torch.empty( + (self.num_instances, self.n_nodes, 3), + dtype=torch.float32, + device=self.device, + ) + self._velocity_buffer = torch.empty_like(self._position_buffer) + default_positions = self.nodal_pos_w + default_velocities = self.nodal_vel_w + self._default_nodal_state_w = torch.cat( + (default_positions, default_velocities), + dim=-1, + ) + + @staticmethod + def _check_batch_status(status: int | None, operation: str) -> None: + if status is not None and int(status) < 0: + raise RuntimeError( + f"DexSim particle batch failed to {operation}: status {status}." + ) + + @property + def nodal_pos_w(self) -> torch.Tensor: + """Return current Newton particle positions in world frame.""" + status = self.batch.fetch_particle_positions( + self._position_buffer.reshape(-1, 3) + ) + self._check_batch_status(status, "fetch positions") + return self._position_buffer.clone() + + @property + def nodal_vel_w(self) -> torch.Tensor: + """Return current Newton particle velocities in world frame.""" + status = self.batch.fetch_particle_velocities( + self._velocity_buffer.reshape(-1, 3) + ) + self._check_batch_status(status, "fetch velocities") + return self._velocity_buffer.clone() + + @property + def default_nodal_state_w(self) -> torch.Tensor: + """Return the particle state captured when Spawn was bound.""" + return self._default_nodal_state_w.clone() + + @property + def nodal_state_w(self) -> torch.Tensor: + """Return current nodal state ``[position, velocity]`` in world frame.""" + return torch.cat((self.nodal_pos_w, self.nodal_vel_w), dim=-1) + + @property + def root_pos_w(self) -> torch.Tensor: + """Return the mean nodal position for each deformable instance.""" + return self.nodal_pos_w.mean(dim=1) + + @property + def root_vel_w(self) -> torch.Tensor: + """Return the mean nodal velocity for each deformable instance.""" + return self.nodal_vel_w.mean(dim=1) + + def _apply_nodal_state( + self, + positions: torch.Tensor, + velocities: torch.Tensor, + env_ids: Sequence[int], + ) -> None: + """Apply packed state to selected particle-set instances.""" + env_ids = [int(env_id) for env_id in env_ids] + if not env_ids: + return + if len(set(env_ids)) != len(env_ids): + raise ValueError(f"env_ids must not contain duplicates, got {env_ids}.") + + expected_shape = (len(env_ids), self.n_nodes, 3) + if tuple(positions.shape) != expected_shape: + raise ValueError( + f"positions must have shape {expected_shape}, got " + f"{tuple(positions.shape)}." + ) + if tuple(velocities.shape) != expected_shape: + raise ValueError( + f"velocities must have shape {expected_shape}, got " + f"{tuple(velocities.shape)}." + ) + + if env_ids == list(range(self.num_instances)): + batch = self.batch + else: + batch = self.scene.create_particle_set_batch( + [self.entities[env_id] for env_id in env_ids] + ) + packed_positions = ( + positions.to( + device=self.device, + dtype=torch.float32, + ) + .contiguous() + .reshape(-1, 3) + ) + packed_velocities = ( + velocities.to( + device=self.device, + dtype=torch.float32, + ) + .contiguous() + .reshape(-1, 3) + ) + position_status = batch.apply_particle_positions(packed_positions) + self._check_batch_status(position_status, "apply positions") + velocity_status = batch.apply_particle_velocities(packed_velocities) + self._check_batch_status(velocity_status, "apply velocities") diff --git a/embodichain/lab/sim/objects/deformable/surface.py b/embodichain/lab/sim/objects/deformable/surface.py new file mode 100644 index 000000000..6d7aa25c6 --- /dev/null +++ b/embodichain/lab/sim/objects/deformable/surface.py @@ -0,0 +1,31 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Newton surface-deformable object implementation.""" + +from __future__ import annotations + +from .base import DeformableObject + +__all__ = [ + "SurfaceDeformableObject", +] + + +class SurfaceDeformableObject(DeformableObject): + """A batch of Newton cloth particle sets.""" + + deformable_type = "surface" diff --git a/embodichain/lab/sim/objects/deformable/volume.py b/embodichain/lab/sim/objects/deformable/volume.py new file mode 100644 index 000000000..f3f532b9b --- /dev/null +++ b/embodichain/lab/sim/objects/deformable/volume.py @@ -0,0 +1,68 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Newton volume-deformable object implementation.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Sequence + +import numpy as np +import torch + +from .base import DeformableObject + +if TYPE_CHECKING: + from dexsim.scene import SpawnedSoftBodyParticleSet + +__all__ = [ + "VolumeDeformableObject", +] + + +class VolumeDeformableObject(DeformableObject): + """A batch of Newton volumetric soft-body particle sets.""" + + deformable_type = "volume" + + def _initialize_topology( + self, + entities: Sequence[SpawnedSoftBodyParticleSet], + ) -> None: + super()._initialize_topology(entities) + triangles = [ + np.asarray(entity.get_surface_triangles(), dtype=np.int32).reshape(-1, 3) + for entity in entities + ] + triangle_counts = {len(item) for item in triangles} + if len(triangle_counts) != 1: + raise ValueError( + "All instances of one soft body must share surface triangle " + f"count, got {sorted(triangle_counts)}." + ) + self._collision_surface_triangles = torch.as_tensor( + np.stack(triangles), + dtype=torch.int32, + device=self.device, + ).clone() + + def get_collision_surface_triangles( + self, env_ids: Sequence[int] | None = None + ) -> torch.Tensor: + """Return the tetrahedral surface topology for selected instances.""" + ids = self._resolve_env_ids(env_ids) + index = torch.as_tensor(ids, dtype=torch.long, device=self.device) + return self._collision_surface_triangles.index_select(0, index).clone() diff --git a/embodichain/lab/sim/objects/light.py b/embodichain/lab/sim/objects/light.py index 065267333..f497a96ae 100644 --- a/embodichain/lab/sim/objects/light.py +++ b/embodichain/lab/sim/objects/light.py @@ -46,6 +46,7 @@ def __init__( ) -> None: super().__init__(cfg, entities, device) + self.reset() def set_color( self, colors: torch.Tensor, env_ids: Sequence[int] | None = None diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index 39185a8a0..783ec0909 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -17,17 +17,22 @@ from __future__ import annotations import torch -import dexsim import numpy as np -from dataclasses import dataclass, MISSING -from typing import List, Sequence, Union +from copy import deepcopy +from dataclasses import dataclass +from typing import TYPE_CHECKING, List, Sequence from functools import cached_property -from dexsim.models import MeshObject -from dexsim.types import RigidBodyGPUAPIReadType, RigidBodyGPUAPIWriteType -from dexsim.engine import CudaArray, MaterialInst, PhysicsScene -from embodichain.lab.sim.cfg import RigidObjectCfg, RigidBodyAttributesCfg +from dexsim.scene import Scene +from dexsim.engine import MaterialInst +from embodichain.lab.sim.cfg import RigidBodyPhysicsCfg, RigidObjectCfg +from embodichain.lab.sim.objects.backends import ( + SceneRigidBodyView, + is_newton_scene, +) +from embodichain.lab.sim.objects.backends.base import RigidBodyViewBase +from embodichain.lab.sim.physics.newton import is_newton_gradient_mode from embodichain.lab.sim.shapes import MeshCfg from embodichain.lab.sim import ( VisualMaterial, @@ -45,47 +50,51 @@ get_combined_triangles, get_combined_vertices, ) -from embodichain.utils.math import convert_quat from embodichain.utils.math import matrix_from_quat, quat_from_matrix, matrix_from_euler from embodichain.utils import logger +if TYPE_CHECKING: + from dexsim.scene import SpawnedRigidBody + +_UINT64_MAX = (1 << 64) - 1 __all__ = ["RigidBodyData", "RigidObject", "RigidObjectCfg"] @dataclass class RigidBodyData: - """Data manager for rigid body with body type of dynamic or kinematic. + """Scene-batch data manager for dynamic or kinematic rigid bodies. - Note: - 1. The pose data managed by dexsim is in the format of (qx, qy, qz, qw, x, y, z), but in SimulationManager, we use (x, y, z, qw, qx, qy, qz) format. + All pose/velocity/acceleration data uses EmbodiChain convention: + ``(x, y, z, qx, qy, qz, qw)``. """ def __init__( - self, entities: List[MeshObject], ps: PhysicsScene, device: torch.device + self, + entities: Sequence[SpawnedRigidBody], + scene: Scene, + device: torch.device, ) -> None: """Initialize the RigidBodyData. Args: - entities (List[MeshObject]): List of MeshObjects representing the rigid bodies. - ps (PhysicsScene): The physics scene. - device (torch.device): The device to use for the rigid body data. + entities: Rigid-body handles owned by ``scene``. + scene: Finalized DexSim Scene. + device: Device to use for the rigid-body data. """ + if not isinstance(scene, Scene): + raise TypeError("RigidBodyData requires a finalized DexSim Scene.") self.entities = entities - self.ps = ps + self.scene = scene self.num_instances = len(entities) self.device = device - - # get gpu indices for the entities. - self.gpu_indices = ( - torch.as_tensor( - [entity.get_gpu_index() for entity in self.entities], - dtype=torch.int32, - device=self.device, - ) - if self.device.type == "cuda" - else None + self.body_view: RigidBodyViewBase = SceneRigidBodyView.from_entities( + scene, entities, device ) + # Kept for backward compatibility with callers that index gpu_indices directly. + # Scene-backed views expose logical batch rows until backend IDs become available. + # Use the ``gpu_indices`` property instead of caching here. + # Initialize rigid body data. self._pose = torch.zeros( (self.num_instances, 7), dtype=torch.float32, device=self.device @@ -102,77 +111,121 @@ def __init__( self._ang_acc = torch.zeros( (self.num_instances, 3), dtype=torch.float32, device=self.device ) - # center of mass pose in format (x, y, z, qw, qx, qy, qz) - self.default_com_pose = torch.zeros( - (self.num_instances, 7), dtype=torch.float32, device=self.device - ) + # Initialization-time physical-property snapshots. These are captured + # after backend materialization and remain unchanged by runtime writes. + self._default_mass: torch.Tensor | None = None + self._default_inertia: torch.Tensor | None = None + self._default_com_pose: torch.Tensor | None = None + + # center of mass pose in format (x, y, z, qx, qy, qz, qw) self._com_pose = torch.zeros( (self.num_instances, 7), dtype=torch.float32, device=self.device ) + # Physical property buffers + self._mass = torch.zeros( + (self.num_instances, 1), dtype=torch.float32, device=self.device + ) + self._inertia = torch.zeros( + (self.num_instances, 3), dtype=torch.float32, device=self.device + ) + self._friction = torch.zeros( + (self.num_instances, 1), dtype=torch.float32, device=self.device + ) @property - def pose(self) -> torch.Tensor: - if self.device.type == "cpu": - # Fetch pose from CPU entities - xyzs = torch.as_tensor( - np.array([entity.get_location() for entity in self.entities]), - dtype=torch.float32, - device=self.device, - ) - quats = torch.as_tensor( - np.array( - [entity.get_rotation_quat() for entity in self.entities], - ), - dtype=torch.float32, - device=self.device, - ) - quats = convert_quat(quats, to="wxyz") - self._pose = torch.cat((xyzs, quats), dim=-1) - else: - self.ps.gpu_fetch_rigid_body_data( - data=self._pose, - gpu_indices=self.gpu_indices, - data_type=RigidBodyGPUAPIReadType.POSE, + def default_physical_properties_initialized(self) -> bool: + """Whether the backend-resolved physical-property defaults are available.""" + return ( + self._default_mass is not None + and self._default_inertia is not None + and self._default_com_pose is not None + ) + + @property + def default_mass(self) -> torch.Tensor: + """Initialization-time mass with shape ``(N,)``.""" + if self._default_mass is None: + raise RuntimeError("Default rigid-body mass has not been captured yet.") + return self._default_mass + + @property + def default_inertia(self) -> torch.Tensor: + """Initialization-time inertia diagonal with shape ``(N, 3)``.""" + if self._default_inertia is None: + raise RuntimeError("Default rigid-body inertia has not been captured yet.") + return self._default_inertia + + @property + def default_com_pose(self) -> torch.Tensor: + """Initialization-time local COM pose as an ``xyz + xyzw`` tensor.""" + if self._default_com_pose is None: + raise RuntimeError("Default rigid-body COM pose has not been captured yet.") + return self._default_com_pose + + def capture_default_physical_properties( + self, + *, + mass: torch.Tensor, + inertia: torch.Tensor, + com_pose: torch.Tensor, + ) -> None: + """Capture backend-resolved physical properties exactly once.""" + expected_shapes = { + "mass": (self.num_instances,), + "inertia": (self.num_instances, 3), + "com_pose": (self.num_instances, 7), + } + values = {"mass": mass, "inertia": inertia, "com_pose": com_pose} + for name, value in values.items(): + if tuple(value.shape) != expected_shapes[name]: + raise ValueError( + f"Expected {name} shape {expected_shapes[name]}, got {tuple(value.shape)}." + ) + + if self.default_physical_properties_initialized: + raise RuntimeError( + "Default rigid-body physical properties are already captured." ) - self._pose[:, :4] = convert_quat(self._pose[:, :4], to="wxyz") - self._pose = self._pose[:, [4, 5, 6, 0, 1, 2, 3]] - return self._pose + + self._default_mass = mass.to(self.device, dtype=torch.float32).clone() + self._default_inertia = inertia.to(self.device, dtype=torch.float32).clone() + self._default_com_pose = com_pose.to(self.device, dtype=torch.float32).clone() + + @property + def is_newton_backend(self) -> bool: + return self.body_view.is_newton_backend + + @property + def gpu_indices(self) -> torch.Tensor: + """Body ID tensor (backward-compatible alias for ``body_view.body_ids_tensor``).""" + return self.body_view.body_ids_tensor + + def body_ids_for(self, env_ids: Sequence[int]) -> torch.Tensor: + return self.body_view.select_body_ids(env_ids) + + @property + def pose(self) -> torch.Tensor: + if self.body_view.can_fetch_pose: + self.body_view.fetch_pose(self._pose) + return self._pose + + logger.log_error(f"RigidBodyData pose requested but body view is not ready.") @property def lin_vel(self) -> torch.Tensor: - if self.device.type == "cpu": - # Fetch linear velocity from CPU entities - self._lin_vel = torch.as_tensor( - np.array([entity.get_linear_velocity() for entity in self.entities]), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_rigid_body_data( - data=self._lin_vel, - gpu_indices=self.gpu_indices, - data_type=RigidBodyGPUAPIReadType.LINEAR_VELOCITY, - ) - return self._lin_vel + if self.body_view.is_ready: + self.body_view.fetch_linear_velocity(self._lin_vel) + return self._lin_vel + + logger.log_error("RigidBodyData lin_vel requested but body view is not ready.") @property def ang_vel(self) -> torch.Tensor: - if self.device.type == "cpu": - # Fetch angular velocity from CPU entities - self._ang_vel = torch.as_tensor( - np.array( - [entity.get_angular_velocity() for entity in self.entities], - ), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_rigid_body_data( - data=self._ang_vel, - gpu_indices=self.gpu_indices, - data_type=RigidBodyGPUAPIReadType.ANGULAR_VELOCITY, - ) - return self._ang_vel + if self.body_view.is_ready: + self.body_view.fetch_angular_velocity(self._ang_vel) + return self._ang_vel + + logger.log_error("RigidBodyData ang_vel requested but body view is not ready.") @property def vel(self) -> torch.Tensor: @@ -185,39 +238,19 @@ def vel(self) -> torch.Tensor: @property def lin_acc(self) -> torch.Tensor: - if self.device.type == "cpu": - self._lin_acc = torch.as_tensor( - np.array( - [entity.get_linear_acceleration() for entity in self.entities], - ), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_rigid_body_data( - data=self._lin_acc, - gpu_indices=self.gpu_indices, - data_type=RigidBodyGPUAPIReadType.LINEAR_ACCELERATION, - ) - return self._lin_acc + if self.body_view.is_ready: + self.body_view.fetch_linear_acceleration(self._lin_acc) + return self._lin_acc + + logger.log_error("RigidBodyData lin_acc requested but body view is not ready.") @property def ang_acc(self) -> torch.Tensor: - if self.device.type == "cpu": - self._ang_acc = torch.as_tensor( - np.array( - [entity.get_angular_acceleration() for entity in self.entities], - ), - dtype=torch.float32, - device=self.device, - ) - else: - self.ps.gpu_fetch_rigid_body_data( - data=self._ang_acc, - gpu_indices=self.gpu_indices, - data_type=RigidBodyGPUAPIReadType.ANGULAR_ACCELERATION, - ) - return self._ang_acc + if self.body_view.is_ready: + self.body_view.fetch_angular_acceleration(self._ang_acc) + return self._ang_acc + + logger.log_error("RigidBodyData ang_acc requested but body view is not ready.") @property def acc(self) -> torch.Tensor: @@ -228,21 +261,33 @@ def acc(self) -> torch.Tensor: """ return torch.cat((self.lin_acc, self.ang_acc), dim=-1) + @property + def mass(self) -> torch.Tensor: + """Get current masses with shape ``(N,)``.""" + if not self.body_view.is_ready: + logger.log_error("RigidBodyData mass requested but body view is not ready.") + self.body_view.fetch_mass(self._mass) + return self._mass.squeeze(-1) + + @property + def inertia(self) -> torch.Tensor: + """Get current inertia diagonals with shape ``(N, 3)``.""" + if not self.body_view.is_ready: + logger.log_error( + "RigidBodyData inertia requested but body view is not ready." + ) + self.body_view.fetch_inertia_diagonal(self._inertia) + return self._inertia + @property def com_pose(self) -> torch.Tensor: """Get the center of mass pose of the rigid bodies. Returns: - torch.Tensor: The center of mass pose with shape (N, 7). + torch.Tensor: The center-of-mass pose with shape ``(N, 7)`` in + ``(x, y, z, qx, qy, qz, qw)`` order. """ - for i, entity in enumerate(self.entities): - pos, quat = entity.get_physical_body().get_cmass_local_pose() - self._com_pose[i, :3] = torch.as_tensor( - pos, dtype=torch.float32, device=self.device - ) - self._com_pose[i, 3:7] = torch.as_tensor( - quat, dtype=torch.float32, device=self.device - ) + self.body_view.fetch_com_local_pose(self._com_pose) return self._com_pose @@ -254,80 +299,180 @@ class RigidObject(BatchEntity): - Dynamic: Actors that can move and are affected by physics. - Kinematic: Actors that can move but are not affected by physics. + Args: + cfg: Configuration for the rigid object. + device: Device to use (CPU or CUDA). + """ def __init__( self, cfg: RigidObjectCfg, - entities: List[MeshObject] = None, device: torch.device = torch.device("cpu"), ) -> None: - self.body_type = cfg.body_type + """Create an unregistered rigid-object facade. - self._world = dexsim.default_world() - self._ps = self._world.get_physics_scene() - - self._all_indices = torch.arange(len(entities), dtype=torch.int32).tolist() - - # data for managing body data (only for dynamic and kinematic bodies) on GPU. + ``SpawnScene`` supplies the replicated instance count when it registers + the facade and supplies the finalized ``Scene`` only when binding it. + This keeps construction independent of a process-global manager. + """ + self.cfg = deepcopy(cfg) + self.uid = self.cfg.uid + self.device = device + self.body_type = self.cfg.body_type + self._entities: list[SpawnedRigidBody] = [] + self._declared_num_instances: int | None = None + self._spawn_result: Scene | None = None + self._ps = None + self._world = None self._data: RigidBodyData | None = None - if self.is_static is False: - self._data = RigidBodyData(entities=entities, ps=self._ps, device=device) - - # For rendering purposes, each instance can have its own material. - self._visual_material: List[VisualMaterialInst] = [None] * len(entities) + self._all_indices: list[int] = [] + self._visual_material: List[VisualMaterialInst] = [] self.is_shared_visual_material = False + self._has_collision_visible_node = False - # Determine if we should use USD properties or cfg properties. - if not cfg.use_usd_properties: - for entity in entities: - entity.set_body_scale(*cfg.body_scale) - entity.set_physical_attr(cfg.attrs.attr()) - else: - # Read current properties from USD-loaded entities and write back to cfg - # Use first entity as reference - first_entity: MeshObject = entities[0] + def _initialize_spawn_declaration(self, num_instances: int) -> None: + """Initialize instance-dependent declaration state from ``SpawnScene``.""" + if num_instances <= 0: + raise ValueError("A declared RigidObject requires num_instances > 0.") + if self._declared_num_instances is not None: + if self._declared_num_instances != num_instances: + raise RuntimeError( + f"RigidObject {self.uid!r} is already declared for " + f"{self._declared_num_instances} instances." + ) + return - cfg.body_scale = tuple(first_entity.get_body_scale()) - cfg.attrs = RigidBodyAttributesCfg().from_dict( - first_entity.get_physical_attr().as_dict() + self._declared_num_instances = num_instances + self._all_indices = list(range(num_instances)) + self._visual_material = [None] * num_instances + + def _require_declared_num_instances(self) -> int: + """Return the Spawn-provided instance count or raise a lifecycle error.""" + if self._declared_num_instances is None: + raise RuntimeError( + f"RigidObject {self.uid!r} must be registered through SpawnScene " + "before it can be used." ) + return self._declared_num_instances - super().__init__(cfg, entities, device) + @property + def is_spawn_bound(self) -> bool: + """Whether this facade is bound to one finalized Scene.""" + return self._spawn_result is not None - self._initialize_existing_visual_material() + @property + def is_declared(self) -> bool: + """Whether this facade is waiting for its Scene binding.""" + return self._world is None - # set default collision filter - self._set_default_collision_filter() + @property + def num_instances(self) -> int: + if self._entities: + return len(self._entities) + return self._require_declared_num_instances() - if device.type == "cuda": - self._world.update(0.001) - self.reset() + def attach_spawn_handles( + self, + entities: Sequence[SpawnedRigidBody], + ) -> None: + """Store materialized handles without initializing runtime Batch data. + + Default may call this before Spawn finalization so native metadata is + available early. ``bind_spawn()`` remains responsible for creating + result-dependent Batch/Data state after finalization. + """ + handles = list(entities) + expected = self._require_declared_num_instances() + if len(handles) != expected: + raise ValueError( + f"RigidObject {self.uid!r} expected " + f"{expected} Spawn handles, got {len(handles)}." + ) + self._entities = handles + + def _initialize_spawn_bound(self, result: Scene) -> None: + """Create result-dependent runtime state on this declared facade.""" + if not isinstance(result, Scene): + raise TypeError( + "RigidObject binding requires a finalized DexSim Scene; use " + "SimulationManager.prepare()." + ) - # update default center of mass pose (only for non-static bodies with body data). - if self.body_data is not None: - self.body_data.default_com_pose = self.body_data.com_pose.clone() + entities = list(self._entities) + expected = self._require_declared_num_instances() + if len(entities) != expected: + raise ValueError( + f"RigidObject {self.uid!r} expected {expected} Spawn handles, " + f"got {len(entities)}." + ) + + cfg = deepcopy(self.cfg) + self.__dict__.pop("user_ids", None) + self._spawn_result = result + self.body_type = cfg.body_type + self._world = result.world + self._ps = None + self._all_indices = torch.arange(len(entities), dtype=torch.int32).tolist() - # TODO: Must be called after setting all attributes. - # May be improved in the future. - if cfg.attrs.enable_collision is False: - flag = torch.zeros(len(entities), dtype=torch.bool) - self.enable_collision(flag) + # Dynamic and kinematic bodies expose finalized Scene batch data. + self._data = None + if not self.is_static: + self._data = RigidBodyData( + entities=entities, + scene=result, + device=self.device, + ) - # reserve flag for collision visible node existence + # For rendering purposes, each instance can have its own material. + self._visual_material = [None] * len(entities) + self.is_shared_visual_material = False + + super().__init__(cfg, entities, self.device) + self._initialize_existing_visual_material() + self._apply_initial_state() + if self._data is not None: + self._capture_default_physical_properties() self._has_collision_visible_node = False + def bind_spawn( + self, + result: Scene, + ) -> None: + """Atomically bind a declared facade to stable Spawn handles.""" + if self.is_spawn_bound: + raise RuntimeError(f"RigidObject {self.uid!r} is already Spawn-bound.") + if not self.is_declared: + raise RuntimeError( + f"RigidObject {self.uid!r} was not created as a Spawn declaration." + ) + + declared_state = self.__dict__.copy() + try: + self._initialize_spawn_bound(result) + except Exception: + self.__dict__.clear() + self.__dict__.update(declared_state) + raise + def __str__(self) -> str: - parent_str = super().__str__() - max_hull = self.cfg.max_convex_hull_num - if max_hull is MISSING: - if isinstance(self.cfg.shape, MeshCfg): - max_hull = self.cfg.shape.max_convex_hull_num - else: - max_hull = 1 + if self.is_declared: + parent_str = ( + f"{self.__class__}: declared {self.num_instances} Spawn objects " + f"| uid: {self.uid} | device: {self.device}" + ) + else: + parent_str = super().__str__() + max_hull = ( + self.cfg.shape.collision.max_hulls + if isinstance(self.cfg.shape, MeshCfg) + and self.cfg.shape.collision is not None + and self.cfg.shape.collision.max_hulls is not None + else 1 + ) return ( parent_str - + f" | body type: {self.body_type} | max_convex_hull_num: {max_hull}" + + f" | body type: {self.body_type} | collision max_hulls: {max_hull}" ) @cached_property @@ -356,12 +501,135 @@ def body_data(self) -> RigidBodyData | None: return self._data + @property + def default_mass(self) -> torch.Tensor: + """Initialization-time mass retained for backward compatibility.""" + if self._data is None: + raise RuntimeError( + "Static rigid objects do not have a default mass buffer." + ) + return self._data.default_mass + + def _capture_default_physical_properties(self) -> None: + """Capture materialized mass properties as immutable reset defaults.""" + if self._data is None or self._data.default_physical_properties_initialized: + return + if not self._data.body_view.is_ready: + logger.log_error( + "Cannot capture default rigid-body physical properties before " + "the backend view is ready." + ) + self._data.capture_default_physical_properties( + mass=self.get_mass(), + inertia=self.get_inertia(), + com_pose=self._data.com_pose, + ) + + def _restore_default_physical_properties(self, env_ids: Sequence[int]) -> None: + """Restore initialization-time mass properties for selected rows.""" + if ( + self._data is None + or not self._data.default_physical_properties_initialized + or self.is_non_dynamic + or len(env_ids) == 0 + ): + return + + index = torch.as_tensor(env_ids, dtype=torch.long, device=self.device) + self.set_mass(self._data.default_mass[index], env_ids=env_ids) + self.set_inertia(self._data.default_inertia[index], env_ids=env_ids) + self.set_com_pose(self._data.default_com_pose[index], env_ids=env_ids) + + def _get_newton_attr(self, env_idx: int): + """Return DexSim Newton metadata physical attributes for an entity.""" + entity = self._entities[env_idx] + entity_handle = int(entity.get_native_handle()) + if entity_handle < 0: + entity_handle &= _UINT64_MAX + + manager = getattr(self._ps, "manager", None) + attr = None + if manager is not None: + attr = ( + getattr(manager, "dexsim_meta", {}).get(entity_handle, {}).get("attr") + ) + if attr is None: + logger.log_error( + f"Newton physical attributes for rigid object '{self.uid}' env {env_idx} are unavailable." + ) + return attr + + def _get_newton_attr_or_none(self, env_idx: int): + """Return the Newton meta PhysicalAttr, or None when not present. + + Unlike :meth:`_get_newton_attr` this does not raise: objects created + from grouped Spawn descriptors may not carry a legacy ``attr`` mirror. + Used by not-ready setter paths to tolerate that representation. + """ + entity = self._entities[env_idx] + entity_handle = int(entity.get_native_handle()) + if entity_handle < 0: + entity_handle &= _UINT64_MAX + manager = getattr(self._ps, "manager", None) + if manager is None: + return None + return getattr(manager, "dexsim_meta", {}).get(entity_handle, {}).get("attr") + + def _set_newton_attr_meta(self, env_idx: int, physical_attr) -> None: + """Mirror a :class:`dexsim.types.PhysicalAttr` onto the stored Newton meta. + + Newton only models a subset of physical attributes at runtime (mass, + friction, restitution, contact_offset, COM, inertia); the remaining + fields (damping, ccd, sleep thresholds, solver iters, ...) are carried + as metadata for rebuild and for getter consistency. This helper keeps + that mirror in sync so :meth:`get_damping` / :meth:`get_mass` and the + next scene rebuild see the user's intent. + """ + attr = self._get_newton_attr(env_idx) + for name in ( + "mass", + "density", + "dynamic_friction", + "static_friction", + "restitution", + "contact_offset", + "rest_offset", + "linear_damping", + "angular_damping", + "sleep_threshold", + "enable_ccd", + "max_depenetration_velocity", + "min_position_iters", + "min_velocity_iters", + "max_linear_velocity", + "max_angular_velocity", + ): + setattr(attr, name, getattr(physical_attr, name)) + + def _warn_newton_unsupported(self, api_name: str) -> None: + logger.log_warning( + f"Newton backend does not support RigidObject.{api_name} runtime updates. " + "Skipping this call." + ) + + def _newton_lifecycle_state(self) -> str: + manager = getattr(self._ps, "manager", None) + return getattr(getattr(manager, "lifecycle_state", None), "name", "") + + def _can_use_newton_entity_dynamics_fallback(self) -> bool: + """Return whether per-entity Newton patches are safe before GPU view is ready. + + DexSim Newton only supports MeshObject force/torque helpers in ``BUILDER`` + state. Calling them while the model is ``STALE`` can index stale body ids. + """ + return self._newton_lifecycle_state() == "BUILDER" + @property def body_state(self) -> torch.Tensor: """Get the body state of the rigid object. The body state of a rigid object is represented as a tensor with the following format: - [x, y, z, qw, qx, qy, qz, lin_x, lin_y, lin_z, ang_x, ang_y, ang_z] + [x, y, z, qx, qy, qz, qw, lin_x, lin_y, lin_z, ang_x, ang_y, ang_z] If the rigid object is static, linear and angular velocities will be zero. @@ -396,15 +664,6 @@ def is_non_dynamic(self) -> bool: """ return self.body_type in ("static", "kinematic") - def _set_default_collision_filter(self) -> None: - collision_filter_data = torch.zeros( - size=(self.num_instances, 4), dtype=torch.int32 - ) - for i in range(self.num_instances): - collision_filter_data[i, 0] = i - collision_filter_data[i, 1] = 1 - self.set_collision_filter(collision_filter_data) - def set_collision_filter( self, filter_data: torch.Tensor, env_ids: Sequence[int] | None = None ) -> None: @@ -425,6 +684,26 @@ def set_collision_filter( f"Length of env_ids {len(local_env_ids)} does not match pose length {len(filter_data)}." ) + if self.is_spawn_bound: + if self._data is None: + raise NotImplementedError( + "Runtime collision-filter updates are unavailable for static " + "Spawn rigid objects." + ) + body_ids = self._data.body_ids_for(local_env_ids) + self._data.body_view.apply_collision_filter(filter_data, body_ids) + return + + if is_newton_scene(self._ps): + if self._data is None: + raise NotImplementedError( + "Runtime collision-filter updates are unavailable for static " + "Newton rigid objects." + ) + body_ids = self._data.body_ids_for(local_env_ids) + self._data.body_view.apply_collision_filter(filter_data, body_ids) + return + filter_data_np = filter_data.cpu().numpy().astype(np.uint32) for i, env_idx in enumerate(local_env_ids): self._entities[env_idx].get_physical_body().set_collision_filter_data( @@ -447,57 +726,52 @@ def set_local_pose( f"Length of env_ids {len(local_env_ids)} does not match pose length {len(pose)}." ) - if self.device.type == "cpu" or self.is_static: - pose = pose.cpu() - if pose.dim() == 2 and pose.shape[1] == 7: - pose_matrix = torch.eye(4).unsqueeze(0).repeat(pose.shape[0], 1, 1) - pose_matrix[:, :3, 3] = pose[:, :3] - pose_matrix[:, :3, :3] = matrix_from_quat(pose[:, 3:7]) - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].set_local_pose(pose_matrix[i]) - elif pose.dim() == 3 and pose.shape[1:] == (4, 4): - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].set_local_pose(pose[i]) - else: - logger.log_error( - f"Invalid pose shape {pose.shape}. Expected (N, 7) or (N, 4, 4)." - ) - + # Normalize pose to (N, 7) format in (x, y, z, qx, qy, qz, qw). + if pose.dim() == 2 and pose.shape[1] == 7: + target_pose = pose.to(device=self.device, dtype=torch.float32) + elif pose.dim() == 3 and pose.shape[1:] == (4, 4): + xyz = pose[:, :3, 3] + quat = quat_from_matrix(pose[:, :3, :3]) + target_pose = torch.cat((xyz, quat), dim=-1).to( + device=self.device, dtype=torch.float32 + ) else: - if pose.dim() == 2 and pose.shape[1] == 7: - xyz = pose[:, :3] - quat = convert_quat(pose[:, 3:7], to="xyzw") - elif pose.dim() == 3 and pose.shape[1:] == (4, 4): - xyz = pose[:, :3, 3] - quat = quat_from_matrix(pose[:, :3, :3]) - quat = convert_quat(quat, to="xyzw") - else: - logger.log_error( - f"Invalid pose shape {pose.shape}. Expected (N, 7) or (N, 4, 4)." - ) - - # we should keep `pose_` life cycle to the end of the function. - pose = torch.cat((quat, xyz), dim=-1) - indices = self.body_data.gpu_indices[local_env_ids] - torch.cuda.synchronize(self.device) - self._ps.gpu_apply_rigid_body_data( - data=pose.clone(), - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.POSE, + logger.log_error( + f"Invalid pose shape {pose.shape}. Expected (N, 7) or (N, 4, 4)." ) + return + + # Use backend view when pose writes are supported (Newton BUILDER/READY). + if ( + self._data is not None + and self._data.body_view.can_apply_pose + and not self.is_static + ): + body_ids = self._data.body_ids_for(local_env_ids) + self._data.body_view.apply_pose(target_pose, body_ids) + return + + # Static bodies and non-ready backends (notably Newton before finalize) + # still accept direct entity pose updates. + target_pose = target_pose.cpu() + pose_matrix = torch.eye(4).unsqueeze(0).repeat(len(local_env_ids), 1, 1) + pose_matrix[:, :3, 3] = target_pose[:, :3] + pose_matrix[:, :3, :3] = matrix_from_quat(target_pose[:, 3:7]) + for i, env_idx in enumerate(local_env_ids): + self._entities[env_idx].set_local_pose(pose_matrix[i]) def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: """Get local pose of the rigid object. Args: - to_matrix (bool, optional): If True, return the pose as a 4x4 matrix. If False, return as (x, y, z, qw, qx, qy, qz). Defaults to False. + to_matrix (bool, optional): If True, return the pose as a 4x4 matrix. If False, return as (x, y, z, qx, qy, qz, qw). Defaults to False. Returns: torch.Tensor: The local pose of the rigid object with shape (N, 7) or (N, 4, 4) depending on `to_matrix`. """ def get_local_pose_cpu( - entities: List[MeshObject], to_matrix: bool + entities: Sequence[SpawnedRigidBody], to_matrix: bool ) -> torch.Tensor: """Helper function to get local pose on CPU.""" if to_matrix: @@ -514,7 +788,6 @@ def get_local_pose_cpu( np.array([entity.get_rotation_quat() for entity in entities]), dtype=torch.float32, ) - quats = convert_quat(quats, to="wxyz") pose = torch.cat((xyzs, quats), dim=-1) return pose @@ -522,7 +795,7 @@ def get_local_pose_cpu( if self.is_static: return get_local_pose_cpu(self._entities, to_matrix).to(self.device) - pose = self.body_data.pose + pose = self.body_data.pose.clone() if to_matrix: xyz = pose[:, :3] mat = matrix_from_quat(pose[:, 3:7]) @@ -580,28 +853,38 @@ def add_force_torque( f"Length of env_ids {len(local_env_ids)} does not match torque length {len(torque)}." ) - if self.device.type == "cpu": - for i, env_idx in enumerate(local_env_ids): - if force is not None: - self._entities[env_idx].add_force(force[i].cpu().numpy()) - if torque is not None: - self._entities[env_idx].add_torque(torque[i].cpu().numpy()) + if pos is not None: + logger.log_warning( + "RigidObject.add_force_torque(pos=...) is not supported yet; " + "applying wrench at center of mass." + ) - else: - indices = self.body_data.gpu_indices[local_env_ids] - torch.cuda.synchronize(self.device) + if self._data is not None and self._data.body_view.is_ready: + body_ids = self._data.body_ids_for(local_env_ids) if force is not None: - self._ps.gpu_apply_rigid_body_data( - data=force, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.FORCE, - ) + self._data.body_view.apply_force(force, body_ids) if torque is not None: - self._ps.gpu_apply_rigid_body_data( - data=torque, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.TORQUE, - ) + self._data.body_view.apply_torque(torque, body_ids) + elif ( + self._data is not None + and self._data.is_newton_backend + and self._can_use_newton_entity_dynamics_fallback() + ): + force_np = force.detach().cpu().numpy() if force is not None else None + torque_np = torque.detach().cpu().numpy() if torque is not None else None + for i, env_idx in enumerate(local_env_ids): + entity = self._entities[env_idx] + if force_np is not None: + entity.add_force(force_np[i]) + if torque_np is not None: + entity.add_torque(torque_np[i]) + elif self._data is not None and self._data.is_newton_backend: + logger.log_warning( + "Cannot apply force or torque while Newton model is stale or " + "unprepared; call SimulationManager.prepare() first." + ) + else: + logger.log_error("Cannot apply force or torque before body view is ready.") def set_velocity( self, @@ -638,57 +921,142 @@ def set_velocity( f"Length of env_ids {len(local_env_ids)} does not match ang_vel length {len(ang_vel)}." ) - if self.device.type == "cpu": - for i, env_idx in enumerate(local_env_ids): - if lin_vel is not None: - self._entities[env_idx].set_linear_velocity( - lin_vel[i].cpu().numpy() - ) - if ang_vel is not None: - self._entities[env_idx].set_angular_velocity( - ang_vel[i].cpu().numpy() - ) - else: - indices = self.body_data.gpu_indices[local_env_ids] - torch.cuda.synchronize(self.device) + if self._data is not None and self._data.body_view.is_ready: + body_ids = self._data.body_ids_for(local_env_ids) if lin_vel is not None: - self._ps.gpu_apply_rigid_body_data( - data=lin_vel, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.LINEAR_VELOCITY, - ) + self._data.body_view.apply_linear_velocity(lin_vel, body_ids) if ang_vel is not None: - self._ps.gpu_apply_rigid_body_data( - data=ang_vel, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.ANGULAR_VELOCITY, - ) + self._data.body_view.apply_angular_velocity(ang_vel, body_ids) + elif ( + self._data is not None + and self._data.is_newton_backend + and self._can_use_newton_entity_dynamics_fallback() + ): + lin_vel_np = lin_vel.detach().cpu().numpy() if lin_vel is not None else None + ang_vel_np = ang_vel.detach().cpu().numpy() if ang_vel is not None else None + for i, env_idx in enumerate(local_env_ids): + entity = self._entities[env_idx] + if lin_vel_np is not None: + entity.set_linear_velocity(lin_vel_np[i]) + if ang_vel_np is not None: + entity.set_angular_velocity(ang_vel_np[i]) + elif self._data is not None and self._data.is_newton_backend: + logger.log_warning( + "Cannot set velocity while Newton model is stale or unprepared; " + "call SimulationManager.prepare() first." + ) + else: + logger.log_error("Cannot set velocity before body view is ready.") def set_attrs( self, - attrs: Union[RigidBodyAttributesCfg, List[RigidBodyAttributesCfg]], + attrs: RigidBodyPhysicsCfg | list[RigidBodyPhysicsCfg], env_ids: Sequence[int] | None = None, ) -> None: """Set physical attributes for the rigid object. Args: - attrs (Union[RigidBodyAttributesCfg, List[RigidBodyAttributesCfg]]): The physical attributes to set. + attrs: Grouped physical attributes, shared or one per environment. env_ids (Sequence[int] | None, optional): Environment indices. If None, then all indices are used. """ local_env_ids = self._all_indices if env_ids is None else env_ids - if isinstance(attrs, List) and len(local_env_ids) != len(attrs): + if isinstance(attrs, list) and len(local_env_ids) != len(attrs): logger.log_error( f"Length of env_ids {len(local_env_ids)} does not match attrs length {len(attrs)}." ) - # TODO: maybe need to improve the physical attributes setter efficiency. - if isinstance(attrs, RigidBodyAttributesCfg): - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].set_physical_attr(attrs.attr()) + # Resolve per-env physical attrs into a flat list aligned with local_env_ids. + if isinstance(attrs, RigidBodyPhysicsCfg): + physical_attrs = [attrs.to_dexsim_physical_attr() for _ in local_env_ids] else: - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].set_physical_attr(attrs[i].attr()) + physical_attrs = [a.to_dexsim_physical_attr() for a in attrs] + + if self.is_spawn_bound: + if self._data is None: + raise NotImplementedError( + "Runtime physical attributes are unavailable for static " + "Spawn rigid objects." + ) + body_ids = self._data.body_ids_for(local_env_ids) + view = self._data.body_view + + def _stack(field: str) -> torch.Tensor: + return torch.as_tensor( + [getattr(attr, field) for attr in physical_attrs], + dtype=torch.float32, + device=self.device, + ).unsqueeze(-1) + + if any( + attr.static_friction != attr.dynamic_friction for attr in physical_attrs + ): + logger.log_warning( + "DexSim Spawn exposes one backend-neutral friction value; " + "set_attrs() uses dynamic_friction for both coefficients." + ) + view.apply_mass(_stack("mass"), body_ids) + view.apply_friction(_stack("dynamic_friction"), body_ids) + view.apply_restitution(_stack("restitution"), body_ids) + view.apply_contact_offset(_stack("contact_offset"), body_ids) + view.apply_damping( + torch.cat( + (_stack("linear_damping"), _stack("angular_damping")), + dim=1, + ), + body_ids, + ) + return + + if is_newton_scene(self._ps): + self._set_newton_attrs(physical_attrs, local_env_ids) + return + + # TODO: maybe need to improve the physical attributes setter efficiency. + for i, env_idx in enumerate(local_env_ids): + self._entities[env_idx].set_physical_attr(physical_attrs[i]) + + def _set_newton_attrs( + self, + physical_attrs: list, + local_env_ids, + ) -> None: + """Apply physical attributes on the Newton backend. + + Newton models only a subset of physical attributes at runtime + (mass, friction, restitution, contact_offset); the rest (damping, ccd, + sleep thresholds, solver iters, rest_offset, static_friction) are + metadata carried for rebuild and getter consistency. When the Newton + model is finalized (READY/STALE) the supported subset is pushed live + via the batch scene API; beforehand (BUILDER) the attributes are only + mirrored onto the meta so the next finalize consumes them. + """ + for i, env_idx in enumerate(local_env_ids): + self._set_newton_attr_meta(env_idx, physical_attrs[i]) + + if self._data is None or not self._data.body_view.is_ready: + logger.log_debug( + "Newton model is not prepared; physical attributes are mirrored " + "to metadata and applied at the next prepare()." + ) + return + + body_ids = self._data.body_ids_for(local_env_ids) + view = self._data.body_view + device = self.device + + def _stack(field: str) -> torch.Tensor: + return torch.as_tensor( + [getattr(a, field) for a in physical_attrs], + dtype=torch.float32, + device=device, + ).unsqueeze(-1) + + # Newton-supported runtime subset. + view.apply_mass(_stack("mass"), body_ids) + view.apply_friction(_stack("dynamic_friction"), body_ids) + view.apply_restitution(_stack("restitution"), body_ids) + view.apply_contact_offset(_stack("contact_offset"), body_ids) def set_mass( self, mass: torch.Tensor, env_ids: Sequence[int] | None = None @@ -706,9 +1074,24 @@ def set_mass( f"Length of env_ids {len(local_env_ids)} does not match mass length {len(mass)}." ) - mass = mass.cpu().numpy() + if self._data is not None and self._data.body_view.is_ready: + body_ids = self._data.body_ids_for(local_env_ids) + self._data.body_view.apply_mass( + mass.to(dtype=torch.float32, device=self.device).unsqueeze(-1), + body_ids, + ) + return + + mass_np = mass.cpu().numpy() for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].get_physical_body().set_mass(mass[i]) + if is_newton_scene(self._ps): + # Not finalized: mirror to meta (consumed at next finalize). The + # Default-backend set_mass is not patched for Newton entities. + attr = self._get_newton_attr_or_none(env_idx) + if attr is not None: + attr.mass = float(mass_np[i]) + else: + self._entities[env_idx].get_physical_body().set_mass(mass_np[i]) def get_mass(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """Get mass for the rigid object. @@ -721,9 +1104,38 @@ def get_mass(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """ local_env_ids = self._all_indices if env_ids is None else env_ids + if self.is_spawn_bound and self.is_static: + # Static actors have no finite runtime mass (and Newton therefore + # gives them no body id), but the legacy API exposed their authored + # configuration. Preserve that readable metadata contract without + # manufacturing a dynamic-body batch solely for property queries. + configured_mass = self.cfg.attrs.to_dexsim_physical_attr().mass + value = 0.0 if configured_mass is None else float(configured_mass) + return torch.full( + (len(local_env_ids),), + value, + dtype=torch.float32, + device=self.device, + ) + + if self._data is not None and self._data.body_view.is_ready: + if env_ids is None: + return self._data.mass + body_ids = self._data.body_ids_for(local_env_ids) + buf = torch.empty( + (len(local_env_ids), 1), + dtype=torch.float32, + device=self.device, + ) + self._data.body_view.fetch_mass(buf, body_ids) + return buf.squeeze(-1) + masses = [] for _, env_idx in enumerate(local_env_ids): - mass = self._entities[env_idx].get_physical_body().get_mass() + if is_newton_scene(self._ps): + mass = self._get_newton_attr(env_idx).mass + else: + mass = self._entities[env_idx].get_physical_body().get_mass() masses.append(mass) return torch.as_tensor(masses, dtype=torch.float32, device=self.device) @@ -744,12 +1156,30 @@ def set_friction( f"Length of env_ids {len(local_env_ids)} does not match friction length {len(friction)}." ) - friction = friction.cpu().numpy() - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].get_physical_body().set_dynamic_friction( - friction[i] + if self._data is not None and self._data.body_view.is_ready: + body_ids = self._data.body_ids_for(local_env_ids) + self._data.body_view.apply_friction( + friction.to(dtype=torch.float32, device=self.device).unsqueeze(-1), + body_ids, ) - self._entities[env_idx].get_physical_body().set_static_friction(friction[i]) + return + + friction_np = friction.cpu().numpy() + for i, env_idx in enumerate(local_env_ids): + if is_newton_scene(self._ps): + # Not finalized: mirror to meta (Newton has a single mu; consumed + # at next finalize). The Default-backend friction setters are not + # patched for Newton entities. + attr = self._get_newton_attr_or_none(env_idx) + if attr is not None: + attr.dynamic_friction = float(friction_np[i]) + else: + self._entities[env_idx].get_physical_body().set_dynamic_friction( + friction_np[i] + ) + self._entities[env_idx].get_physical_body().set_static_friction( + friction_np[i] + ) def get_friction(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """Get friction for the rigid object. @@ -762,11 +1192,28 @@ def get_friction(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """ local_env_ids = self._all_indices if env_ids is None else env_ids + if self.is_spawn_bound and self.is_static: + return torch.full( + (len(local_env_ids),), + float(self.cfg.attrs.to_dexsim_physical_attr().dynamic_friction), + dtype=torch.float32, + device=self.device, + ) + + if self._data is not None and self._data.body_view.is_ready: + body_ids = self._data.body_ids_for(local_env_ids) + buf = self._data._friction[: len(local_env_ids)] + self._data.body_view.fetch_friction(buf, body_ids) + return buf.squeeze(-1) + frictions = [] for _, env_idx in enumerate(local_env_ids): - friction = ( - self._entities[env_idx].get_physical_body().get_dynamic_friction() - ) + if is_newton_scene(self._ps): + friction = self._get_newton_attr(env_idx).dynamic_friction + else: + friction = ( + self._entities[env_idx].get_physical_body().get_dynamic_friction() + ) frictions.append(friction) return torch.as_tensor(frictions, dtype=torch.float32, device=self.device) @@ -779,6 +1226,12 @@ def set_damping( Args: damping (torch.Tensor): The damping to set with shape (N, 2), where the first column is linear damping and the second column is angular damping. env_ids (Sequence[int] | None, optional): Environment indices. If None, then all indices are used. + + .. attention:: + The Newton backend does not simulate per-body linear/angular damping + (its damping is a global solver knob). On Newton this call mirrors + the values onto the attribute metadata so :meth:`get_damping` and + scene rebuilds stay consistent, but has no runtime effect. """ local_env_ids = self._all_indices if env_ids is None else env_ids @@ -787,13 +1240,31 @@ def set_damping( f"Length of env_ids {len(local_env_ids)} does not match damping length {len(damping)}." ) - damping = damping.cpu().numpy() + damping = damping.to(dtype=torch.float32, device=self.device) + + if self.is_spawn_bound: + if self._data is None: + raise NotImplementedError( + "Runtime damping is unavailable for static Spawn rigid objects." + ) + body_ids = self._data.body_ids_for(local_env_ids) + self._data.body_view.apply_damping(damping, body_ids) + return + + if is_newton_scene(self._ps): + for i, env_idx in enumerate(local_env_ids): + attr = self._get_newton_attr(env_idx) + attr.linear_damping = float(damping[i, 0].item()) + attr.angular_damping = float(damping[i, 1].item()) + return + + damping_np = damping.cpu().numpy() for i, env_idx in enumerate(local_env_ids): self._entities[env_idx].get_physical_body().set_linear_damping( - damping[i, 0] + damping_np[i, 0] ) self._entities[env_idx].get_physical_body().set_angular_damping( - damping[i, 1] + damping_np[i, 1] ) def get_damping(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: @@ -807,14 +1278,38 @@ def get_damping(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """ local_env_ids = self._all_indices if env_ids is None else env_ids + if self.is_spawn_bound: + if self._data is None: + return torch.tensor( + [ + self.cfg.attrs.to_dexsim_physical_attr().linear_damping, + self.cfg.attrs.to_dexsim_physical_attr().angular_damping, + ], + dtype=torch.float32, + device=self.device, + ).repeat(len(local_env_ids), 1) + body_ids = self._data.body_ids_for(local_env_ids) + damping = torch.empty( + (len(local_env_ids), 2), + dtype=torch.float32, + device=self.device, + ) + self._data.body_view.fetch_damping(damping, body_ids) + return damping + dampings = [] for _, env_idx in enumerate(local_env_ids): - linear_damping = ( - self._entities[env_idx].get_physical_body().get_linear_damping() - ) - angular_damping = ( - self._entities[env_idx].get_physical_body().get_angular_damping() - ) + if is_newton_scene(self._ps): + attr = self._get_newton_attr(env_idx) + linear_damping = attr.linear_damping + angular_damping = attr.angular_damping + else: + linear_damping = ( + self._entities[env_idx].get_physical_body().get_linear_damping() + ) + angular_damping = ( + self._entities[env_idx].get_physical_body().get_angular_damping() + ) dampings.append([linear_damping, angular_damping]) return torch.as_tensor(dampings, dtype=torch.float32, device=self.device) @@ -835,11 +1330,26 @@ def set_inertia( f"Length of env_ids {len(local_env_ids)} does not match inertia length {len(inertia)}." ) - inertia = inertia.cpu().numpy() - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].get_physical_body().set_mass_space_inertia_tensor( - inertia[i] + if self._data is not None and self._data.body_view.is_ready: + body_ids = self._data.body_ids_for(local_env_ids) + self._data.body_view.apply_inertia_diagonal( + inertia.to(dtype=torch.float32, device=self.device), + body_ids, ) + return + + inertia_np = inertia.cpu().numpy() + for i, env_idx in enumerate(local_env_ids): + if is_newton_scene(self._ps): + # Not finalized: mirror to meta (consumed at next finalize). The + # Default-backend inertia setter is not patched for Newton entities. + attr = self._get_newton_attr_or_none(env_idx) + if attr is not None: + attr.inertia = np.asarray(inertia_np[i], dtype=np.float32) + else: + self._entities[ + env_idx + ].get_physical_body().set_mass_space_inertia_tensor(inertia_np[i]) def get_inertia(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """Get inertia tensor for the rigid object. @@ -852,13 +1362,37 @@ def get_inertia(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """ local_env_ids = self._all_indices if env_ids is None else env_ids + if self.is_spawn_bound and self.is_static: + # Static actors have infinite mass, so no finite inertia tensor is + # represented by either Spawn backend. + return torch.zeros( + (len(local_env_ids), 3), + dtype=torch.float32, + device=self.device, + ) + + if self._data is not None and self._data.body_view.is_ready: + if env_ids is None: + return self._data.inertia + body_ids = self._data.body_ids_for(local_env_ids) + buf = torch.empty( + (len(local_env_ids), 3), + dtype=torch.float32, + device=self.device, + ) + self._data.body_view.fetch_inertia_diagonal(buf, body_ids) + return buf + inertias = [] for _, env_idx in enumerate(local_env_ids): - inertia = ( - self._entities[env_idx] - .get_physical_body() - .get_mass_space_inertia_tensor() - ) + if is_newton_scene(self._ps): + inertia = self._get_newton_attr(env_idx).inertia + else: + inertia = ( + self._entities[env_idx] + .get_physical_body() + .get_mass_space_inertia_tensor() + ) inertias.append(inertia) return torch.as_tensor( @@ -1109,7 +1643,7 @@ def set_body_scale( def set_com_pose( self, com_pose: torch.Tensor, env_ids: Sequence[int] | None = None ) -> None: - """Set the center of mass pose of the rigid body. The pose format is (x, y, z, qw, qx, qy, qz). + """Set the center of mass pose of the rigid body. The pose format is (x, y, z, qx, qy, qz, qw). Args: com_pose (torch.Tensor): The center of mass pose to set with shape (N, 7). @@ -1128,11 +1662,13 @@ def set_com_pose( f"Length of env_ids {len(local_env_ids)} does not match com_pose length {len(com_pose)}." ) - com_pose = com_pose.cpu().numpy() - for i, env_idx in enumerate(local_env_ids): - pos = com_pose[i, :3] - quat = com_pose[i, 3:7] - self._entities[env_idx].get_physical_body().set_cmass_local_pose(pos, quat) + if self._data is not None: + target_com_pose = com_pose.to(device=self.device, dtype=torch.float32) + body_ids = self._data.body_ids_for(local_env_ids) + self._data.body_view.apply_com_local_pose(target_com_pose, body_ids) + return + + logger.log_error("Cannot set center of mass pose before body view is ready.") def set_body_type(self, body_type: str) -> None: """Set the body type of the rigid object. @@ -1142,9 +1678,28 @@ def set_body_type(self, body_type: str) -> None: Args: body_type (str): The body type to set. Must be one of 'dynamic', or 'kinematic'. + + .. attention:: + On the Newton backend, body type (dynamic/kinematic/static) is fixed + at body registration and cannot be changed at runtime; switching it + would require re-registering the body and rebuilding the model. This + call is therefore a no-op on Newton. """ from dexsim.types import ActorType + if self.is_spawn_bound: + raise NotImplementedError( + "Changing actor topology after Spawn binding requires a public " + "descriptor mutation transaction and is not implemented yet." + ) + + if is_newton_scene(self._ps): + logger.log_warning( + "Newton backend does not support changing RigidObject body type at " + "runtime (it is fixed at registration). Skipping set_body_type call." + ) + return + if body_type not in ("dynamic", "kinematic"): logger.log_error( f"Invalid body type {body_type}. Must be one of 'dynamic', or 'kinematic'." @@ -1254,36 +1809,29 @@ def clear_dynamics(self, env_ids: Sequence[int] | None = None) -> None: local_env_ids = self._all_indices if env_ids is None else env_ids - if self.device.type == "cpu": - for env_idx in local_env_ids: - self._entities[env_idx].clear_dynamics() - else: - # Apply zero force and torque to the rigid bodies. + if self._data is not None and self._data.body_view.is_ready: zeros = torch.zeros( (len(local_env_ids), 3), dtype=torch.float32, device=self.device ) - indices = self.body_data.gpu_indices[local_env_ids] - torch.cuda.synchronize(self.device) - self._ps.gpu_apply_rigid_body_data( - data=zeros, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.LINEAR_VELOCITY, - ) - self._ps.gpu_apply_rigid_body_data( - data=zeros, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.ANGULAR_VELOCITY, - ) - self._ps.gpu_apply_rigid_body_data( - data=zeros, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.FORCE, - ) - self._ps.gpu_apply_rigid_body_data( - data=zeros, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.TORQUE, + body_ids = self._data.body_ids_for(local_env_ids) + self._data.body_view.apply_linear_velocity(zeros, body_ids) + self._data.body_view.apply_angular_velocity(zeros, body_ids) + self._data.body_view.apply_force(zeros, body_ids) + self._data.body_view.apply_torque(zeros, body_ids) + elif ( + self._data is not None + and self._data.is_newton_backend + and self._can_use_newton_entity_dynamics_fallback() + ): + for env_idx in local_env_ids: + self._entities[env_idx].clear_dynamics() + elif self._data is not None and self._data.is_newton_backend: + logger.log_warning( + "Cannot clear dynamics while Newton model is stale or unprepared; " + "call SimulationManager.prepare() first." ) + else: + logger.log_error("Cannot clear dynamics before body view is ready.") def set_physical_visible( self, @@ -1300,6 +1848,13 @@ def set_physical_visible( if len(rgba) != 4: logger.log_error(f"Invalid rgba {rgba}, should be a sequence of 4 floats.") + if self.is_spawn_bound: + color = np.asarray(rgba, dtype=np.float32) + for entity in self._entities: + self._spawn_result.set_physical_visible(entity, color, visible) + self._has_collision_visible_node = True + return + # create collision visible node if not exist if visible: if not self._has_collision_visible_node: @@ -1329,14 +1884,19 @@ def set_visible(self, visible: bool = True) -> None: for i, env_idx in enumerate(self._all_indices): self._entities[env_idx].set_visible(visible) - def reset(self, env_ids: Sequence[int] | None = None) -> None: - local_env_ids = self._all_indices if env_ids is None else env_ids - num_instances = len(local_env_ids) - - self.restore_visual_material(env_ids=local_env_ids) - - self.set_attrs(self.cfg.attrs, env_ids=local_env_ids) - + def _build_cfg_init_pose(self, env_ids: Sequence[int]) -> torch.Tensor: + """Build initial root poses from cfg as ``(N, 4, 4)`` matrices.""" + num_instances = len(env_ids) + if self.cfg.init_local_pose is not None: + return ( + torch.as_tensor( + self.cfg.init_local_pose, + dtype=torch.float32, + device=self.device, + ) + .reshape(1, 4, 4) + .repeat(num_instances, 1, 1) + ) pos = torch.as_tensor( self.cfg.init_pos, dtype=torch.float32, device=self.device ) @@ -1355,14 +1915,70 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: ) pose[:, :3, 3] = pos pose[:, :3, :3] = mat - self.set_local_pose(pose, env_ids=local_env_ids) + return pose + + def _apply_initial_state(self) -> None: + """Apply cfg initial pose after construction. + + The Default backend runs a full reset. Newton applies init pose in + ``BUILDER`` via the scene batch API; velocities are cleared after + preparation through :meth:`SimulationManager.prepare`. + """ + if self.is_spawn_bound: + if self._spawn_result.backend == "dexsim": + # DexSim Direct GPU readiness performs native warm-up updates. + # Re-apply the authored state after the batch becomes usable + # so prepare() itself is not an observable simulation step. + self.reset() + else: + # Newton finalization materializes the descriptor pose without + # advancing simulation; only one-step dynamics buffers need + # clearing after batch binding. + if not is_newton_gradient_mode(self._spawn_result): + self.clear_dynamics() + return + + if is_newton_scene(self._ps): + if self._newton_lifecycle_state() == "BUILDER": + self.set_local_pose( + self._build_cfg_init_pose(self._all_indices), + env_ids=self._all_indices, + ) + return + + if self.device.type == "cuda": + self._world.update(0.001) + self.reset() + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + local_env_ids = self._all_indices if env_ids is None else env_ids + + self.restore_visual_material(env_ids=local_env_ids) + + # Preserve the legacy Default-backend attribute reset before restoring + # the backend-resolved mass-property snapshot below. + if not self.is_spawn_bound and not is_newton_scene(self._ps): + self.set_attrs(self.cfg.attrs, env_ids=local_env_ids) + + self._restore_default_physical_properties(local_env_ids) self.clear_dynamics(env_ids=local_env_ids) + self.set_local_pose( + self._build_cfg_init_pose(local_env_ids), env_ids=local_env_ids + ) + def destroy(self) -> None: + if self.is_declared or self.is_spawn_bound: + # SimulationManager owns topology removal and Scene lifetime. + # Direct facade destruction must never bypass that owner. + return env = self._world.get_env() arenas = env.get_all_arenas() if len(arenas) == 0: arenas = [env] for i, entity in enumerate(self._entities): - arenas[i].remove_actor(entity) + if is_newton_scene(self._ps): + arenas[i].remove_actor(entity.get_name()) + else: + arenas[i].remove_actor(entity) diff --git a/embodichain/lab/sim/objects/rigid_object_group.py b/embodichain/lab/sim/objects/rigid_object_group.py index 0f6192d28..c36d40a6f 100644 --- a/embodichain/lab/sim/objects/rigid_object_group.py +++ b/embodichain/lab/sim/objects/rigid_object_group.py @@ -16,249 +16,294 @@ from __future__ import annotations -import torch -import dexsim +from copy import deepcopy +from typing import TYPE_CHECKING, Sequence + import numpy as np +import torch +from dexsim.scene import Scene + +from embodichain.lab.sim import BatchEntity +from embodichain.lab.sim.cfg import RigidObjectGroupCfg +from embodichain.lab.sim.material import VisualMaterial +from embodichain.lab.sim.objects.backends.scene import SceneRigidBodyView +from embodichain.utils.math import ( + matrix_from_euler, + matrix_from_quat, + quat_from_matrix, +) -from dataclasses import dataclass -from typing import List, Sequence, Union +from ._mesh_utils import get_combined_triangles, get_combined_vertices -from dexsim.models import MeshObject -from dexsim.types import RigidBodyGPUAPIReadType, RigidBodyGPUAPIWriteType -from dexsim.engine import CudaArray, PhysicsScene -from embodichain.lab.sim.cfg import ( - RigidObjectGroupCfg, - RigidBodyAttributesCfg, -) -from embodichain.lab.sim import ( - BatchEntity, -) -from embodichain.lab.sim.material import VisualMaterial, VisualMaterialInst -from ._mesh_utils import ( - get_combined_triangles, - get_combined_vertices, -) -from embodichain.utils.math import convert_quat -from embodichain.utils.math import matrix_from_quat, quat_from_matrix, matrix_from_euler -from embodichain.utils import logger +if TYPE_CHECKING: + from dexsim.scene import SpawnedRigidBody __all__ = ["RigidBodyGroupData", "RigidObjectGroup", "RigidObjectGroupCfg"] -@dataclass class RigidBodyGroupData: - """Data manager for rigid body group with body type of dynamic or kinematic.""" + """Expose one flat Scene rigid-body batch as ``[env, object, ...]`` tensors.""" def __init__( - self, entities: List[List[MeshObject]], ps: PhysicsScene, device: torch.device + self, + body_view: SceneRigidBodyView, + *, + num_instances: int, + num_objects: int, + device: torch.device, ) -> None: - """Initialize the RigidBodyGroupData. - - Args: - entities (List[List[MeshObject]]): List of List MeshObjects representing the rigid body group. - ps (PhysicsScene): The physics scene. - device (torch.device): The device to use for the rigid body group data. - """ - self.entities = entities - self.ps = ps - self.num_instances = len(entities) - self.num_objects = len(entities[0]) + self.body_view = body_view + self.num_instances = num_instances + self.num_objects = num_objects self.device = device - - # get gpu indices for the rigid bodies with shape of (num_instances, num_objects) - self.gpu_indices = ( - torch.as_tensor( - [ - [entity.get_gpu_index() for entity in instance] - for instance in entities - ], - dtype=torch.int32, - device=self.device, - ) - if self.device.type == "cuda" - else None + self._pose = torch.empty( + (num_instances, num_objects, 7), dtype=torch.float32, device=device ) - - # Initialize rigid body group data tensors. Shape of (num_instances, num_objects, data_dim) - self._pose = torch.zeros( - (self.num_instances, self.num_objects, 7), + self._lin_vel = torch.empty( + (num_instances, num_objects, 3), dtype=torch.float32, device=device + ) + self._ang_vel = torch.empty_like(self._lin_vel) + self._mass = torch.empty( + (num_instances, num_objects, 1), dtype=torch.float32, - device=self.device, + device=device, ) - self._lin_vel = torch.zeros( - (self.num_instances, self.num_objects, 3), + self._inertia = torch.empty( + (num_instances, num_objects, 3), dtype=torch.float32, - device=self.device, + device=device, ) - self._ang_vel = torch.zeros( - (self.num_instances, self.num_objects, 3), + self._com_pose = torch.empty( + (num_instances, num_objects, 7), dtype=torch.float32, - device=self.device, + device=device, ) + self._default_mass: torch.Tensor | None = None + self._default_inertia: torch.Tensor | None = None + self._default_com_pose: torch.Tensor | None = None @property def pose(self) -> torch.Tensor: - if self.device.type == "cpu": - # Fetch pose from CPU entities - xyzs = torch.as_tensor( - [ - [entity.get_location() for entity in instance] - for instance in self.entities - ], - device=self.device, - ) - quats = torch.as_tensor( - [ - [entity.get_rotation_quat() for entity in instance] - for instance in self.entities - ], - device=self.device, - ) - quats = convert_quat(quats.reshape(-1, 4), to="wxyz").reshape( - -1, self.num_objects, 4 - ) - return torch.cat((xyzs, quats), dim=-1) - else: - pose = self._pose.reshape(-1, 7) - self.ps.gpu_fetch_rigid_body_data( - data=pose, - gpu_indices=self.gpu_indices.flatten(), - data_type=RigidBodyGPUAPIReadType.POSE, - ) - pose = convert_quat(pose[:, :4], to="wxyz") - pose = pose[:, [4, 5, 6, 0, 1, 2, 3]] - return self._pose + """Local poses in EmbodiChain ``xyz + xyzw`` order.""" + flat = self._pose.reshape(-1, 7) + self.body_view.fetch_pose(flat) + return self._pose @property def lin_vel(self) -> torch.Tensor: - if self.device.type == "cpu": - # Fetch linear velocity from CPU entities - self._lin_vel = torch.as_tensor( - [ - [entity.get_linear_velocity() for entity in instance] - for instance in self.entities - ], - dtype=torch.float32, - device=self.device, - ) - else: - lin_vel = self._lin_vel.reshape(-1, 3) - self.ps.gpu_fetch_rigid_body_data( - data=lin_vel, - gpu_indices=self.gpu_indices.flatten(), - data_type=RigidBodyGPUAPIReadType.LINEAR_VELOCITY, - ) + self.body_view.fetch_linear_velocity(self._lin_vel.reshape(-1, 3)) return self._lin_vel @property def ang_vel(self) -> torch.Tensor: - if self.device.type == "cpu": - # Fetch angular velocity from CPU entities - self._ang_vel = torch.as_tensor( - [ - [entity.get_angular_velocity() for entity in instance] - for instance in self.entities - ], - dtype=torch.float32, - device=self.device, - ) - else: - ang_vel = self._ang_vel.reshape(-1, 3) - self.ps.gpu_fetch_rigid_body_data( - data=ang_vel, - gpu_indices=self.gpu_indices.flatten(), - data_type=RigidBodyGPUAPIReadType.ANGULAR_VELOCITY, - ) + self.body_view.fetch_angular_velocity(self._ang_vel.reshape(-1, 3)) return self._ang_vel @property def vel(self) -> torch.Tensor: - """Get the linear and angular velocities of the rigid bodies. - - Returns: - torch.Tensor: The linear and angular velocities concatenated, with shape (num_instances, num_objects, 6). - """ + """Linear and angular velocities with shape ``[env, object, 6]``.""" return torch.cat((self.lin_vel, self.ang_vel), dim=-1) + @property + def mass(self) -> torch.Tensor: + """Current masses with shape ``[env, object]``.""" + self.body_view.fetch_mass(self._mass.reshape(-1, 1)) + return self._mass.squeeze(-1) + + @property + def inertia(self) -> torch.Tensor: + """Current inertia diagonals with shape ``[env, object, 3]``.""" + self.body_view.fetch_inertia_diagonal(self._inertia.reshape(-1, 3)) + return self._inertia + + @property + def com_pose(self) -> torch.Tensor: + """Current local COM poses in Group ``xyz + xyzw`` convention.""" + flat = self._com_pose.reshape(-1, 7) + self.body_view.fetch_com_local_pose(flat) + return self._com_pose + + @property + def default_physical_properties_initialized(self) -> bool: + """Whether initialization-time mass properties are available.""" + return ( + self._default_mass is not None + and self._default_inertia is not None + and self._default_com_pose is not None + ) + + @property + def default_mass(self) -> torch.Tensor: + """Initialization-time masses with shape ``[env, object]``.""" + if self._default_mass is None: + raise RuntimeError("Default rigid-object Group masses are unavailable.") + return self._default_mass + + @property + def default_inertia(self) -> torch.Tensor: + """Initialization-time inertia diagonals.""" + if self._default_inertia is None: + raise RuntimeError("Default rigid-object Group inertias are unavailable.") + return self._default_inertia + + @property + def default_com_pose(self) -> torch.Tensor: + """Initialization-time local COM poses in ``xyz + xyzw`` order.""" + if self._default_com_pose is None: + raise RuntimeError("Default rigid-object Group COM poses are unavailable.") + return self._default_com_pose + + def capture_default_physical_properties( + self, + *, + mass: torch.Tensor, + inertia: torch.Tensor, + com_pose: torch.Tensor, + ) -> None: + """Capture backend-resolved Group mass properties exactly once.""" + expected_shapes = { + "mass": (self.num_instances, self.num_objects), + "inertia": (self.num_instances, self.num_objects, 3), + "com_pose": (self.num_instances, self.num_objects, 7), + } + values = {"mass": mass, "inertia": inertia, "com_pose": com_pose} + for name, value in values.items(): + if tuple(value.shape) != expected_shapes[name]: + raise ValueError( + f"Expected {name} shape {expected_shapes[name]}, " + f"got {tuple(value.shape)}." + ) + if self.default_physical_properties_initialized: + raise RuntimeError( + "Default rigid-object Group mass properties are already captured." + ) + + self._default_mass = mass.to(self.device, dtype=torch.float32).clone() + self._default_inertia = inertia.to(self.device, dtype=torch.float32).clone() + self._default_com_pose = com_pose.to(self.device, dtype=torch.float32).clone() + class RigidObjectGroup(BatchEntity): - """RigidObjectGroup represents a batch of rigid bodies in the simulation.""" + """A two-dimensional view over rigid objects owned by DexSim Spawn.""" def __init__( self, cfg: RigidObjectGroupCfg, - entities: List[List[MeshObject]] = None, device: torch.device = torch.device("cpu"), ) -> None: - self.body_type = cfg.body_type - - self._world = dexsim.default_world() - self._ps = self._world.get_physics_scene() - - self._all_indices = torch.arange(len(entities), dtype=torch.int32).tolist() - self._all_obj_indices = torch.arange( - len(entities[0]), dtype=torch.int32 - ).tolist() - - # data for managing body data (only for dynamic and kinematic bodies) on GPU. - self._data = RigidBodyGroupData(entities=entities, ps=self._ps, device=device) + """Create an unregistered rigid-object-group facade. - body_cfgs = list(cfg.rigid_objects.values()) - for instance in entities: - for i, body in enumerate(instance): - body.set_body_scale(*body_cfgs[i].body_scale) - body.set_physical_attr(body_cfgs[i].attrs.attr()) + ``SpawnScene`` supplies the replicated instance count at declaration + time and the finalized ``Scene`` at binding time. + """ + self.cfg = deepcopy(cfg) + self.uid = self.cfg.uid + self.device = device + self.body_type = self.cfg.body_type + self._declared_num_objects = len(self.cfg.rigid_objects) + self._entities: list[list[SpawnedRigidBody]] = [] + self._declared_num_instances: int | None = None + self._spawn_result: Scene | None = None + self._data: RigidBodyGroupData | None = None + self._all_indices: list[int] = [] + self._all_obj_indices = list(range(self._declared_num_objects)) + + def _initialize_spawn_declaration(self, num_instances: int) -> None: + """Initialize instance-dependent declaration state from ``SpawnScene``.""" + if num_instances <= 0: + raise ValueError("A declared RigidObjectGroup requires num_instances > 0.") + if self._declared_num_instances is not None: + if self._declared_num_instances != num_instances: + raise RuntimeError( + f"RigidObjectGroup {self.uid!r} is already declared for " + f"{self._declared_num_instances} instances." + ) + return - if device.type == "cuda": - self._world.update(0.001) + self._declared_num_instances = num_instances + self._all_indices = list(range(num_instances)) - super().__init__(cfg, entities, device) + def _require_declared_num_instances(self) -> int: + """Return the Spawn-provided instance count or raise a lifecycle error.""" + if self._declared_num_instances is None: + raise RuntimeError( + f"RigidObjectGroup {self.uid!r} must be registered through " + "SpawnScene before it can be used." + ) + return self._declared_num_instances - # set default collision filter - self._set_default_collision_filter() + @property + def is_declared(self) -> bool: + """Whether this facade is waiting for Spawn materialization.""" + return self._spawn_result is None - # reserve flag for collision visible node existence - n_instances = len(self._entities[0]) - self._has_collision_visible_node_list = [False] * n_instances + @property + def is_spawn_bound(self) -> bool: + """Whether this facade is bound to a finalized Scene.""" + return self._spawn_result is not None - def __str__(self) -> str: - parent_str = super().__str__() + @property + def num_instances(self) -> int: return ( - parent_str - + f" | body type: {self.body_type} | num_objects: {self.num_objects}" + len(self._entities) + if self._entities + else self._require_declared_num_instances() ) @property def num_objects(self) -> int: - """Get the number of objects in each rigid body instance. - - Returns: - int: The number of objects in each rigid body instance. - """ - return self._data.num_objects + return self._declared_num_objects @property def body_data(self) -> RigidBodyGroupData: - """Get the rigid body data manager for this rigid object. - - Returns: - RigidBodyGroupData: The rigid body data manager. - """ + if self._data is None: + raise RuntimeError( + f"RigidObjectGroup {self.uid!r} is not bound; call SimulationManager.prepare()." + ) return self._data - @property - def body_state(self) -> torch.Tensor: - """Get the body state of the rigid object. - - The body state of a rigid object is represented as a tensor with the following format: - [x, y, z, qw, qx, qy, qz, lin_x, lin_y, lin_z, ang_x, ang_y, ang_z] + def _capture_default_physical_properties(self) -> None: + """Capture materialized Group mass properties as reset defaults.""" + data = self.body_data + if data.default_physical_properties_initialized: + return + data.capture_default_physical_properties( + mass=data.mass, + inertia=data.inertia, + com_pose=data.com_pose, + ) - If the rigid object is static, linear and angular velocities will be zero. + def _restore_default_physical_properties( + self, env_ids: Sequence[int] | torch.Tensor | None + ) -> None: + """Restore initialization-time Group mass properties for selected rows.""" + data = self.body_data + if self.is_non_dynamic or not data.default_physical_properties_initialized: + return + env, objects, _ = self._selected_indices(env_ids) + if not env: + return + env_index = torch.as_tensor(env, dtype=torch.long, device=self.device) + obj_index = torch.as_tensor(objects, dtype=torch.long, device=self.device) + self.set_mass( + data.default_mass[env_index[:, None], obj_index[None, :]], + env_ids=env, + obj_ids=objects, + ) + self.set_inertia( + data.default_inertia[env_index[:, None], obj_index[None, :]], + env_ids=env, + obj_ids=objects, + ) + self.set_com_pose( + data.default_com_pose[env_index[:, None], obj_index[None, :]], + env_ids=env, + obj_ids=objects, + ) - Returns: - torch.Tensor: The body state of the rigid object with shape (num_instances, num_objects, 13), - where N is the number of instances. - """ + @property + def body_state(self) -> torch.Tensor: + """Pose and velocity with shape ``[env, object, 13]``.""" return torch.cat( (self.body_data.pose, self.body_data.lin_vel, self.body_data.ang_vel), dim=-1, @@ -266,143 +311,255 @@ def body_state(self) -> torch.Tensor: @property def is_non_dynamic(self) -> bool: - """Check if the rigid object is non-dynamic (static or kinematic). + return self.body_type in ("static", "kinematic") - Returns: - bool: True if the rigid object is non-dynamic, False otherwise. + def attach_spawn_handles(self, entities: Sequence[SpawnedRigidBody]) -> None: + """Store env-major handles without initializing the group's Batch data. + + ``bind_spawn()`` creates the result-dependent runtime view after Spawn + finalization. """ - return self.body_type in ("static", "kinematic") + expected = self._require_declared_num_instances() * self.num_objects + if len(entities) != expected: + raise ValueError( + f"RigidObjectGroup {self.uid!r} expected {expected} Spawn handles, " + f"got {len(entities)}." + ) + self._entities = [ + list(entities[start : start + self.num_objects]) + for start in range(0, len(entities), self.num_objects) + ] + + def _initialize_spawn_bound(self, result: Scene) -> None: + """Create result-dependent runtime state on this declared facade.""" + if not isinstance(result, Scene): + raise TypeError( + "RigidObjectGroup binding requires a finalized DexSim Scene; use " + "SimulationManager.prepare()." + ) - def _set_default_collision_filter(self) -> None: - collision_filter_data = torch.zeros( - size=(self.num_instances, 4), dtype=torch.int32 - ) - for i in range(self.num_instances): - collision_filter_data[i, 0] = i - collision_filter_data[i, 1] = 1 - self.set_collision_filter(collision_filter_data) + rows = [list(row) for row in self._entities] + expected = self._require_declared_num_instances() + if len(rows) != expected or any(len(row) != self.num_objects for row in rows): + raise ValueError( + f"RigidObjectGroup {self.uid!r} expected " + f"{expected}x{self.num_objects} Spawn handles." + ) - def set_collision_filter( - self, filter_data: torch.Tensor, env_ids: Sequence[int] | None = None - ) -> None: - """set collision filter data for the rigid object group. + cfg = deepcopy(self.cfg) + self._spawn_result = result + self.body_type = cfg.body_type + self._all_indices = list(range(len(rows))) + self._all_obj_indices = list(range(self._declared_num_objects)) + flat_entities = [entity for row in rows for entity in row] + body_view = SceneRigidBodyView.from_entities(result, flat_entities, self.device) + self._data = RigidBodyGroupData( + body_view, + num_instances=len(rows), + num_objects=self._declared_num_objects, + device=self.device, + ) - Args: - filter_data (torch.Tensor): [N, 4] of int. - First element of each object is arena id. - If 2nd element is 0, the object will collision with all other objects in world. - 3rd and 4th elements are not used currently. + super().__init__(cfg, rows, self.device) + self._capture_default_physical_properties() + self.reset() + + def bind_spawn(self, result: Scene) -> None: + """Atomically bind the declaration facade to env-major Spawn handles.""" + if self.is_spawn_bound: + raise RuntimeError(f"RigidObjectGroup {self.uid!r} is already Spawn-bound.") + if not self.is_declared: + raise RuntimeError( + f"RigidObjectGroup {self.uid!r} was not created as a Spawn declaration." + ) - env_ids (Sequence[int] | None, optional): Environment indices. If None, then all indices are used. Defaults to None. - """ - local_env_ids = self._all_indices if env_ids is None else env_ids + declared_state = self.__dict__.copy() + try: + self._initialize_spawn_bound(result) + except Exception: + self.__dict__.clear() + self.__dict__.update(declared_state) + raise - if len(local_env_ids) != len(filter_data): - logger.log_error( - f"Length of env_ids {len(local_env_ids)} does not match pose length {len(filter_data)}." + def __str__(self) -> str: + if self.is_declared: + return ( + f"{self.__class__}: declared {self.num_instances}x{self.num_objects} " + f"Spawn objects | uid: {self.uid} | device: {self.device}" ) + return ( + super().__str__() + + f" | body type: {self.body_type} | num_objects: {self.num_objects}" + ) - filter_data_np = filter_data.cpu().numpy().astype(np.uint32) - for i, env_idx in enumerate(local_env_ids): - for entity in self._entities[env_idx]: - entity.get_physical_body().set_collision_filter_data(filter_data_np[i]) + def _selected_indices( + self, + env_ids: Sequence[int] | torch.Tensor | None, + obj_ids: Sequence[int] | torch.Tensor | None = None, + ) -> tuple[list[int], list[int], torch.Tensor]: + env = ( + self._all_indices + if env_ids is None + else torch.as_tensor(env_ids).reshape(-1).cpu().tolist() + ) + objects = ( + self._all_obj_indices + if obj_ids is None + else torch.as_tensor(obj_ids).reshape(-1).cpu().tolist() + ) + if any(index < 0 or index >= self.num_instances for index in env): + raise IndexError("RigidObjectGroup environment index is out of range.") + if any(index < 0 or index >= self.num_objects for index in objects): + raise IndexError("RigidObjectGroup object index is out of range.") + rows = torch.as_tensor( + [ + env_id * self.num_objects + obj_id + for env_id in env + for obj_id in objects + ], + dtype=torch.long, + device=self.device, + ) + return env, objects, rows - def set_local_pose( + def get_mass( self, - pose: torch.Tensor, - env_ids: Sequence[int] | None = None, - obj_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | None = None, + obj_ids: Sequence[int] | torch.Tensor | None = None, + ) -> torch.Tensor: + """Return selected masses with shape ``[env, object]``.""" + env, objects, _ = self._selected_indices(env_ids, obj_ids) + env_index = torch.as_tensor(env, dtype=torch.long, device=self.device) + obj_index = torch.as_tensor(objects, dtype=torch.long, device=self.device) + return self.body_data.mass[env_index[:, None], obj_index[None, :]] + + def set_mass( + self, + mass: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor | None = None, + obj_ids: Sequence[int] | torch.Tensor | None = None, ) -> None: - """Set local pose of the rigid object group. + """Set selected masses from a tensor shaped ``[env, object]``.""" + env, objects, rows = self._selected_indices(env_ids, obj_ids) + mass = torch.as_tensor(mass, dtype=torch.float32, device=self.device) + expected_shape = (len(env), len(objects)) + if tuple(mass.shape) != expected_shape: + raise ValueError( + f"Expected mass shape {expected_shape}, got {tuple(mass.shape)}." + ) + self.body_data.body_view.apply_mass(mass.reshape(-1, 1), rows) - Args: - pose (torch.Tensor): The local pose of the rigid object group with shape (num_instances, num_objects, 7) or - (num_instances, num_objects, 4, 4). - env_ids (Sequence[int] | None, optional): Environment indices. If None, then all indices are used. - obj_ids (Sequence[int] | None, optional): Object indices within the group. If None, all objects are set. Defaults to None. - """ - local_env_ids = self._all_indices if env_ids is None else env_ids - local_obj_ids = self._all_obj_indices if obj_ids is None else obj_ids + def get_inertia( + self, + env_ids: Sequence[int] | torch.Tensor | None = None, + obj_ids: Sequence[int] | torch.Tensor | None = None, + ) -> torch.Tensor: + """Return selected inertia diagonals with shape ``[env, object, 3]``.""" + env, objects, _ = self._selected_indices(env_ids, obj_ids) + env_index = torch.as_tensor(env, dtype=torch.long, device=self.device) + obj_index = torch.as_tensor(objects, dtype=torch.long, device=self.device) + return self.body_data.inertia[env_index[:, None], obj_index[None, :]] - if len(local_env_ids) != len(pose): - logger.log_error( - f"Length of env_ids {len(local_env_ids)} does not match pose length {len(pose)}." + def set_inertia( + self, + inertia: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor | None = None, + obj_ids: Sequence[int] | torch.Tensor | None = None, + ) -> None: + """Set selected inertia diagonals.""" + env, objects, rows = self._selected_indices(env_ids, obj_ids) + inertia = torch.as_tensor(inertia, dtype=torch.float32, device=self.device) + expected_shape = (len(env), len(objects), 3) + if tuple(inertia.shape) != expected_shape: + raise ValueError( + f"Expected inertia shape {expected_shape}, " + f"got {tuple(inertia.shape)}." ) + self.body_data.body_view.apply_inertia_diagonal(inertia.reshape(-1, 3), rows) - if self.device.type == "cpu": - pose = pose.cpu() - if pose.dim() == 3 and pose.shape[2] == 7: - reshape_pose = pose.reshape(-1, 7) - pose_matrix = ( - torch.eye(4).unsqueeze(0).repeat(reshape_pose.shape[0], 1, 1) - ) - pose_matrix[:, :3, 3] = reshape_pose[:, :3] - pose_matrix[:, :3, :3] = matrix_from_quat(reshape_pose[:, 3:7]) - pose = pose_matrix.reshape(-1, len(local_obj_ids), 4, 4) - elif pose.dim() == 4 and pose.shape[2:] == (4, 4): - pass - else: - logger.log_error( - f"Invalid pose shape {pose.shape}. Expected (num_instances, num_objects, 7) or (num_instances, num_objects, 4, 4)." - ) + def get_com_pose( + self, + env_ids: Sequence[int] | torch.Tensor | None = None, + obj_ids: Sequence[int] | torch.Tensor | None = None, + ) -> torch.Tensor: + """Return selected local COM poses in Group ``xyz + xyzw`` order.""" + env, objects, _ = self._selected_indices(env_ids, obj_ids) + env_index = torch.as_tensor(env, dtype=torch.long, device=self.device) + obj_index = torch.as_tensor(objects, dtype=torch.long, device=self.device) + return self.body_data.com_pose[env_index[:, None], obj_index[None, :]] - for i, env_idx in enumerate(local_env_ids): - for j, obj_idx in enumerate(local_obj_ids): - self._entities[env_idx][obj_idx].set_local_pose(pose[i, j]) + def set_com_pose( + self, + com_pose: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor | None = None, + obj_ids: Sequence[int] | torch.Tensor | None = None, + ) -> None: + """Set selected local COM poses in Group ``xyz + xyzw`` order.""" + env, objects, rows = self._selected_indices(env_ids, obj_ids) + com_pose = torch.as_tensor(com_pose, dtype=torch.float32, device=self.device) + expected_shape = (len(env), len(objects), 7) + if tuple(com_pose.shape) != expected_shape: + raise ValueError( + f"Expected COM pose shape {expected_shape}, " + f"got {tuple(com_pose.shape)}." + ) + self.body_data.body_view.apply_com_local_pose(com_pose.reshape(-1, 7), rows) - else: - if pose.dim() == 3 and pose.shape[2] == 7: - xyz = pose[..., :3].reshape(-1, 3) - quat = pose[..., 3:7].reshape(-1, 4) - quat = convert_quat(quat, to="xyzw") - elif pose.dim() == 4 and pose.shape[2:] == (4, 4): - xyz = pose[..., :3, 3].reshape(-1, 3) - mat = pose[..., :3, :3].reshape(-1, 3, 3) - quat = quat_from_matrix(mat) - quat = convert_quat(quat, to="xyzw") - else: - logger.log_error( - f"Invalid pose shape {pose.shape}. Expected (N, 7) or (N, 4, 4)." - ) + def set_collision_filter( + self, + filter_data: torch.Tensor, + env_ids: Sequence[int] | None = None, + ) -> None: + """Set one collision filter value for every selected member in each env.""" + env, objects, rows = self._selected_indices(env_ids) + values = filter_data.to(device=self.device, dtype=torch.int32).reshape(-1, 4) + if len(values) != len(env): + raise ValueError( + f"Expected {len(env)} collision filters, got {len(values)}." + ) + expanded = values[:, None, :].expand(-1, len(objects), -1).reshape(-1, 4) + self.body_data.body_view.apply_collision_filter(expanded, rows) - # we should keep `pose_` life cycle to the end of the function. - pose = torch.cat((quat, xyz), dim=-1) - indices = self.body_data.gpu_indices[local_env_ids][ - :, local_obj_ids - ].flatten() - torch.cuda.synchronize(self.device) - self._ps.gpu_apply_rigid_body_data( - data=pose.clone(), - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.POSE, + def set_local_pose( + self, + pose: torch.Tensor, + env_ids: Sequence[int] | None = None, + obj_ids: Sequence[int] | None = None, + ) -> None: + """Set Group poses in ``xyz+xyzw`` or homogeneous-matrix form.""" + env, objects, rows = self._selected_indices(env_ids, obj_ids) + expected_prefix = (len(env), len(objects)) + pose = pose.to(device=self.device, dtype=torch.float32) + if tuple(pose.shape) == (*expected_prefix, 7): + target = pose.reshape(-1, 7) + elif tuple(pose.shape) == (*expected_prefix, 4, 4): + flat = pose.reshape(-1, 4, 4) + target = torch.cat( + ( + flat[:, :3, 3], + quat_from_matrix(flat[:, :3, :3]), + ), + dim=-1, ) - self._world.sync_poses_gpu_to_cpu( - rigid_pose=CudaArray(pose), rigid_gpu_indices=CudaArray(indices) + else: + raise ValueError( + f"Expected pose shape {(*expected_prefix, 7)} or " + f"{(*expected_prefix, 4, 4)}, got {tuple(pose.shape)}." ) + self.body_data.body_view.apply_pose(target, rows) def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: - """Get local pose of the rigid object group. - - Args: - to_matrix (bool, optional): If True, return the pose as a 4x4 matrix. If False, return as (x, y, z, qw, qx, qy, qz). Defaults to False. - - Returns: - torch.Tensor: The local pose of the rigid object with shape (num_instances, num_objects, 7) or (num_instances, num_objects, 4, 4) depending on `to_matrix`. - """ + """Return all Group poses as ``xyz+xyzw`` or homogeneous matrices.""" pose = self.body_data.pose - if to_matrix: - pose = pose.reshape(-1, 7) - xyz = pose[:, :3] - mat = matrix_from_quat(pose[:, 3:7]) - pose = ( - torch.eye(4, dtype=torch.float32, device=self.device) - .unsqueeze(0) - .repeat(self.num_instances * self.num_objects, 1, 1) - ) - pose[:, :3, 3] = xyz - pose[:, :3, :3] = mat - pose = pose.reshape(self.num_instances, self.num_objects, 4, 4) - return pose + if not to_matrix: + return pose + flat = pose.reshape(-1, 7) + result = torch.eye(4, dtype=torch.float32, device=self.device).repeat( + len(flat), 1, 1 + ) + result[:, :3, 3] = flat[:, :3] + result[:, :3, :3] = matrix_from_quat(flat[:, 3:7]) + return result.reshape(self.num_instances, self.num_objects, 4, 4) def get_object_vertices( self, @@ -410,34 +567,19 @@ def get_object_vertices( env_ids: Sequence[int] | None = None, scale: bool = False, ) -> torch.Tensor: - """Get one constituent object's vertices across selected environments. - - Args: - object_id: Constituent object index within the group. - env_ids: Environment indices. If ``None``, returns all instances. - scale: Whether to apply each object's body scale. - - Returns: - Vertices with shape ``(N, num_vertices, 3)``. - """ - if not 0 <= object_id < self.num_objects: - raise IndexError( - f"object_id {object_id} is outside [0, {self.num_objects - 1}]." - ) - ids = self._all_indices if env_ids is None else env_ids + """Return one member's render vertices across selected environments.""" + env, objects, _ = self._selected_indices(env_ids, [object_id]) + object_id = objects[0] vertices = np.asarray( - [ - get_combined_vertices(self._entities[env_id][object_id]) - for env_id in ids - ], + [get_combined_vertices(self._entities[index][object_id]) for index in env], dtype=np.float32, ) if scale: scales = np.asarray( - [self._entities[env_id][object_id].get_body_scale() for env_id in ids], + [self._entities[index][object_id].get_body_scale() for index in env], dtype=np.float32, ) - vertices = vertices * scales[:, None, :] + vertices *= scales[:, None, :] return torch.as_tensor(vertices, dtype=torch.float32, device=self.device) def get_object_triangles( @@ -445,35 +587,17 @@ def get_object_triangles( object_id: int, env_ids: Sequence[int] | None = None, ) -> torch.Tensor: - """Get one constituent object's triangle indices. - - Args: - object_id: Constituent object index within the group. - env_ids: Environment indices. If ``None``, returns all instances. - - Returns: - Triangle indices with shape ``(N, num_triangles, 3)``. - """ - if not 0 <= object_id < self.num_objects: - raise IndexError( - f"object_id {object_id} is outside [0, {self.num_objects - 1}]." - ) - ids = self._all_indices if env_ids is None else env_ids + """Return one member's render triangles across selected environments.""" + env, objects, _ = self._selected_indices(env_ids, [object_id]) + object_id = objects[0] triangles = np.asarray( - [ - get_combined_triangles(self._entities[env_id][object_id]) - for env_id in ids - ], + [get_combined_triangles(self._entities[index][object_id]) for index in env], dtype=np.int32, ) return torch.as_tensor(triangles, dtype=torch.int32, device=self.device) def get_user_ids(self) -> torch.Tensor: - """Get the user ids of the rigid body group. - - Returns: - torch.Tensor: A tensor of shape (num_envs, num_objects) representing the user ids of the rigid body group. - """ + """Return render user ids with shape ``[env, object]``.""" return torch.as_tensor( [ [entity.get_user_id() for entity in instance] @@ -484,164 +608,79 @@ def get_user_ids(self) -> torch.Tensor: ) def clear_dynamics(self, env_ids: Sequence[int] | None = None) -> None: - """Clear the dynamics of the rigid bodies by resetting velocities and applying zero forces and torques. - - Args: - env_ids (Sequence[int] | None): Environment indices. If None, then all indices are used. - """ + """Clear velocity and one-step wrench buffers for selected envs.""" if self.is_non_dynamic: return - - local_env_ids = self._all_indices if env_ids is None else env_ids - - if self.device.type == "cpu": - for env_idx in local_env_ids: - for entity in self._entities[env_idx]: - entity.clear_dynamics() - else: - # Apply zero force and torque to the rigid bodies. - zeros = torch.zeros( - (len(local_env_ids) * self.num_objects, 3), - dtype=torch.float32, - device=self.device, - ) - indices = self.body_data.gpu_indices[local_env_ids].flatten() - torch.cuda.synchronize(self.device) - self._ps.gpu_apply_rigid_body_data( - data=zeros, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.LINEAR_VELOCITY, - ) - self._ps.gpu_apply_rigid_body_data( - data=zeros, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.ANGULAR_VELOCITY, - ) - self._ps.gpu_apply_rigid_body_data( - data=zeros, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.FORCE, - ) - self._ps.gpu_apply_rigid_body_data( - data=zeros, - gpu_indices=indices, - data_type=RigidBodyGPUAPIWriteType.TORQUE, - ) + _, _, rows = self._selected_indices(env_ids) + zeros = torch.zeros((len(rows), 3), dtype=torch.float32, device=self.device) + view = self.body_data.body_view + view.apply_linear_velocity(zeros, rows) + view.apply_angular_velocity(zeros, rows) + view.apply_force(zeros, rows) + view.apply_torque(zeros, rows) def set_visual_material( - self, mat: VisualMaterial, env_ids: Sequence[int] | None = None + self, + mat: VisualMaterial, + env_ids: Sequence[int] | None = None, ) -> None: - """Set visual material for the rigid object group. - - Note: - For each entity in the rigid object group, a unique material instance will be created and shared - among all objects in that entity. - - Args: - mat (VisualMaterial): The material to set. - env_ids (Sequence[int] | None, optional): Environment indices. If None, then all indices are used. - """ - local_env_ids = self._all_indices if env_ids is None else env_ids - - for i, env_idx in enumerate(local_env_ids): - mat_inst = mat.create_instance(f"{mat.uid}_{self.uid}_{env_idx}") - for j, entity in enumerate(self._entities[env_idx]): - entity.set_material(mat_inst.mat) - - # Note: The rigid object group is not supported to change the visual material once created. - # If needed, we should create a visual material dict to store the material instances, and - # implement a get_visual_material method to retrieve the material instances. + """Assign one material instance to all members in each selected env.""" + env, _, _ = self._selected_indices(env_ids) + for env_id in env: + material = mat.create_instance(f"{mat.uid}_{self.uid}_{env_id}") + for entity in self._entities[env_id]: + entity.set_material(material.mat) def reset(self, env_ids: Sequence[int] | None = None) -> None: - local_env_ids = self._all_indices if env_ids is None else env_ids - num_instances = len(local_env_ids) - - self.cfg: RigidObjectGroupCfg - body_cfgs = list(self.cfg.rigid_objects.values()) - - init_pos = [] - init_rot = [] - for cfg in body_cfgs: - init_pos.append(cfg.init_pos) - init_rot.append(cfg.init_rot) - - # (num_objects, 3) - pos = torch.as_tensor(init_pos, dtype=torch.float32, device=self.device) - rot = ( - torch.as_tensor(init_rot, dtype=torch.float32, device=self.device) - * torch.pi - / 180.0 - ) - # Convert pos and rot to shape (num_instances, num_objects, dim) - pos = pos.unsqueeze_(0).repeat(num_instances, 1, 1) - rot = rot.unsqueeze_(0).repeat(num_instances, 1, 1) - - mat = matrix_from_euler(rot.reshape(-1, 3), "XYZ") - # Init pose with shape (num_instances, num_objects, 4, 4) - pose = ( - torch.eye(4, dtype=torch.float32, device=self.device) - .unsqueeze_(0) - .repeat(num_instances * self.num_objects, 1, 1) - ) - pose[:, :3, 3] = pos.reshape(-1, 3) - pose[:, :3, :3] = mat - pose = pose.reshape(num_instances, self.num_objects, 4, 4) - self.set_local_pose(pose, env_ids=local_env_ids) - - self.clear_dynamics(env_ids=local_env_ids) + env, _, _ = self._selected_indices(env_ids) + self._restore_default_physical_properties(env) + member_poses = [] + for cfg in self.cfg.rigid_objects.values(): + if cfg.init_local_pose is not None: + member_poses.append( + torch.as_tensor( + cfg.init_local_pose, + dtype=torch.float32, + device=self.device, + ).reshape(4, 4) + ) + continue + pose = torch.eye(4, dtype=torch.float32, device=self.device) + pose[:3, 3] = torch.as_tensor( + cfg.init_pos, dtype=torch.float32, device=self.device + ) + rotation = torch.as_tensor( + cfg.init_rot, dtype=torch.float32, device=self.device + ) + pose[:3, :3] = matrix_from_euler( + (rotation * torch.pi / 180.0).reshape(1, 3), "XYZ" + )[0] + member_poses.append(pose) + pose = torch.stack(member_poses).repeat(len(env), 1, 1) + self.set_local_pose(pose.reshape(len(env), self.num_objects, 4, 4), env_ids=env) + self.clear_dynamics(env_ids=env) def set_physical_visible( self, visible: bool = True, rgba: Sequence[float] | None = None, - ): - """set collion render visibility - - Args: - visible (bool, optional): is collision body visible. Defaults to True. - rgba (Sequence[float] | None, optional): collision body visible rgba. It will be defined at the first time the function is called. Defaults to None. - """ - rgba = rgba if rgba is not None else (0.8, 0.2, 0.2, 0.7) - if len(rgba) != 4: - logger.log_error(f"Invalid rgba {rgba}, should be a sequence of 4 floats.") - - # create collision visible node if not exist - if visible: - for i, env_idx in enumerate(self._all_indices): - for intance_id, entity in enumerate(self._entities[env_idx]): - if not self._has_collision_visible_node_list[intance_id]: - entity.create_physical_visible_node( - np.array( - [ - rgba[0], - rgba[1], - rgba[2], - rgba[3], - ] - ) - ) - self._has_collision_visible_node_list[intance_id] = True - - # create collision visible node if not exist - for i, env_idx in enumerate(self._all_indices): - for entity in self._entities[env_idx]: - entity.set_physical_visible(visible) + ) -> None: + """Set collision-geometry visibility for every Group member.""" + color = np.asarray( + (0.8, 0.2, 0.2, 0.7) if rgba is None else rgba, + dtype=np.float32, + ) + if color.shape != (4,): + raise ValueError("Collision visualization color must contain four values.") + for instance in self._entities: + for entity in instance: + self._spawn_result.set_physical_visible(entity, color, visible) def set_visible(self, visible: bool = True) -> None: - """Set the visibility of the rigid object group. - - Args: - visible (bool, optional): Whether the rigid object group is visible. Defaults to True. - """ - for i, env_idx in enumerate(self._all_indices): - for entity in self._entities[env_idx]: + """Set render visibility for every Group member.""" + for instance in self._entities: + for entity in instance: entity.set_visible(visible) def destroy(self) -> None: - env = self._world.get_env() - arenas = env.get_all_arenas() - if len(arenas) == 0: - arenas = [env] - for i, instance in enumerate(self._entities): - for entity in instance: - arenas[i].remove_actor(entity) + """Leave topology destruction to SimulationManager and Scene.""" diff --git a/embodichain/lab/sim/objects/robot.py b/embodichain/lab/sim/objects/robot.py index 37aef4063..a38ed5eec 100644 --- a/embodichain/lab/sim/objects/robot.py +++ b/embodichain/lab/sim/objects/robot.py @@ -19,11 +19,10 @@ import torch import numpy as np -from typing import Dict, List, Literal, Sequence, Tuple +from typing import TYPE_CHECKING, Dict, List, Literal, Sequence, Tuple from dataclasses import dataclass, field from tensordict import TensorDict -from dexsim.engine import Articulation as _Articulation from embodichain.lab.sim.cfg import RobotCfg, RobotWorkspaceCfg from embodichain.lab.sim.motion.solvers import SolverCfg, BaseSolver from embodichain.lab.sim.objects import Articulation @@ -39,6 +38,9 @@ ) from embodichain.utils import logger +if TYPE_CHECKING: + from dexsim.scene import SpawnedArticulation + @dataclass class ControlGroup: @@ -71,12 +73,9 @@ class Robot(Articulation): def __init__( self, cfg: RobotCfg, - entities: List[_Articulation], device: torch.device = torch.device("cpu"), ) -> None: - - self._entities = entities - self.cfg = cfg + """Create an unregistered robot facade.""" # Initialize joint ids for control parts. self._joint_ids: Dict[str, List[int]] = {} @@ -91,13 +90,7 @@ def __init__( # cache I/O unless a task actually requests workspace sampling. self._workspaces: Dict[str, RobotWorkspace] = {} - if self.cfg.control_parts: - self._init_control_parts(self.cfg.control_parts) - - super().__init__(cfg, entities, device) - - if self.cfg.solver_cfg: - self.init_solver(self.cfg.solver_cfg) + super().__init__(cfg, device) def __str__(self) -> str: parent_str = super().__str__() @@ -106,6 +99,26 @@ def __str__(self) -> str: + f" | control_parts: {self.control_parts}, solvers: {self._solvers}" ) + def attach_spawn_handles( + self, + entities: Sequence[SpawnedArticulation], + ) -> None: + """Store handles and expose robot metadata without creating Batch data. + + Runtime Batch/Data initialization remains the responsibility of + ``bind_spawn()`` after Spawn finalization. + """ + super().attach_spawn_handles(entities) + if self.cfg.control_parts: + self._init_control_parts(self.cfg.control_parts) + + def _initialize_spawn_bound_extension(self) -> None: + """Initialize robot-specific runtime state after Scene binding.""" + if self.cfg.control_parts: + self._init_control_parts(self.cfg.control_parts) + if self.cfg.solver_cfg: + self.init_solver(self.cfg.solver_cfg) + @property def control_parts(self) -> Dict[str, List[str]] | None: """Get the control parts of the robot.""" @@ -788,7 +801,9 @@ def compute_fk( of ``qpos`` for full-articulation FK. Returns: - torch.Tensor: The forward kinematics result with shape (num_envs, 7) or (num_envs, 4, 4) if `to_matrix` is True. + torch.Tensor: The forward-kinematics result with shape + ``(num_envs, 7)`` in ``(x, y, z, qx, qy, qz, qw)`` order, or + ``(num_envs, 4, 4)`` if ``to_matrix`` is True. """ local_env_ids = self._all_indices if env_ids is None else env_ids @@ -858,7 +873,8 @@ def compute_ik( The input pose should be in the local arena frame. Args: - pose (torch.Tensor): The end effector pose of the robot, (num_envs, 7) or (num_envs, 4, 4). + pose (torch.Tensor): The end-effector pose as ``(num_envs, 7)`` in + ``(x, y, z, qx, qy, qz, qw)`` order or ``(num_envs, 4, 4)``. joint_seed (torch.Tensor | None): The joint positions to use as a seed for the IK computation, (num_envs, dof). If None, the zero joint positions will be used as the seed. name (str | None): The name of the control part to compute the IK for. If None, the default part is used. @@ -942,7 +958,9 @@ def compute_batch_fk( to_matrix (bool): If True, returns the transformation in the form of a 4x4 matrix. Returns: - torch.Tensor: The forward kinematics result with shape (num_envs, batch, 7) or (num_envs, batch, 4, 4) if `to_matrix` is True. + torch.Tensor: The forward-kinematics result with shape + ``(num_envs, batch, 7)`` in ``xyz + xyzw`` order, or + ``(num_envs, batch, 4, 4)`` if ``to_matrix`` is True. """ local_env_ids = self._all_indices if env_ids is None else env_ids if not self._solvers: @@ -1002,7 +1020,8 @@ def compute_batch_ik( The input pose should be in the local arena frame. Args: - pose (torch.Tensor): The end effector pose of the robot, (num_envs, n_batch, 7) or (num_envs, n_batch, 4, 4). + pose (torch.Tensor): End-effector poses as ``(num_envs, n_batch, 7)`` + in ``xyz + xyzw`` order or ``(num_envs, n_batch, 4, 4)``. joint_seed (torch.Tensor | None): The joint positions to use as a seed for the IK computation, (num_envs, n_batch, dof). If None, the zero joint positions will be used as the seed. name (str | None): The name of the control part to compute the IK for. If None, the default part is used. env_ids (Sequence[int] | None): Environment indices to apply the positions. Defaults to all environments. @@ -1098,8 +1117,7 @@ def _init_control_parts(self, control_parts: Dict[str, List[str]]) -> None: joint names or regular expressions that match joint names. """ joint_name_to_ids = { - name: i - for i, name in enumerate(self._entities[0].get_actived_joint_names()) + name: i for i, name in enumerate(self._state_joint_names()) } for name, joint_names in control_parts.items(): # convert joint_names which is a regular expression to a list of joint names @@ -1135,12 +1153,16 @@ def set_joint_drive( max_velocity: torch.Tensor | None = None, friction: torch.Tensor | None = None, armature: torch.Tensor | None = None, - drive_type: str = "force", + drive_type: str | None = "force", joint_ids: Sequence[int] | None = None, env_ids: Sequence[int] | None = None, + *, + target_mode: str | int | None = None, ) -> None: """Set the drive properties for the robot. - Different from Articulation, default drive type is 'force' instead of 'none' + + With no explicit mode, robots retain their position+velocity force + drive default. Args: stiffness (torch.Tensor): The stiffness of the joint drive with shape (len(env_ids), len(joint_ids)). @@ -1149,9 +1171,10 @@ def set_joint_drive( max_velocity (torch.Tensor): The maximum velocity of the joint drive with shape (len(env_ids), len(joint_ids)). friction (torch.Tensor): The joint friction coefficient with shape (len(env_ids), len(joint_ids)). armature (torch.Tensor): The joint armature with shape (len(env_ids), len(joint_ids)). - drive_type (str, optional): The type of drive to apply. Defaults to "force". + drive_type: Drive type to apply. Defaults to ``"force"``. joint_ids (Sequence[int] | None, optional): The joint indices to apply the drive to. If None, applies to all joints. Defaults to None. env_ids (Sequence[int] | None, optional): The environment indices to apply the drive to. If None, applies to all environments. Defaults to None. + target_mode: Portable target mode name or integer value 0 through 4. """ super().set_joint_drive( stiffness=stiffness, @@ -1163,6 +1186,7 @@ def set_joint_drive( drive_type=drive_type, joint_ids=joint_ids, env_ids=env_ids, + target_mode=target_mode, ) def _set_default_joint_drive(self) -> None: @@ -1170,7 +1194,7 @@ def _set_default_joint_drive(self) -> None: import numbers from embodichain.utils.string import resolve_matching_names_values - drive_props = [ + joint_property_targets = [ ("damping", self.default_joint_damping), ("stiffness", self.default_joint_stiffness), ("max_effort", self.default_joint_max_effort), @@ -1179,8 +1203,8 @@ def _set_default_joint_drive(self) -> None: ("armature", self.default_joint_armature), ] - for prop_name, default_array in drive_props: - value = getattr(self.cfg.drive_pros, prop_name, None) + for prop_name, default_array in joint_property_targets: + value = getattr(self.cfg.joint_drive_props, prop_name, None) if value is None: continue if isinstance(value, numbers.Number): @@ -1220,11 +1244,19 @@ def _set_default_joint_drive(self) -> None: except Exception as e: logger.log_error(f"Failed to set {prop_name}: {e}") - drive_pros = self.cfg.drive_pros - if isinstance(drive_pros, dict): - drive_type = drive_pros.get("drive_type", "force") + joint_drive_props = self.cfg.joint_drive_props + if isinstance(joint_drive_props, dict): + drive_type = joint_drive_props.get("drive_type") + target_mode = joint_drive_props.get("target_mode") else: - drive_type = getattr(drive_pros, "drive_type", "force") + drive_type = getattr(joint_drive_props, "drive_type", None) + target_mode = getattr(joint_drive_props, "target_mode", None) + if isinstance(target_mode, dict): + logger.log_warning( + "Per-joint target_mode mappings require a Spawn-bound robot; " + "the retained raw-robot path preserves its current target modes." + ) + target_mode = None # Apply drive parameters to all articulations in the batch self.set_joint_drive( @@ -1235,6 +1267,7 @@ def _set_default_joint_drive(self) -> None: friction=self.default_joint_friction, armature=self.default_joint_armature, drive_type=drive_type, + target_mode=target_mode, ) def _sync_solver_limits(self, name: str | None = None) -> None: @@ -1395,21 +1428,29 @@ def _extract_control_group(self, joint_names: List[str]) -> ControlGroup: """ control_group = ControlGroup() joint_id_list = [] + state_joint_ids = { + name: index for index, name in enumerate(self._state_joint_names()) + } + source_joint_ids = { + name: index + for index, name in enumerate(self._entities[0].get_actived_joint_names()) + } for joint_name in joint_names: - if joint_name in self.joint_names: - joint_index = self.joint_names.index(joint_name) - joint_id_list.append(joint_index) + if joint_name in state_joint_ids and joint_name in source_joint_ids: + joint_id_list.append(state_joint_ids[joint_name]) control_group.joint_names.append(joint_name) # Set root link for first joint if len(control_group.link_names) == 0: parent_names = self._entities[0].get_ancestral_link_names( - joint_index + source_joint_ids[joint_name] ) control_group.link_names.extend(parent_names) - child_name = self._entities[0].get_child_link_name(joint_index) + child_name = self._entities[0].get_child_link_name( + source_joint_ids[joint_name] + ) control_group.link_names.append(child_name) control_group.joint_ids = joint_id_list @@ -1449,6 +1490,17 @@ def set_physical_visible( ) link_names = self.get_control_part_link_names(name=control_part) + if self.is_spawn_bound: + for env_idx in self._all_indices: + entity = self._entities[env_idx] + for link_name in link_names: + self._spawn_result.set_physical_visible( + (entity, link_name), rgba, visible + ) + for link_name in link_names: + self._has_collision_visible_node_dict[link_name] = True + return + # create collision visible node if not exist if visible: for i, env_idx in enumerate(self._all_indices): diff --git a/embodichain/lab/sim/objects/soft_object.py b/embodichain/lab/sim/objects/soft_object.py deleted file mode 100644 index 52344150c..000000000 --- a/embodichain/lab/sim/objects/soft_object.py +++ /dev/null @@ -1,535 +0,0 @@ -# ---------------------------------------------------------------------------- -# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# ---------------------------------------------------------------------------- - -from __future__ import annotations - -import torch -import dexsim -import numpy as np -from functools import cached_property - -from dataclasses import dataclass -from typing import List, Sequence, Union - -from dexsim.models import MeshObject -from dexsim.engine import PhysicsScene, SoftBody -from dexsim.types import SoftBodyGPUAPIReadWriteType -from scipy.spatial import ConvexHull, QhullError -from embodichain.lab.sim.common import ( - BatchEntity, -) -from embodichain.lab.sim.material import ( - VisualMaterial, - VisualMaterialInst, - _capture_render_materials, - _restore_render_materials, - _wrap_first_render_material, -) -from embodichain.utils.math import ( - matrix_from_euler, -) -from embodichain.utils import logger -from embodichain.lab.sim.cfg import ( - SoftObjectCfg, -) -from embodichain.utils.math import xyz_quat_to_4x4_matrix - -__all__ = ["SoftBodyData", "SoftObject", "SoftObjectCfg"] - - -@dataclass -class SoftBodyData: - """Data manager for soft body - - Note: - 1. The pose data managed by dexsim is in the format of (qx, qy, qz, qw, x, y, z), but in EmbodiChain, we use (x, y, z, qw, qx, qy, qz) format. - """ - - def __init__( - self, entities: List[MeshObject], ps: PhysicsScene, device: torch.device - ) -> None: - """Initialize the SoftBodyData. - - Args: - entities (List[MeshObject]): List of MeshObjects representing the soft bodies. - ps (PhysicsScene): The physics scene. - device (torch.device): The device to use for the soft body data. - """ - self.entities = entities - # TODO: soft body data can only be stored in cuda device for now. - self.device = device - # TODO: inorder to retrieve arena position, we need to access the node of each entity. - self.ps = ps - self.num_instances = len(entities) - - self.soft_bodies: Sequence[SoftBody] = [ - self.entities[i].get_physical_body() for i in range(self.num_instances) - ] - self.n_collision_vertices = self.soft_bodies[0].get_num_vertices() - self.n_sim_vertices = self.soft_bodies[0].get_num_sim_vertices() - - self._rest_position_buffer = torch.empty( - (self.num_instances, self.n_collision_vertices, 4), - device=self.device, - dtype=torch.float32, - ) - for i, softbody in enumerate(self.soft_bodies): - self._rest_position_buffer[i] = softbody.get_position_inv_mass_buffer() - - self._rest_sim_position_buffer = torch.empty( - (self.num_instances, self.n_sim_vertices, 4), - device=self.device, - dtype=torch.float32, - ) - - for i, softbody in enumerate(self.soft_bodies): - self._rest_sim_position_buffer[i] = ( - softbody.get_sim_position_inv_mass_buffer() - ) - - self._collision_position = torch.zeros( - (self.num_instances, self.n_collision_vertices, 3), - device=self.device, - dtype=torch.float32, - ) - self._sim_vertex_velocity = torch.zeros( - (self.num_instances, self.n_sim_vertices, 3), - device=self.device, - dtype=torch.float32, - ) - self._sim_vertex_position = torch.zeros( - (self.num_instances, self.n_sim_vertices, 3), - device=self.device, - dtype=torch.float32, - ) - - @property - def rest_collision_vertices(self): - """Get the rest position buffer of the soft bodies.""" - return self._rest_position_buffer[:, :, :3].clone() - - @property - def rest_sim_vertices(self): - """Get the rest sim position buffer of the soft bodies.""" - return self._rest_sim_position_buffer[:, :, :3].clone() - - @property - def collision_position(self): - """Get the current vertex position buffer of the soft bodies.""" - for i, softbody in enumerate(self.soft_bodies): - self._collision_position[i] = softbody.get_position_inv_mass_buffer()[:, :3] - return self._collision_position.clone() - - @property - def sim_vertex_position(self): - """Get the current sim vertex position buffer of the soft bodies.""" - for i, softbody in enumerate(self.soft_bodies): - self._sim_vertex_position[i] = softbody.get_sim_position_inv_mass_buffer()[ - :, :3 - ] - return self._sim_vertex_position.clone() - - @property - def sim_vertex_velocity(self): - """Get the current vertex velocity buffer of the soft bodies.""" - for i, softbody in enumerate(self.soft_bodies): - self._sim_vertex_velocity[i] = softbody.get_sim_velocity_buffer()[:, :3] - return self._sim_vertex_velocity.clone() - - @cached_property - def collision_surface_triangles(self) -> torch.Tensor: - """Build a stable surface approximation for collision vertices. - - DexSim exposes live PhysX collision vertices but not their triangle - connectivity. The convex hull provides a stable topology whose indices - continue to reference the live collision-vertex buffer. - - Returns: - Cached convex-hull triangle indices. - """ - vertices = self.rest_collision_vertices[0].detach().cpu().numpy() - if vertices.shape[0] < 4: - logger.log_warning( - "Soft-body collision geometry has fewer than four vertices; " - "its visualization surface will be empty." - ) - triangles = np.empty((0, 3), dtype=np.int32) - else: - try: - triangles = np.asarray( - ConvexHull(vertices).simplices, - dtype=np.int32, - ) - except QhullError as error: - try: - triangles = np.asarray( - ConvexHull(vertices, qhull_options="QJ").simplices, - dtype=np.int32, - ) - except QhullError: - logger.log_warning( - "Unable to build a soft-body visualization surface from " - f"collision vertices: {error!r}" - ) - triangles = np.empty((0, 3), dtype=np.int32) - return torch.as_tensor( - triangles, - dtype=torch.int32, - device=self.device, - ) - - -class SoftObject(BatchEntity): - """SoftObject represents a batch of soft body in the simulation.""" - - def __init__( - self, - cfg: SoftObjectCfg, - entities: List[MeshObject] = None, - device: torch.device = torch.device("cpu"), - ) -> None: - self._world: dexsim.World = dexsim.default_world() - self._ps = self._world.get_physics_scene() - self._all_indices = torch.arange(len(entities), dtype=torch.int32).tolist() - - self._data = SoftBodyData(entities=entities, ps=self._ps, device=device) - - self._world.update(0.001) - - self._visual_material: List[VisualMaterialInst | None] = [None] * len(entities) - self.is_shared_visual_material = False - - super().__init__(cfg=cfg, entities=entities, device=device) - - self._initialize_existing_visual_material() - - # set default collision filter - self._set_default_collision_filter() - - def _initialize_existing_visual_material(self) -> None: - """Wrap asset-parsed materials during soft-object construction. - - For a multi-segment render body, the first segment with a valid - material is registered as the environment's representative material. - """ - self._original_visual_material = [[] for _ in self._entities] - self._original_visual_material_inst = [None] * len(self._entities) - for env_idx, entity in enumerate(self._entities): - render_body = entity.get_render_body() - if render_body is None: - continue - original_materials = _capture_render_materials(render_body) - self._original_visual_material[env_idx] = original_materials - wrapped = _wrap_first_render_material(original_materials) - if wrapped is not None: - self._visual_material[env_idx] = wrapped - self._original_visual_material_inst[env_idx] = wrapped - - def set_visual_material( - self, - mat: VisualMaterial, - env_ids: Sequence[int] | None = None, - shared: bool = False, - ) -> None: - """Set visual material for the soft object. - - Args: - mat: The material template to assign. - env_ids: Environment indices. If None, all instances are used. - shared: Whether selected environments share one material instance. - """ - local_env_ids = self._all_indices if env_ids is None else env_ids - if shared: - if len(local_env_ids) != self.num_instances: - logger.log_error("Cannot share material instance for partial env_ids.") - mat_inst = mat.create_instance(f"{mat.uid}_{self.uid}") - for env_idx in local_env_ids: - self._entities[env_idx].set_material(mat_inst.mat) - self._visual_material[env_idx] = mat_inst - self.is_shared_visual_material = True - else: - for env_idx in local_env_ids: - mat_inst = mat.create_instance(f"{mat.uid}_{self.uid}_{env_idx}") - self._entities[env_idx].set_material(mat_inst.mat) - self._visual_material[env_idx] = mat_inst - self.is_shared_visual_material = False - - def restore_visual_material(self, env_ids: Sequence[int] | None = None) -> None: - """Restore visual materials captured when the soft object was created. - - Args: - env_ids: Environment indices. If None, all instances are restored. - """ - if not hasattr(self, "_original_visual_material"): - return - local_env_ids = self._all_indices if env_ids is None else env_ids - for env_idx in local_env_ids: - render_body = self._entities[env_idx].get_render_body() - if render_body is None: - continue - _restore_render_materials( - render_body, self._original_visual_material[env_idx] - ) - self._visual_material[env_idx] = self._original_visual_material_inst[ - env_idx - ] - self.is_shared_visual_material = False - - def get_visual_material_inst( - self, env_ids: Sequence[int] | None = None - ) -> List[VisualMaterialInst | None]: - """Get the material instance registered for each selected environment. - - Args: - env_ids: Environment indices. If None, all instances are returned. - - Returns: - The existing material wrappers, or None where an asset has no material. - """ - ids = env_ids if env_ids is not None else range(self.num_instances) - return [self._visual_material[i] for i in ids] - - def _set_default_collision_filter(self) -> None: - collision_filter_data = torch.zeros( - size=(self.num_instances, 4), dtype=torch.int32 - ) - for i in range(self.num_instances): - collision_filter_data[i, 0] = i - collision_filter_data[i, 1] = 1 - self.set_collision_filter(collision_filter_data) - - def set_collision_filter( - self, filter_data: torch.Tensor, env_ids: Sequence[int] | None = None - ) -> None: - """Set collision filter data for the soft object. - - Args: - filter_data (torch.Tensor): [N, 4] of int. - First element of each object is arena id. - If 2nd element is 0, the object will collision with all other objects in world. - 3rd and 4th elements are not used currently. - - env_ids (Sequence[int] | None): Environment indices. If None, then all indices are used. - """ - local_env_ids = self._all_indices if env_ids is None else env_ids - - if len(local_env_ids) != len(filter_data): - logger.log_error( - f"Length of env_ids {len(local_env_ids)} does not match pose length {len(filter_data)}." - ) - - filter_data_np = filter_data.cpu().numpy().astype(np.uint32) - for i, env_idx in enumerate(local_env_ids): - self._entities[env_idx].get_physical_body().set_collision_filter_data( - filter_data_np[i] - ) - - @property - def body_data(self) -> SoftBodyData | None: - """Get the soft body data manager for this soft object. - - Returns: - SoftBodyData | None: The soft body data manager. - """ - return self._data - - def set_local_pose( - self, pose: torch.Tensor, env_ids: Sequence[int] | None = None - ) -> None: - """Set local pose of the soft object. - - Args: - pose (torch.Tensor): The local pose of the soft object with shape (N, 7) or (N, 4, 4). - env_ids (Sequence[int] | None): Environment indices. If None, then all indices are used. - """ - from embodichain.lab.sim import SimulationManager - - sim = SimulationManager.get_instance() - local_env_ids = self._all_indices if env_ids is None else env_ids - - if len(local_env_ids) != len(pose): - logger.log_error( - f"Length of env_ids {len(local_env_ids)} does not match pose length {len(pose)}." - ) - - if pose.dim() == 2 and pose.shape[1] == 7: - pose4x4 = xyz_quat_to_4x4_matrix(pose) - elif pose.dim() == 3 and pose.shape[1:3] == (4, 4): - pose4x4 = pose - else: - logger.log_error( - f"Invalid pose shape {pose.shape}. Expected (N, 7) or (N, 4, 4)." - ) - - arena_offsets = sim.arena_offsets - for i, env_idx in enumerate(local_env_ids): - # TODO: soft body cannot directly set by `set_local_pose` currently. - rest_collision_vertices = self.body_data.rest_collision_vertices[i] - rest_sim_vertices = self.body_data.rest_sim_vertices[i] - rotation = pose4x4[i][:3, :3] - translation = pose4x4[i][:3, 3] - - # apply transformation to local rest vertices and back - rest_collision_vertices_local = rest_collision_vertices - arena_offsets[i] - transformed_collision_vertices = ( - rest_collision_vertices_local @ rotation.T + translation - ) - transformed_collision_vertices = ( - transformed_collision_vertices + arena_offsets[i] - ) - - rest_sim_vertices_local = rest_sim_vertices - arena_offsets[i] - transformed_sim_vertices = ( - rest_sim_vertices_local @ rotation.T + translation - ) - transformed_sim_vertices = transformed_sim_vertices + arena_offsets[i] - - # apply vertices to soft body - soft_body: SoftBody = self._entities[env_idx].get_physical_body() - collision_position_buffer = soft_body.get_position_inv_mass_buffer() - sim_position_buffer = soft_body.get_sim_position_inv_mass_buffer() - sim_velocity_buffer = soft_body.get_sim_velocity_buffer() - - collision_position_buffer[:, :3] = transformed_collision_vertices - sim_position_buffer[:, :3] = transformed_sim_vertices - sim_velocity_buffer[:, :3] = 0.0 - - soft_body.mark_dirty(SoftBodyGPUAPIReadWriteType.ALL) - # TODO: currently soft body has no wake up interface, use set_wake_counter and pass in a positive value to wake it up - soft_body.set_wake_counter(0.4) - - def get_rest_collision_vertices(self) -> torch.Tensor: - """Get the rest collision vertices of the soft object. - - Returns: - torch.Tensor: The rest collision vertices with shape (N, num_collision_vertices, 3). - """ - return self.body_data.rest_collision_vertices - - def get_rest_sim_vertices(self) -> torch.Tensor: - """Get the rest sim vertices of the soft object. - - Returns: - torch.Tensor: The rest sim vertices with shape (N, num_sim_vertices, 3). - """ - return self.body_data.rest_sim_vertices - - def get_current_collision_vertices(self) -> torch.Tensor: - """Get the current collision vertices of the soft object. - - Returns: - torch.Tensor: The current collision vertices with shape (N, num_collision_vertices, 3). - """ - return self.body_data.collision_position - - def get_current_sim_vertices(self) -> torch.Tensor: - """Get the current sim vertices of the soft object. - - Returns: - torch.Tensor: The current sim vertices with shape (N, num_sim_vertices, 3). - """ - return self.body_data.sim_vertex_position - - def get_current_sim_vertex_velocities(self) -> torch.Tensor: - """Get the current sim vertex velocities of the soft object. - - Returns: - torch.Tensor: The current sim vertex velocities with shape (N, num_sim_vertices, 3). - """ - return self.body_data.sim_vertex_velocity - - def get_collision_surface_triangles( - self, env_ids: Sequence[int] | None = None - ) -> torch.Tensor: - """Get approximate collision-surface triangles for selected instances. - - DexSim currently exposes live soft-body collision vertices without - their topology. This method returns a cached convex-hull topology, so - it is suitable for low-frequency external visualization but does not - preserve concave details of the render mesh. - - Args: - env_ids: Environment indices. If ``None``, returns all instances. - - Returns: - Triangle indices with shape ``(N, num_triangles, 3)``. - """ - ids = self._all_indices if env_ids is None else env_ids - return ( - self.body_data.collision_surface_triangles.unsqueeze(0) - .expand(len(ids), -1, -1) - .clone() - ) - - def get_triangles(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: - """Get approximate surface triangles for generic mesh consumers. - - Args: - env_ids: Environment indices. If ``None``, returns all instances. - - Returns: - Triangle indices with shape ``(N, num_triangles, 3)``. - """ - return self.get_collision_surface_triangles(env_ids=env_ids) - - def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: - """Get local pose of the soft object. - - Args: - to_matrix (bool, optional): If True, return the pose as a 4x4 matrix. If False, return as (x, y, z, qw, qx, qy, qz). Defaults to False. - - Returns: - torch.Tensor: The local pose of the soft object with shape (N, 7) or (N, 4, 4) depending on `to_matrix`. - """ - raise NotImplementedError("Getting local pose for SoftObject is not supported.") - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - local_env_ids = self._all_indices if env_ids is None else env_ids - num_instances = len(local_env_ids) - - self.restore_visual_material(env_ids=local_env_ids) - - # TODO: set attr for soft body after loading in physics scene. - - # rest soft body to init_pos - pos = torch.as_tensor( - self.cfg.init_pos, dtype=torch.float32, device=self.device - ) - rot = ( - torch.as_tensor(self.cfg.init_rot, dtype=torch.float32, device=self.device) - * torch.pi - / 180.0 - ) - pos = pos.unsqueeze(0).repeat(num_instances, 1) - rot = rot.unsqueeze(0).repeat(num_instances, 1) - mat = matrix_from_euler(rot, "XYZ") - pose = ( - torch.eye(4, dtype=torch.float32, device=self.device) - .unsqueeze(0) - .repeat(num_instances, 1, 1) - ) - pose[:, :3, 3] = pos - pose[:, :3, :3] = mat - self.set_local_pose(pose, env_ids=local_env_ids) - - def destroy(self) -> None: - # TODO: not tested yet - env = self._world.get_env() - arenas = env.get_all_arenas() - if len(arenas) == 0: - arenas = [env] - for i, entity in enumerate(self._entities): - arenas[i].remove_actor(entity) diff --git a/embodichain/lab/sim/physics/__init__.py b/embodichain/lab/sim/physics/__init__.py new file mode 100644 index 000000000..deb91a875 --- /dev/null +++ b/embodichain/lab/sim/physics/__init__.py @@ -0,0 +1,73 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Physics backend registry and factory. + +Selects a concrete :class:`PhysicsBackend` from a physics config via +:func:`embodichain.lab.sim.cfg.physics_backend_from_cfg` and instantiates it +with the owning :class:`SimulationManager` as its back-reference. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from embodichain.lab.sim.cfg import physics_backend_from_cfg +from embodichain.utils import logger + +from .base import PhysicsBackend +from .default import DefaultPhysicsBackend +from .newton import NewtonPhysicsBackend + +if TYPE_CHECKING: + from embodichain.lab.sim.sim_manager import SimulationManager + +__all__ = [ + "PhysicsBackend", + "DefaultPhysicsBackend", + "NewtonPhysicsBackend", + "make_physics_backend", +] + +#: Registry of backend name -> backend class. +_BACKENDS: dict[str, type[PhysicsBackend]] = { + "default": DefaultPhysicsBackend, + "newton": NewtonPhysicsBackend, +} + + +def make_physics_backend(physics_cfg, manager: "SimulationManager") -> PhysicsBackend: + """Construct the physics backend for ``physics_cfg``. + + The backend subclass is selected by the *type* of ``physics_cfg`` + (via :func:`physics_backend_from_cfg`), so passing a + :class:`~embodichain.lab.sim.cfg.NewtonPhysicsCfg` activates the Newton + backend and a + :class:`~embodichain.lab.sim.cfg.DefaultPhysicsCfg` activates the default + backend. + + Args: + physics_cfg: The physics backend configuration. + manager: The owning :class:`SimulationManager` (passed as the + backend's back-reference). + + Returns: + The instantiated :class:`PhysicsBackend`. + """ + name = physics_backend_from_cfg(physics_cfg) + cls = _BACKENDS.get(name) + if cls is None: + logger.log_error(f"Unknown physics backend: {name!r}.") + return cls(manager) diff --git a/embodichain/lab/sim/physics/base.py b/embodichain/lab/sim/physics/base.py new file mode 100644 index 000000000..b241fcca5 --- /dev/null +++ b/embodichain/lab/sim/physics/base.py @@ -0,0 +1,196 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Spawn-aware physics-backend abstraction for :class:`SimulationManager`. + +This module defines the contract that every physics backend (Default, Newton, +...) satisfies. The owning :class:`SimulationManager` +holds a single :class:`PhysicsBackend` instance as ``self.physics`` and +delegates backend-specific world configuration, compatibility scene access, +and capability queries to it. Scene topology and runtime readiness are owned +by DexSim's ``SceneBuilder`` and finalized ``Scene``. + +The design deliberately mirrors IsaacLab's split of an orchestrator +(``SimulationContext``) from a swappable physics manager (``PhysicsManager``), +with one departure: EmbodiChain keeps the backend as a true *instance* member +rather than a class-singleton, because :class:`SimulationManager` is itself a +multiton (one instance per ``instance_id``) and a class-singleton backend +would break that. + +.. note:: + This ABC covers the *manager-level* backend surface (lifecycle, scene, + capabilities, world-config). The per-asset read/write contract lives in + :mod:`embodichain.lab.sim.objects.backends` (``RigidBodyViewBase`` / + ``ArticulationViewBase``). +""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + import dexsim + + from embodichain.lab.sim.sim_manager import SimulationManager, SimulationManagerCfg + +__all__ = ["PhysicsBackend"] + + +class PhysicsBackend(ABC): + """Abstract base class for a swappable physics backend. + + A backend is constructed with a back-reference to its owning + :class:`SimulationManager` (from which it reaches the dexsim world, the + resolved device, the asset registries and the physics config). All + Manager-level backend behavior is expressed as overrides of the methods + and properties below. The backend name remains available for diagnostics + and backwards-compatible public predicates, but operational decisions use + hooks and capability flags. + """ + + #: Backend identifier, e.g. ``"default"`` or ``"newton"``. + name: str = "" + + def __init__(self, manager: "SimulationManager") -> None: + self._manager: "SimulationManager" = manager + + # ------------------------------------------------------------------ # + # Construction / world-config activation + # ------------------------------------------------------------------ # + @abstractmethod + def configure_world( + self, + world_config: "dexsim.WorldConfig", + sim_config: "SimulationManagerCfg", + ) -> None: + """Apply backend-specific fields to the dexsim ``WorldConfig``. + + Called from :meth:`SimulationManager._convert_sim_config` after the + shared world-config fields and the resolved device have been set, so + implementations may read ``self._manager.device``. + + Args: + world_config: The dexsim world config to mutate in place. + sim_config: The full simulation manager config. + """ + + @abstractmethod + def activate(self, sim_config: "SimulationManagerCfg") -> None: + """Perform backend setup immediately after the dexsim World is created. + + Default configures the native DexSim globals. Newton is already + registered from ``WorldConfig.newton_cfg`` and therefore has no + additional activation work. + """ + + def prepare_spawn_runtime(self, result: "dexsim.scene.Scene") -> None: + """Prepare runtime buffers for one committed Spawn topology revision. + + :class:`SimulationManager` calls this hook once per topology revision, + after source configuration and before facade binding. Backends that do + not need a separate runtime-preparation step keep the default no-op. + + Args: + result: The committed Spawn result being prepared. + """ + del result + + def sync_render_state(self, result: "dexsim.scene.Scene") -> None: + """Publish the current physics state to render resources without stepping. + + Backends whose physics and render state share native storage require no + work. Backends with a separate render bridge override this hook. + + Args: + result: The finalized Spawn result whose state should be published. + """ + del result + + def prepare_for_teardown(self) -> None: + """Release backend-owned views before Spawn releases their parents. + + :class:`SimulationManager` calls this during deferred destruction, + after render workers stop and before it closes the Spawn result. A + backend can use this boundary to synchronize device work and release + borrowed render or physics views while their World-owned native + parents are still alive. Backends without such views keep the default + no-op implementation. + """ + + # ------------------------------------------------------------------ # + # Scene access + # ------------------------------------------------------------------ # + @abstractmethod + def get_scene(self): + """Return a backend compatibility scene, or raise if none exists.""" + + @property + def newton_manager(self): + """Return ``None`` because Spawn does not use ``NewtonManager``. + + The Newton backend overrides this property with an actionable error so + callers do not accidentally mix the removed manager ownership domain + with the World-owned Spawn backend. + """ + return None + + @property + def differentiable_runtime(self): + """Return no differentiable runtime for non-Newton backends.""" + return None + + @property + def solver_type(self) -> str | None: + """Return the configured or resolved backend solver type, if exposed.""" + return None + + # ------------------------------------------------------------------ # + # Capabilities (override in subclasses; defaults are conservative) + # ------------------------------------------------------------------ # + @property + def supports_volume_deformables(self) -> bool: + """Whether this backend has a volume-deformable object adapter.""" + return False + + @property + def supports_surface_deformables(self) -> bool: + """Whether this backend has a surface-deformable object adapter.""" + return False + + @property + def supports_rigid_object_group(self) -> bool: + """Whether this backend supports rigid object groups.""" + return False + + @property + def supports_robot(self) -> bool: + """Whether this backend supports robots (articulated URDF assets).""" + return False + + @property + def supports_rigid_constraints(self) -> bool: + """Whether this backend supports native rigid constraints.""" + return False + + @property + def supports_contact_sensor(self) -> bool: + """Whether this backend supports the native contact sensor.""" + return False + + @property + def can_disable_manual_update(self) -> bool: + """Whether ``set_manual_update(False)`` is permitted on this backend.""" + return True diff --git a/embodichain/lab/sim/physics/default.py b/embodichain/lab/sim/physics/default.py new file mode 100644 index 000000000..87d023dea --- /dev/null +++ b/embodichain/lab/sim/physics/default.py @@ -0,0 +1,95 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Default physics backend implementation integrated through DexSim.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import dexsim + +from embodichain.lab.sim.cfg import DefaultPhysicsCfg + +from .base import PhysicsBackend + +if TYPE_CHECKING: + from embodichain.lab.sim.sim_manager import SimulationManagerCfg + +__all__ = ["DefaultPhysicsBackend"] + + +class DefaultPhysicsBackend(PhysicsBackend): + """Default backend using DexSim's native GPU or CPU physics path.""" + + name = "default" + + @property + def solver_type(self) -> str: + """Return the native PhysX constraint solver selected for the scene.""" + return "TGS" if dexsim.get_physics_config().enable_tgs else "PGS" + + # -- construction / world-config activation ------------------------- # + def configure_world(self, world_config, sim_config: "SimulationManagerCfg") -> None: + cfg = sim_config.physics_cfg + assert isinstance(cfg, DefaultPhysicsCfg) + world_config.length_tolerance = cfg.length_tolerance + world_config.speed_tolerance = cfg.speed_tolerance + if self._manager.device.type == "cuda": + world_config.enable_gpu_sim = True + world_config.direct_gpu_api = True + + def activate(self, sim_config: "SimulationManagerCfg") -> None: + cfg = sim_config.physics_cfg + assert isinstance(cfg, DefaultPhysicsCfg) + dexsim.set_physics_config(**cfg.to_dexsim_args()) + dexsim.set_physics_gpu_memory_config(**cfg.gpu_memory.to_dict()) + + def prepare_spawn_runtime(self, result: "dexsim.scene.Scene") -> None: + """Initialize Direct GPU buffers for a committed CUDA topology.""" + del result + if self._manager.device.type == "cuda": + self._manager._world.init_gpu_physics() + + # -- scene ---------------------------------------------------------- # + def get_scene(self): + """Return the Default backend's compatibility scene after Spawn is prepared.""" + self._manager.prepare() + return self._manager._world.get_physics_scene() + + # -- capabilities --------------------------------------------------- # + @property + def supports_volume_deformables(self) -> bool: + return False + + @property + def supports_surface_deformables(self) -> bool: + return False + + @property + def supports_rigid_object_group(self) -> bool: + return True + + @property + def supports_robot(self) -> bool: + return True + + @property + def supports_rigid_constraints(self) -> bool: + return True + + @property + def supports_contact_sensor(self) -> bool: + return True diff --git a/embodichain/lab/sim/physics/newton.py b/embodichain/lab/sim/physics/newton.py new file mode 100644 index 000000000..003964f24 --- /dev/null +++ b/embodichain/lab/sim/physics/newton.py @@ -0,0 +1,195 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""World-owned Newton (Warp) physics backend configuration.""" + +from __future__ import annotations + +import importlib +from typing import TYPE_CHECKING +import weakref + +import warp as wp + +from .base import PhysicsBackend + +if TYPE_CHECKING: + import dexsim + + from embodichain.lab.sim.sim_manager import SimulationManagerCfg + +__all__ = ["NewtonPhysicsBackend"] + + +def is_newton_gradient_mode(result) -> bool: + """Return whether a finalized Spawn result uses Newton gradients.""" + if result is None or getattr(result, "backend", None) != "newton": + return False + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(result.world) + if backend is None: + return False + return bool( + backend.cfg.requires_grad + or (backend.model is not None and backend.model.requires_grad) + ) + + +class NewtonPhysicsBackend(PhysicsBackend): + """The Warp-based Newton physics backend integrated through DexSim.""" + + name = "newton" + + def __init__(self, manager) -> None: + super().__init__(manager) + self._differentiable_runtime = None + self._runtime_device: str | None = None + self._configured_solver_type: str | None = None + + @property + def solver_type(self) -> str | None: + """Return the configured or scene-resolved Newton solver type.""" + world = getattr(self._manager, "_world", None) + if world is not None: + from dexsim.engine.newton_physics.backend_registry import ( + get_newton_backend, + ) + + backend = get_newton_backend(world) + if backend is not None: + return str(backend.solver_type) + return self._configured_solver_type + + # -- construction / world-config activation ------------------------- # + + @property + def cuda_graph_status(self) -> str: + """Return the World-owned graph state without triggering capture.""" + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(self._manager._world) + return backend.cuda_graph_status if backend is not None else "pending" + + def configure_world(self, world_config, sim_config: "SimulationManagerCfg") -> None: + importlib.import_module("dexsim.engine.newton_physics") + + newton_physics_cfg = sim_config.physics_cfg + newton_cfg = newton_physics_cfg.to_dexsim_cfg( + gpu_id=sim_config.gpu_id, + ) + self._configured_solver_type = str(newton_cfg.solver_cfg.solver_type) + self._runtime_device = str(newton_cfg.device) + world_config.newton_cfg = newton_cfg + + def activate(self, sim_config: "SimulationManagerCfg") -> None: + del sim_config + # WorldConfig.newton_cfg registers the World-owned NewtonBackend. + # SceneBuilder.finalize() completes its model; no second manager-level + # activation or rebuild domain participates. + + def sync_render_state(self, result: "dexsim.scene.Scene") -> None: + """Publish Newton state through DexSim's render bridge without stepping.""" + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(result.world) + if backend is None: + raise RuntimeError( + "Newton backend is unavailable for render-state synchronization." + ) + backend.sync_to_dexsim(result.world) + backend.sync_particle_fluids(result.world) + + def prepare_for_teardown(self) -> None: + """Release Newton render views while Spawn still owns their parents.""" + if self._runtime_device is not None and self._runtime_device.startswith("cuda"): + wp.synchronize_device(self._runtime_device) + + world = getattr(self._manager, "_world", None) + if world is None: + return + + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(world) + if backend is not None: + # NewtonRenderSync retains native link-node wrappers. They must be + # released before Scene.close() drops the owning skeletons; + # otherwise pybind can destruct a child after its native parent. + backend.render_sync.clear() + + @property + def newton_manager(self): + """Reject access to the removed, independently owned Newton manager.""" + raise RuntimeError( + "NewtonManager is not part of Spawn scene ownership. Use " + "SimulationManager.spawn_result and its Spawned*/Batch APIs." + ) + + @property + def differentiable_runtime(self): + """Return the differentiable facade over the Spawn-owned runtime.""" + if self._differentiable_runtime is None: + from embodichain.lab.sim.diff.runtime import NewtonDifferentiableRuntime + + owner_ref = weakref.ref(self) + + def backend_provider(): + owner = owner_ref() + if owner is None: + return None + result = owner._manager.spawn_result + if result is None: + return None + from dexsim.engine.newton_physics.backend_registry import ( + get_newton_backend, + ) + + return get_newton_backend(result.world) + + self._differentiable_runtime = NewtonDifferentiableRuntime(backend_provider) + return self._differentiable_runtime + + # -- scene ---------------------------------------------------------- # + def get_scene(self): + raise RuntimeError( + "Newton Spawn scenes do not expose a PhysicsScene. Use " + "SimulationManager.spawn_result and its Spawned*/Batch APIs." + ) + + # -- capabilities --------------------------------------------------- # + @property + def supports_volume_deformables(self) -> bool: + return True + + @property + def supports_surface_deformables(self) -> bool: + return True + + @property + def supports_robot(self) -> bool: + # Robots are SpawnedArticulations in the World-owned Newton model. + return True + + @property + def supports_rigid_object_group(self) -> bool: + # Groups are env-major views over the Scene rigid-body batch, which + # provides the same state and mass-property API on Newton. + return True + + @property + def can_disable_manual_update(self) -> bool: + # Newton cannot switch between manual and automatic update. + return False diff --git a/embodichain/lab/sim/robots/cobotmagic.py b/embodichain/lab/sim/robots/cobotmagic.py index e6bd9b9f1..afbd53df9 100644 --- a/embodichain/lab/sim/robots/cobotmagic.py +++ b/embodichain/lab/sim/robots/cobotmagic.py @@ -22,10 +22,13 @@ from typing import TYPE_CHECKING, Dict, List, Union from embodichain.lab.sim.cfg import ( + ArticulationRootPropertiesCfg, + CollisionPropertiesCfg, RobotCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, URDFCfg, JointDrivePropertiesCfg, - RigidBodyAttributesCfg, ) from embodichain.lab.sim.motion.solvers import SolverCfg, OPWSolverCfg from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg @@ -122,9 +125,8 @@ def _build_defaults(self, init_dict: dict | None = None) -> None: ), ), } - self.min_position_iters = 8 - self.min_velocity_iters = 2 - self.drive_pros = JointDrivePropertiesCfg( + self.joint_drive_props = JointDrivePropertiesCfg( + drive_type="force", stiffness={ "left_joint[1-6]": 7e4, "right_joint[1-6]": 7e4, @@ -144,10 +146,19 @@ def _build_defaults(self, init_dict: dict | None = None) -> None: "right_joint[7-8]": 3e3, }, ) - self.attrs = RigidBodyAttributesCfg( - static_friction=0.95, - dynamic_friction=0.9, - contact_offset=0.001, + self.root_props = ArticulationRootPropertiesCfg( + min_position_iters=8, + min_velocity_iters=2, + ) + self.attrs = RigidBodyPhysicsCfg( + collision_props=CollisionPropertiesCfg( + contact_offset=0.001, + rest_offset=0.0, + ), + material_props=RigidBodyMaterialCfg( + static_friction=0.95, + dynamic_friction=0.9, + ), ) @property @@ -185,27 +196,48 @@ def build_pk_serial_chain( if __name__ == "__main__": + import argparse + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg - from embodichain.lab.sim.cfg import RenderCfg + from embodichain.lab.sim.cfg import RenderCfg, physics_cfg_for_backend from embodichain.lab.sim.robots import CobotMagicCfg + parser = argparse.ArgumentParser(description="Launch the CobotMagic robot") + parser.add_argument( + "--physics", + choices=("default", "newton"), + default="default", + help="Physics backend to launch (default: default).", + ) + parser.add_argument( + "--device", + type=str, + default=None, + help="Runtime device override; otherwise the selected backend default is used.", + ) + args = parser.parse_args() + torch.set_printoptions(precision=5, sci_mode=False) config = SimulationManagerCfg( headless=True, - sim_device="cpu", + device=args.device, num_envs=2, + physics_cfg=physics_cfg_for_backend(args.physics), render_cfg=RenderCfg(renderer="fast-rt"), ) sim = SimulationManager(config) - config = {"init_pos": [0.0, 0.0, 1.0], "init_qpos": [0.1] * 16} + config = { + "init_pos": [0.0, 0.0, 1.0], + } cfg = CobotMagicCfg.from_dict(config) robot = sim.add_robot(cfg=cfg) - # sim.open_window() + sim.prepare() + sim.open_window() + from IPython import embed - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + embed() # noqa: E702 print("CobotMagic added to the simulation.") diff --git a/embodichain/lab/sim/robots/dexforce_w1/cfg.py b/embodichain/lab/sim/robots/dexforce_w1/cfg.py index 87eb31bb3..cae571e9c 100644 --- a/embodichain/lab/sim/robots/dexforce_w1/cfg.py +++ b/embodichain/lab/sim/robots/dexforce_w1/cfg.py @@ -21,6 +21,15 @@ import numpy as np import torch +if __name__ == "__main__" and not __package__: + # Support running this example by file path from an uninstalled source tree. + import sys + from pathlib import Path + + # Replace the script directory so its ``types.py`` cannot shadow the + # standard-library ``types`` module in compiler subprocesses. + sys.path[0] = str(Path(__file__).resolve().parents[5]) + from typing import TYPE_CHECKING, Dict from embodichain.lab.sim.robots.dexforce_w1.types import ( @@ -39,9 +48,12 @@ ) from embodichain.lab.sim.robots.dexforce_w1.specs import get_w1_version_spec from embodichain.lab.sim.cfg import ( + ArticulationRootPropertiesCfg, + CollisionPropertiesCfg, RobotCfg, JointDrivePropertiesCfg, - RigidBodyAttributesCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, ) from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg from embodichain.utils import configclass @@ -164,7 +176,7 @@ def _build_defaults(self, init_dict: dict | None = None) -> None: Reads ``version``/``with_default_eef`` from ``init_dict``, sets them on ``self``, then populates ``urdf_cfg``, ``control_parts``, - ``solver_cfg``, ``drive_pros`` and ``attrs``. + ``solver_cfg``, ``joint_drive_props`` and ``attrs``. """ init_dict = init_dict or {} self.version = DexforceW1Version.parse( @@ -272,28 +284,38 @@ def _build_default_physics_cfgs( "damping": {ARM_JOINTS: 1e3, BODY_JOINTS: 1e4, HEAD_JOINTS: 1e3}, "max_effort": {ARM_JOINTS: 1e5, BODY_JOINTS: 1e10, HEAD_JOINTS: 1e5}, } - drive_pros = JointDrivePropertiesCfg(**joint_params) + joint_drive_props = JointDrivePropertiesCfg( + drive_type="force", + **joint_params, + ) if with_default_eef: eef_joint_names = DEFAULT_EEF_HAND_JOINT_NAMES - drive_pros.stiffness.update( + joint_drive_props.stiffness.update( {eef_joint_names: DEFAULT_EEF_JOINT_DRIVE_PARAMS["stiffness"]} ) - drive_pros.damping.update( + joint_drive_props.damping.update( {eef_joint_names: DEFAULT_EEF_JOINT_DRIVE_PARAMS["damping"]} ) - drive_pros.max_effort.update( + joint_drive_props.max_effort.update( {eef_joint_names: DEFAULT_EEF_JOINT_DRIVE_PARAMS["max_effort"]} ) return { - "min_position_iters": 32, - "min_velocity_iters": 8, - "drive_pros": drive_pros, - "attrs": RigidBodyAttributesCfg( - static_friction=0.95, - dynamic_friction=0.9, - contact_offset=0.001, + "joint_drive_props": joint_drive_props, + "root_props": ArticulationRootPropertiesCfg( + min_position_iters=32, + min_velocity_iters=8, + ), + "attrs": RigidBodyPhysicsCfg( + collision_props=CollisionPropertiesCfg( + contact_offset=0.001, + rest_offset=0.0, + ), + material_props=RigidBodyMaterialCfg( + static_friction=0.95, + dynamic_friction=0.9, + ), ), } @@ -329,17 +351,43 @@ def build_pk_serial_chain( if __name__ == "__main__": - # Example usage - import numpy as np + import argparse np.set_printoptions(precision=5, suppress=True) from embodichain.lab.sim import SimulationManager, SimulationManagerCfg - - config = SimulationManagerCfg(headless=True, sim_device="cpu", num_envs=4) + from embodichain.lab.sim.cfg import physics_cfg_for_backend + + parser = argparse.ArgumentParser(description="Launch the Dexforce W1 robot") + parser.add_argument( + "--physics", + choices=("default", "newton"), + default="newton", + help="Physics backend to launch (default: newton).", + ) + parser.add_argument( + "--device", + type=str, + default=None, + help="Runtime device override; otherwise the selected backend default is used.", + ) + args = parser.parse_args() + + config = SimulationManagerCfg( + headless=True, + device=args.device, + num_envs=4, + physics_cfg=physics_cfg_for_backend(args.physics), + ) sim = SimulationManager(config) cfg = DexforceW1Cfg.from_dict({"uid": "dexforce_w1", "version": "v021"}) robot = sim.add_robot(cfg=cfg) + sim.prepare() sim.update(step=1) - print("DexforceW1 robot added to the simulation.") + print("DexforceW1 robot added to the simulation.", flush=True) + sim.open_window() + from IPython import embed + + embed() # noqa: E702 + sim.destroy() diff --git a/embodichain/lab/sim/robots/dual_arm.py b/embodichain/lab/sim/robots/dual_arm.py index aa14fcbf3..7db206ab1 100644 --- a/embodichain/lab/sim/robots/dual_arm.py +++ b/embodichain/lab/sim/robots/dual_arm.py @@ -265,7 +265,7 @@ def _resolve_base_cfg(base_robot: str | dict) -> RobotCfg: # --------------------------------------------------------------------------- # -def _mirror_drive_pros( +def _mirror_joint_drive_props( base_drive: JointDrivePropertiesCfg, name_case: dict[str, str] | None = None ) -> JointDrivePropertiesCfg: """Mirror a single-arm drive config across left/right arms. @@ -288,13 +288,14 @@ def _mirror_drive_pros( Returns: A fresh :class:`JointDrivePropertiesCfg` for the dual arm. """ - new = JointDrivePropertiesCfg(drive_type=base_drive.drive_type) - for prop in _DRIVE_PROPS: + new = type(base_drive)(drive_type=base_drive.drive_type) + properties = [*_DRIVE_PROPS, "target_mode"] + for prop in properties: val = getattr(base_drive, prop, None) if val is None: continue if isinstance(val, dict): - mirrored: Dict[str, float] = {} + mirrored: Dict[str, object] = {} for pattern, v in val.items(): mirrored[_prefixed_name(str(pattern), "left_", "joint", name_case)] = v mirrored[_prefixed_name(str(pattern), "right_", "joint", name_case)] = v @@ -404,14 +405,11 @@ def _populate_dual_cfg( ) cfg.solver_cfg = new_solver - cfg.drive_pros = _mirror_drive_pros(base_cfg.drive_pros, name_case) + cfg.joint_drive_props = _mirror_joint_drive_props( + base_cfg.joint_drive_props, name_case + ) cfg.attrs = base_cfg.attrs.copy() - cfg.min_position_iters = base_cfg.min_position_iters - cfg.min_velocity_iters = base_cfg.min_velocity_iters - cfg.fix_base = base_cfg.fix_base - cfg.disable_self_collision = base_cfg.disable_self_collision - cfg.enable_gravity = base_cfg.enable_gravity - cfg.sleep_threshold = base_cfg.sleep_threshold + cfg.root_props = base_cfg.root_props.copy() def build_dual_arm_cfg( @@ -456,7 +454,7 @@ class DualArmRobotCfg(RobotCfg): Two identical arms (the ``base_robot``) are mounted on a shared synthetic ``base_link``. The left/right ``control_parts``, per-arm ``solver_cfg`` and - mirrored ``drive_pros`` are derived automatically by + mirrored ``joint_drive_props`` are derived automatically by :func:`build_dual_arm_cfg`. Example: @@ -575,15 +573,33 @@ def build_pk_serial_chain( if __name__ == "__main__": + import argparse + np.set_printoptions(precision=5, suppress=True) from embodichain.lab.sim import SimulationManager, SimulationManagerCfg - from embodichain.lab.sim.cfg import RenderCfg + from embodichain.lab.sim.cfg import RenderCfg, physics_cfg_for_backend + + parser = argparse.ArgumentParser(description="Launch a dual-arm robot") + parser.add_argument( + "--physics", + choices=("default", "newton"), + default="default", + help="Physics backend to launch (default: default).", + ) + parser.add_argument( + "--device", + type=str, + default=None, + help="Runtime device override; otherwise the selected backend default is used.", + ) + args = parser.parse_args() config = SimulationManagerCfg( headless=True, - sim_device="cpu", + device=args.device, num_envs=1, + physics_cfg=physics_cfg_for_backend(args.physics), render_cfg=RenderCfg(renderer="fast-rt"), ) sim = SimulationManager(config) @@ -609,11 +625,9 @@ def build_pk_serial_chain( } ) robot = sim.add_robot(cfg=cfg) + sim.prepare() sim.open_window() - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - # Round-trip check: from_dict(to_dict()) reproduces the cfg. cfg2 = DualArmRobotCfg.from_dict(cfg.to_dict()) assert cfg2.base_robot == cfg.base_robot diff --git a/embodichain/lab/sim/robots/franka_panda.py b/embodichain/lab/sim/robots/franka_panda.py index e60b1cc3b..48d6e6adc 100644 --- a/embodichain/lab/sim/robots/franka_panda.py +++ b/embodichain/lab/sim/robots/franka_panda.py @@ -24,7 +24,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.cfg import ( JointDrivePropertiesCfg, - RigidBodyAttributesCfg, RobotCfg, URDFCfg, ) @@ -141,7 +140,8 @@ def _build_defaults(self, init_dict: dict | None = None) -> None: ), } - self.drive_pros = JointDrivePropertiesCfg( + self.joint_drive_props = JointDrivePropertiesCfg( + drive_type="force", stiffness={ "fr3_joint[1-7]": 1e4, "fr3_finger_joint[1-2]": 1e3, @@ -188,26 +188,42 @@ def build_pk_serial_chain( if __name__ == "__main__": + import argparse + np.set_printoptions(precision=5, suppress=True) from embodichain.lab.sim import SimulationManager, SimulationManagerCfg - from embodichain.lab.sim.cfg import RenderCfg + from embodichain.lab.sim.cfg import RenderCfg, physics_cfg_for_backend + + parser = argparse.ArgumentParser(description="Launch the Franka Panda robot") + parser.add_argument( + "--physics", + choices=("default", "newton"), + default="default", + help="Physics backend to launch (default: default).", + ) + parser.add_argument( + "--device", + type=str, + default=None, + help="Runtime device override; otherwise the selected backend default is used.", + ) + args = parser.parse_args() config = SimulationManagerCfg( - headless=False, - sim_device="cpu", + headless=True, + device=args.device, num_envs=1, + physics_cfg=physics_cfg_for_backend(args.physics), render_cfg=RenderCfg(renderer="hybrid"), ) sim = SimulationManager(config) cfg = FrankaPandaCfg.from_dict({"robot_type": "panda"}) robot = sim.add_robot(cfg=cfg) + sim.prepare() sim.open_window() - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - from IPython import embed embed() # noqa: F401 diff --git a/embodichain/lab/sim/robots/ur_robot.py b/embodichain/lab/sim/robots/ur_robot.py index 6239d35d4..4f47ba409 100644 --- a/embodichain/lab/sim/robots/ur_robot.py +++ b/embodichain/lab/sim/robots/ur_robot.py @@ -23,7 +23,6 @@ RobotCfg, URDFCfg, JointDrivePropertiesCfg, - RigidBodyAttributesCfg, ) from embodichain.lab.sim.motion.solvers import URSolverCfg from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg @@ -139,7 +138,8 @@ def _build_defaults(self, init_dict: dict | None = None) -> None: ), } - self.drive_pros = JointDrivePropertiesCfg( + self.joint_drive_props = JointDrivePropertiesCfg( + drive_type="force", stiffness={"arm": 1e4}, damping={"arm": 1e3}, max_effort={"arm": _UR_MAX_EFFORT[robot_type]}, @@ -180,17 +180,33 @@ def build_pk_serial_chain( if __name__ == "__main__": - import numpy as np + import argparse np.set_printoptions(precision=5, suppress=True) from embodichain.lab.sim import SimulationManager, SimulationManagerCfg - from embodichain.lab.sim.cfg import RenderCfg + from embodichain.lab.sim.cfg import RenderCfg, physics_cfg_for_backend + + parser = argparse.ArgumentParser(description="Launch a Universal Robot") + parser.add_argument( + "--physics", + choices=("default", "newton"), + default="default", + help="Physics backend to launch (default: default).", + ) + parser.add_argument( + "--device", + type=str, + default=None, + help="Runtime device override; otherwise the selected backend default is used.", + ) + args = parser.parse_args() config = SimulationManagerCfg( - headless=False, - sim_device="cpu", + headless=True, + device=args.device, num_envs=1, + physics_cfg=physics_cfg_for_backend(args.physics), render_cfg=RenderCfg(renderer="fast-rt"), ) sim = SimulationManager(config) @@ -200,11 +216,9 @@ def build_pk_serial_chain( {"robot_type": "ur10e", "init_qpos": [0.0, -1.57, 1.57, -1.57, -1.57, 0.0]} ) robot = sim.add_robot(cfg=cfg) + sim.prepare() sim.open_window() - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - from IPython import embed embed() # noqa: F401 diff --git a/embodichain/lab/sim/sensors/base_sensor.py b/embodichain/lab/sim/sensors/base_sensor.py index 3fb932f0d..a8e43866d 100644 --- a/embodichain/lab/sim/sensors/base_sensor.py +++ b/embodichain/lab/sim/sensors/base_sensor.py @@ -54,8 +54,8 @@ class OffsetCfg: pos: Tuple[float, float, float] = (0.0, 0.0, 0.0) """Position of the sensor in the parent frame. Defaults to (0.0, 0.0, 0.0).""" - quat: Tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) - """Orientation of the sensor in the parent frame as a quaternion (w, x, y, z). Defaults to (1.0, 0.0, 0.0, 0.0).""" + quat: Tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) + """Orientation in the parent frame as ``(x, y, z, w)``. Defaults to identity.""" parent: str | None = None """Name of the parent frame. If not specified, the sensor will be placed in the arena frame. @@ -171,10 +171,18 @@ class BaseSensor(BatchEntity): SUPPORTED_DATA_TYPES = [] def __init__( - self, config: SensorCfg, device: torch.device = torch.device("cpu") + self, + config: SensorCfg, + device: torch.device = torch.device("cpu"), + *, + num_instances: int | None = None, ) -> None: - - num_envs = get_dexsim_arena_num() + num_envs = ( + get_dexsim_arena_num() if num_instances is None else int(num_instances) + ) + if num_envs <= 0: + raise ValueError("A sensor requires at least one simulation instance.") + self._num_instances = num_envs self._data_buffer: TensorDict[str, torch.Tensor] = TensorDict( {}, batch_size=[num_envs], device=device ) @@ -186,7 +194,7 @@ def __init__( @cached_property def num_instances(self) -> int: - return get_dexsim_arena_num() + return self._num_instances @abstractmethod def _build_sensor_from_config( @@ -216,7 +224,8 @@ def get_arena_pose(self, to_matrix: bool = False) -> torch.Tensor: """Get the pose of the sensor in the arena frame. Args: - to_matrix: If True, return the pose as a 4x4 transformation matrix. + to_matrix: If True, return the pose as a 4x4 transformation matrix; + otherwise return ``(x, y, z, qx, qy, qz, qw)``. Returns: A tensor representing the pose of the sensor in the arena frame. diff --git a/embodichain/lab/sim/sensors/camera.py b/embodichain/lab/sim/sensors/camera.py index c65b30c28..99524810f 100644 --- a/embodichain/lab/sim/sensors/camera.py +++ b/embodichain/lab/sim/sensors/camera.py @@ -21,12 +21,15 @@ import dexsim.render as dr from functools import cached_property -from typing import List, Literal, Sequence, Tuple +from typing import TYPE_CHECKING, List, Literal, Sequence, Tuple from embodichain.lab.sim.sensors import BaseSensor, SensorCfg from embodichain.utils.math import matrix_from_quat, quat_from_matrix, look_at_to_pose from embodichain.utils import logger, configclass +if TYPE_CHECKING: + from embodichain.lab.sim.sim_manager import SimulationManager + __all__ = ["Camera", "CameraCfg"] @@ -137,28 +140,32 @@ class Camera(BaseSensor): SUPPORTED_DATA_TYPES = ["color", "depth", "mask", "normal", "position"] def __init__( - self, config: CameraCfg, device: torch.device = torch.device("cpu") + self, + config: CameraCfg, + device: torch.device = torch.device("cpu"), + *, + owner: SimulationManager, ) -> None: + self._world = owner.get_world() + self._arenas = [owner.get_env(i) for i in range(owner.num_envs)] + if len(self._arenas) == 0: + raise ValueError("Camera requires at least one materialized Arena.") + self._camera_names: list[tuple[dexsim.environment.Arena, str]] = [] self._is_attached = False - super().__init__(config, device) + self._is_destroyed = False + super().__init__(config, device, num_instances=len(self._arenas)) + self.reset() def _build_sensor_from_config( self, config: CameraCfg, device: torch.device ) -> None: - self._world = dexsim.default_world() - env = self._world.get_env() - arenas = env.get_all_arenas() - if len(arenas) == 0: - arenas = [env] - num_instances = len(arenas) - self._frame_buffer = self._world.create_camera_group( - [config.width, config.height], num_instances, True + [config.width, config.height], self.num_instances, True ) view_attrib = config.get_view_attrib() - for i, arena in enumerate(arenas): - view_name = f"{self.uid}_view{i + 1}" + for i, arena in enumerate(self._arenas): + view_name = f"{config.uid}_view{i + 1}" view = arena.create_camera( view_name, config.width, @@ -171,6 +178,7 @@ def _build_sensor_from_config( view.set_near(config.near) view.set_far(config.far) self._entities[i] = view + self._camera_names.append((arena, view_name)) # Define a mapping of data types to their respective shapes and dtypes buffer_specs = { @@ -218,10 +226,10 @@ def group_id(self) -> int: @property def is_attached(self) -> bool: - """Check if the camera is attached to a parent entity. + """Return whether all camera views are attached to parent nodes. Returns: - bool: True if the camera is attached to a parent entity, False otherwise. + True after parent attachment and extrinsics application succeed. """ return self._is_attached @@ -270,20 +278,15 @@ def update(self, **kwargs) -> None: self._frame_buffer.get_position_gpu_buffer().to(self.device)[..., :3] ) - def attach_to_parent_nodes( - self, parent_nodes: Sequence[dexsim.engine.Node] - ) -> None: + def attach_to_parent_nodes(self, parent_nodes: Sequence[object]) -> None: """Attach camera views to one resolved parent node per environment. - SimulationManager calls this after resolving ``extrinsics.parent``. - Cameras constructed directly can be attached by supplying their parent - render nodes explicitly. - Args: parent_nodes: Parent render nodes ordered by environment index. Raises: - RuntimeError: If the parent count differs from the camera count. + RuntimeError: If the number of parent nodes does not match the + number of camera instances. ValueError: If any parent node is missing. """ nodes = list(parent_nodes) @@ -296,7 +299,8 @@ def attach_to_parent_nodes( raise ValueError("Camera attachment requires a parent node in every arena.") for entity, parent in zip(self._entities, nodes, strict=True): entity.attach_node(parent) - # Reapply parent-relative extrinsics after reparenting the camera views. + # Extrinsics are expressed in the parent frame. Reapply them after + # reparenting because the camera was initially reset in Arena space. self.reset() self._is_attached = True @@ -308,7 +312,8 @@ def set_local_pose( Note: The pose should be in the OpenGL coordinate system, which means the Y is up and Z is forward. Args: - pose (torch.Tensor): The local pose to set, should be a 4x4 transformation matrix. + pose (torch.Tensor): The local pose as ``(N, 4, 4)`` matrices or + ``(N, 7)`` vectors in ``(x, y, z, qx, qy, qz, qw)`` order. env_ids (Sequence[int] | None): The environment IDs to set the pose for. If None, set for all environments. """ if env_ids is None: @@ -335,7 +340,8 @@ def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: """Get the local pose of the camera. Args: - to_matrix (bool): If True, return the pose as a 4x4 matrix. If False, return as a quaternion. + to_matrix (bool): If True, return the pose as a 4x4 matrix. If + False, return ``(x, y, z, qx, qy, qz, qw)``. Returns: torch.Tensor: The local pose of the camera. @@ -356,19 +362,16 @@ def get_arena_pose(self, to_matrix: bool = False) -> torch.Tensor: """Get the pose of the sensor in the arena frame. Args: - to_matrix (bool): If True, return the pose as a 4x4 transformation matrix. + to_matrix (bool): If True, return the pose as a 4x4 transformation + matrix. If False, return ``(x, y, z, qx, qy, qz, qw)``. Returns: A tensor representing the pose of the sensor in the arena frame. """ - from embodichain.lab.sim.utility import get_dexsim_arenas - - arenas = get_dexsim_arenas() - poses = [] for i, entity in enumerate(self._entities): pose = entity.get_world_pose() - pose[:2, 3] -= arenas[i].get_root_node().get_local_pose()[:2, 3] + pose[:2, 3] -= self._arenas[i].get_root_node().get_local_pose()[:2, 3] poses.append(torch.as_tensor(pose, dtype=torch.float32)) poses = torch.stack(poses, dim=0).to(self.device) @@ -378,6 +381,28 @@ def get_arena_pose(self, to_matrix: bool = False) -> torch.Tensor: return torch.cat((xyz, quat), dim=-1) return poses + def destroy(self) -> None: + """Remove render cameras before releasing their World-owned group.""" + if self._is_destroyed: + return + self._is_destroyed = True + for arena, camera_name in self._camera_names: + try: + arena.remove_camera(camera_name) + except Exception as error: + logger.log_warning( + f"Failed to remove camera {camera_name!r}: {error!r}" + ) + self._entities = [] + self._camera_names = [] + # DexSim currently has no public remove_camera_group API. The group is + # World-owned; dropping this borrowed facade after removing all views + # is the narrowest safe lifetime boundary available to EmbodiChain. + self._frame_buffer = None + self._is_attached = False + self._arenas = [] + self._world = None + def look_at( self, eye: torch.Tensor, diff --git a/embodichain/lab/sim/sensors/contact_sensor.py b/embodichain/lab/sim/sensors/contact_sensor.py index 6520864be..af315c8fc 100644 --- a/embodichain/lab/sim/sensors/contact_sensor.py +++ b/embodichain/lab/sim/sensors/contact_sensor.py @@ -16,19 +16,29 @@ from __future__ import annotations -import dexsim -import math -import torch import uuid +from typing import TYPE_CHECKING, Sequence + +import dexsim import numpy as np +import torch import warp as wp - -from typing import Union, Tuple, Sequence, List, Optional, Dict from tensordict import TensorDict from embodichain.lab.sim.sensors import BaseSensor, SensorCfg -from embodichain.utils import logger, configclass from embodichain.lab.sim.sensors._warp.contact import scatter_contact_data +from embodichain.utils import configclass, logger + +if TYPE_CHECKING: + from dexsim.scene import ContactActorInfo, ContactQuery, ContactQueryCapabilities + + from embodichain.lab.sim.sim_manager import SimulationManager + +__all__ = [ + "ArticulationContactFilterCfg", + "ContactSensor", + "ContactSensorCfg", +] @configclass @@ -39,10 +49,10 @@ class ContactSensorCfg(SensorCfg): collisions between rigid bodies and articulation links. """ - rigid_uid_list: List[str] = [] + rigid_uid_list: list[str] = [] """rigid body contact filter configs""" - articulation_cfg_list: List[ArticulationContactFilterCfg] = [] + articulation_cfg_list: list[ArticulationContactFilterCfg] = [] """articulation link contact filter configs""" filter_need_both_actor: bool = True @@ -65,12 +75,12 @@ class ArticulationContactFilterCfg: articulation_uid: str = "" """Articulation unique identifier.""" - link_name_list: List[str] = [] + link_name_list: list[str] = [] """link names in the articulation whose contacts need to be filtered.""" @classmethod def from_dict( - cls, init_dict: dict[str, str | List[str]] + cls, init_dict: dict[str, str | list[str]] ) -> "ArticulationContactFilterCfg": """Initialize the configuration from a dictionary. @@ -90,7 +100,12 @@ def from_dict( class ContactSensor(BaseSensor): - """Sensor to get contacts from rigid body and articulation links.""" + """Sensor to get contacts from rigid bodies and articulation links. + + The sensor preserves every backend-emitted contact row that passes the + backend's positive-impulse filter. Geometry-only Newton solvers retain all + candidate contact rows. + """ SUPPORTED_DATA_TYPES = [ "position", @@ -103,31 +118,37 @@ class ContactSensor(BaseSensor): ] def __init__( - self, config: ContactSensorCfg, device: torch.device = torch.device("cpu") + self, + config: ContactSensorCfg, + device: torch.device = torch.device("cpu"), + *, + owner: "SimulationManager | None" = None, ) -> None: - from embodichain.lab.sim import SimulationManager + if owner is None: + from embodichain.lab.sim.sim_manager import SimulationManager - self._sim = SimulationManager.get_instance() - """simulation manager reference""" + owner = SimulationManager.get_instance() + self._sim = owner self.item_user_ids: torch.Tensor | None = None - """Dexsim userid of the contact filter items.""" + """Backend-neutral actor IDs selected by the contact query.""" self.item_env_ids: torch.Tensor | None = None - """Environment ids of the contact filter items.""" + """Environment IDs of the selected contact actors.""" self.item_user_env_ids_map: torch.Tensor | None = None - """Map from dexsim userid to environment id.""" + """Compatibility map from contact actor ID to environment ID.""" - self._visualizer: Optional[dexsim.models.PointCloud] = None + self._visualizer: dexsim.models.PointCloud | None = None """contact point visualizer. Default to None""" self.device = device self.cfg = config + self._query: ContactQuery | None = None self._num_contacts_per_env: torch.Tensor | None = None """Number of contacts per environment.""" - super().__init__(config, device) + super().__init__(config, device, num_instances=owner.num_envs) @property def max_total_contacts(self) -> int: @@ -148,94 +169,76 @@ def total_current_contacts(self) -> int: Returns: int: Total number of contacts. """ - return self._num_contacts_per_env.sum().item() + assert self._num_contacts_per_env is not None + return int(self._num_contacts_per_env.sum().item()) - def _precompute_filter_ids(self, config: ContactSensorCfg): - self.item_user_ids = torch.tensor([], dtype=torch.int32, device=self.device) - self.item_env_ids = torch.tensor([], dtype=torch.int32, device=self.device) - self.item_user_env_ids_map = torch.tensor( - [], dtype=torch.int32, device=self.device - ) - for rigid_uid in config.rigid_uid_list: - rigid_object = self._sim.get_rigid_object(rigid_uid) - if rigid_object is None: - logger.log_warning( - f"Rigid body with uid '{rigid_uid}' not found in simulation." - ) - continue - self.item_user_ids = torch.cat( - (self.item_user_ids, rigid_object.get_user_ids()) - ) - env_ids = torch.tensor( - rigid_object._all_indices, dtype=torch.int32, device=self.device - ) - self.item_env_ids = torch.cat((self.item_env_ids, env_ids)) + @property + def contact_capabilities(self) -> "ContactQueryCapabilities": + """Capabilities reported by the active DexSim contact binding.""" + assert self._query is not None + return self._query.capabilities + + def _build_sensor_from_config( + self, + config: ContactSensorCfg, + device: torch.device, + ) -> None: + result = self._sim.spawn_result + if result is None: + raise RuntimeError("ContactSensor requires SimulationManager.prepare().") + + targets: list[object] = [] + for uid in config.rigid_uid_list: + try: + handles = self._sim._spawn_scene.handles(uid) + except KeyError as exc: + raise KeyError(f"Contact rigid-body UID not found: {uid!r}.") from exc + if not handles: + raise RuntimeError(f"Contact rigid body {uid!r} is not materialized.") + targets.extend(handles) for articulation_cfg in config.articulation_cfg_list: - articulation = self._sim.get_articulation(articulation_cfg.articulation_uid) - if articulation is None: - articulation = self._sim.get_robot(articulation_cfg.articulation_uid) - if articulation is None: - logger.log_warning( - f"Articulation with uid '{articulation_cfg.articulation_uid}' not found in simulation." - ) + uid = articulation_cfg.articulation_uid + try: + handles = self._sim._spawn_scene.handles(uid) + except KeyError as exc: + raise KeyError(f"Contact articulation UID not found: {uid!r}.") from exc + if not handles: + raise RuntimeError(f"Contact articulation {uid!r} is not materialized.") + if not articulation_cfg.link_name_list: + targets.extend(handles) continue - all_link_names = articulation.link_names - link_names = ( - all_link_names - if len(articulation_cfg.link_name_list) == 0 - else articulation_cfg.link_name_list - ) - for link_name in link_names: - if link_name not in all_link_names: - logger.log_warning( - f"Link {link_name} not found in articulation {articulation_cfg.uid}." + for handle in handles: + available = set(handle.get_link_names()) + missing = set(articulation_cfg.link_name_list) - available + if missing: + raise ValueError( + f"Contact articulation {uid!r} has no links: {sorted(missing)}." ) - continue - link_user_ids = articulation.get_user_ids(link_name).reshape(-1) - self.item_user_ids = torch.cat((self.item_user_ids, link_user_ids)) - env_ids = torch.tensor( - articulation._all_indices, dtype=torch.int32, device=self.device + targets.extend( + (handle, link_name) for link_name in articulation_cfg.link_name_list ) - self.item_env_ids = torch.cat((self.item_env_ids, env_ids)) - # build user_id to env_id map - max_user_id = int(self.item_user_ids.max().item()) - self.item_user_env_ids_map = torch.full( - size=(max_user_id + 1,), - fill_value=-1, - dtype=self.item_user_ids.dtype, - device=self.device, - ) - self.item_user_env_ids_map[self.item_user_ids] = self.item_env_ids - - def _build_sensor_from_config(self, config: ContactSensorCfg, device: torch.device): - self._precompute_filter_ids(config) - self._world: dexsim.World = dexsim.default_world() - self._ps = self._world.get_physics_scene() - world_config = dexsim.get_world_config() - self.is_use_gpu_physics = device.type == "cuda" and world_config.enable_gpu_sim - if self.is_use_gpu_physics: - self.contact_data_buffer = torch.zeros( - self.max_total_contacts, - 11, - dtype=torch.float32, - device=device, - ) - self.contact_user_ids_buffer = torch.zeros( - self.max_total_contacts, - 2, - dtype=torch.int32, - device=device, + + if not targets: + raise ValueError( + "ContactSensor requires at least one rigid or link target." ) - else: - self._ps.enable_contact_data_update_on_cpu(True) + + self._query = result.create_contact_query( + targets, + match="all" if config.filter_need_both_actor else "any", + capacity=self.max_total_contacts, + capacity_per_env=config.max_contacts_per_env, + device=device, + frame="arena", + ) + self._sync_filter_actor_metadata() num_envs = self.num_instances self._num_contacts_per_env = torch.zeros( num_envs, dtype=torch.int32, device=device ) - # TODO: We may pre-allocate the data buffer for contact data. self._data_buffer = TensorDict( { "position": torch.zeros( @@ -267,16 +270,28 @@ def _build_sensor_from_config(self, config: ContactSensorCfg, device: torch.devi batch_size=[num_envs, config.max_contacts_per_env], device=device, ) - """ - position: [num_envs, num_contacts, 3] tensor, contact position in arena frame - normal: [num_envs, num_contacts, 3] tensor, contact normal - friction: [num_envs, num_contacts, 3] tensor, contact friction. Currently this value is not accurate. - impulse: [num_envs, num_contacts] tensor, contact impulse - distance: [num_envs, num_contacts] tensor, contact distance - user_ids: [num_envs, num_contacts, 2] of int, contact user ids - , use rigid_object.get_user_id() and find which object it belongs to. - is_valid: [num_envs, num_contacts] bool tensor, indicating which contacts are valid - """ + + def _sync_filter_actor_metadata(self) -> None: + assert self._query is not None + actor_ids = self._query.selected_actor_ids + self.item_user_ids = torch.as_tensor( + actor_ids, dtype=torch.int32, device=self.device + ) + self.item_env_ids = torch.as_tensor( + [self._query.actor_info(actor_id).env_id for actor_id in actor_ids], + dtype=torch.int32, + device=self.device, + ) + self.item_user_env_ids_map = torch.full( + (max(actor_ids, default=-1) + 1,), + -1, + dtype=torch.int32, + device=self.device, + ) + if actor_ids: + self.item_user_env_ids_map[self.item_user_ids.to(torch.long)] = ( + self.item_env_ids + ) def update(self, **kwargs) -> None: """Update the sensor state based on the current simulation state. @@ -287,60 +302,29 @@ def update(self, **kwargs) -> None: **kwargs: Additional keyword arguments for sensor update. """ + assert self._query is not None and self._num_contacts_per_env is not None self._num_contacts_per_env.zero_() - # Reset is_valid buffer - self._data_buffer["is_valid"][:] = False - - if not self.is_use_gpu_physics: - contact_data_np, body_user_indices_np = self._ps.get_cpu_contact_buffer() - n_contact = contact_data_np.shape[0] - contact_data = torch.tensor( - contact_data_np, dtype=torch.float32, device=self.device - ) - body_user_indices = torch.tensor( - body_user_indices_np, dtype=torch.int32, device=self.device - ) - else: - n_contact = self._ps.gpu_fetch_contact_data( - self.contact_data_buffer, self.contact_user_ids_buffer - ) - contact_data = self.contact_data_buffer[:n_contact] - body_user_indices = self.contact_user_ids_buffer[:n_contact] + self._data_buffer["is_valid"].zero_() - if n_contact == 0: + contact_buffer = self._query.fetch() + self._sync_filter_actor_metadata() + if contact_buffer.count == 0: return - - filter0_mask = torch.isin(body_user_indices[:, 0], self.item_user_ids) - filter1_mask = torch.isin(body_user_indices[:, 1], self.item_user_ids) - if self.cfg.filter_need_both_actor: - filter_mask = torch.logical_and(filter0_mask, filter1_mask) - else: - filter_mask = torch.logical_or(filter0_mask, filter1_mask) - - if not filter_mask.any(): + env_ids = contact_buffer.env_ids[: contact_buffer.count] + valid = (env_ids >= 0) & (env_ids < self.num_instances) + if not bool(valid.any()): return + contact_data = contact_buffer.data[: contact_buffer.count][valid].contiguous() + actor_ids = contact_buffer.actor_ids[: contact_buffer.count][valid].contiguous() + env_ids = env_ids[valid].contiguous() - filtered_contact_data = contact_data[filter_mask] - filtered_user_ids = body_user_indices[filter_mask] - - # Get environment IDs for the filtered contacts - filtered_env_ids = self.item_user_env_ids_map[filtered_user_ids[:, 0]] - - # Subtract arena offsets from contact positions - contact_offsets = self._sim.arena_offsets[filtered_env_ids] - filtered_contact_data[:, 0:3] = ( - filtered_contact_data[:, 0:3] - contact_offsets - ) # minus arean offsets - - num_contacts = len(filtered_contact_data) - device = str(self.device) wp.launch( kernel=scatter_contact_data, - dim=num_contacts, + dim=contact_data.shape[0], inputs=[ - wp.from_torch(filtered_contact_data), - wp.from_torch(filtered_user_ids), - wp.from_torch(filtered_env_ids), + wp.from_torch(contact_data), + wp.from_torch(actor_ids), + wp.from_torch(env_ids), wp.from_torch(self._num_contacts_per_env), self.cfg.max_contacts_per_env, ], @@ -353,10 +337,10 @@ def update(self, **kwargs) -> None: wp.from_torch(self._data_buffer["user_ids"]), wp.from_torch(self._data_buffer["is_valid"]), ], - device="cuda:0" if device == "cuda" else device, + device=str(self.device), ) - def get_arena_pose(self, to_matrix: bool = False) -> torch.Tensor: + def get_arena_pose(self, to_matrix: bool = False) -> torch.Tensor | None: """Not used. Args: @@ -368,7 +352,7 @@ def get_arena_pose(self, to_matrix: bool = False) -> torch.Tensor: logger.log_error("`get_arena_pose` for contact sensor is not implemented yet.") return None - def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: + def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor | None: """Get the local pose of the camera. Args: @@ -396,40 +380,38 @@ def get_data(self) -> TensorDict: """Retrieve data from the sensor. Returns: - Dict:{ - "position": Tensor of float32 (num_envs, num_contacts, 3) representing the contact positions, - "normal": Tensor of float32 (num_envs, num_contacts, 3) representing the contact normals, - "friction": Tensor of float32 (num_envs, num_contacts, 3) representing the contact friction, - "impulse": Tensor of float32 (num_envs, num_contacts) representing the contact impulses, - "distance": Tensor of float32 (num_envs, num_contacts) representing the contact distances, - "user_ids": Tensor of int32 (num_envs, num_contacts, 2) representing contact user ids - , use rigid_object.get_user_id() and find which object it belongs to. - "is_valid": Tensor of bool (num_envs, num_contacts) indicating which contacts are valid. - } + Batched contact data. ``position`` is in the Arena frame; + ``normal`` points from actor 0 toward actor 1; ``friction`` and + ``impulse`` are impulses; ``distance`` is signed separation; + ``user_ids`` contains backend-neutral contact actor IDs; and + ``is_valid`` marks rows populated by the latest update. Values in + invalid rows are unspecified and may come from an earlier update. """ return self._data_buffer + def get_actor_info(self, actor_id: int) -> "ContactActorInfo": + """Resolve an ID from the ``user_ids`` field to its Spawn identity.""" + assert self._query is not None + return self._query.actor_info(actor_id) + def filter_by_user_ids( self, item_user_ids: torch.Tensor, env_ids: Sequence[int] | None = None ) -> TensorDict: - """Filter contact report by specific user IDs. + """Filter contact report by backend-neutral contact actor IDs. Args: - item_user_ids (torch.Tensor): Tensor of user IDs to filter by. - env_ids (Sequence[int] | None): Environment IDs to filter. If None, filter all environments. + item_user_ids: Actor IDs from this sensor's ``user_ids`` field. + env_ids: Environment IDs to filter. If None, filter all environments. Returns: data: A TensorDict containing only the filtered contacts for the specified environments. """ - if env_ids is None: - env_ids = range(self.num_instances) - - # Vectorized filtering across all specified environments env_ids_tensor = ( - torch.tensor(env_ids, device=self.device) - if isinstance(env_ids, list) - else env_ids + torch.arange(self.num_instances, device=self.device) + if env_ids is None + else torch.as_tensor(list(env_ids), dtype=torch.long, device=self.device) ) + item_user_ids = item_user_ids.to(device=self.device, dtype=torch.int32) # Flatten data across all specified environments env_data = { @@ -465,7 +447,7 @@ def filter_by_user_ids( # Combine valid and user ID filters combined_mask = torch.logical_and(valid_mask, filter_mask) - if not combined_mask.any(): + if not bool(combined_mask.any()): # Return empty TensorDict if no matches return TensorDict( { @@ -495,10 +477,10 @@ def filter_by_user_ids( def set_contact_point_visibility( self, visible: bool = True, - rgba: Optional[Sequence[int]] = None, + rgba: Sequence[float] | None = None, point_size: float = 3.0, env_ids: Sequence[int] | None = None, - ): + ) -> None: if env_ids is None: env_ids = range(self.num_instances) diff --git a/embodichain/lab/sim/sensors/stereo.py b/embodichain/lab/sim/sensors/stereo.py index 2475a3f9e..34b53bfce 100644 --- a/embodichain/lab/sim/sensors/stereo.py +++ b/embodichain/lab/sim/sensors/stereo.py @@ -21,13 +21,16 @@ import numpy as np import dexsim.render as dr -from typing import Dict, Tuple, List, Sequence +from typing import TYPE_CHECKING, Dict, Tuple, List, Sequence from dexsim.utility import inv_transform from embodichain.lab.sim.sensors import Camera, CameraCfg from embodichain.utils.math import matrix_from_euler from embodichain.utils import logger, configclass +if TYPE_CHECKING: + from embodichain.lab.sim.sim_manager import SimulationManager + @configclass class StereoCameraCfg(CameraCfg): @@ -155,8 +158,14 @@ def __init__( self, config: StereoCameraCfg, device: torch.device = torch.device("cpu"), + *, + owner: SimulationManager, ) -> None: - super().__init__(config, device) + super().__init__( + config, + device, + owner=owner, + ) # check valid config if self.cfg.enable_disparity and not self.cfg.enable_depth: @@ -165,21 +174,14 @@ def __init__( def _build_sensor_from_config( self, config: StereoCameraCfg, device: torch.device ) -> None: - self._world = dexsim.default_world() - env = self._world.get_env() - arenas = env.get_all_arenas() - if len(arenas) == 0: - arenas = [env] - num_instances = len(arenas) - self._frame_buffer = self._world.create_camera_group( - [config.width, config.height], num_instances * 2, True + [config.width, config.height], self.num_instances * 2, True ) view_attrib = config.get_view_attrib() left_list = [] right_list = [] - for i, arena in enumerate(arenas): - left_view_name = f"{self.uid}_left_view{i + 1}" + for i, arena in enumerate(self._arenas): + left_view_name = f"{config.uid}_left_view{i + 1}" left_view = arena.create_camera( left_view_name, config.width, @@ -192,9 +194,10 @@ def _build_sensor_from_config( left_view.set_near(config.near) left_view.set_far(config.far) left_list.append(left_view) + self._camera_names.append((arena, left_view_name)) - for i, arena in enumerate(arenas): - right_view_name = f"{self.uid}_right_view{i + 1}" + for i, arena in enumerate(self._arenas): + right_view_name = f"{config.uid}_right_view{i + 1}" right_view = arena.create_camera( right_view_name, config.width, @@ -207,8 +210,9 @@ def _build_sensor_from_config( right_view.set_near(config.near) right_view.set_far(config.far) right_list.append(right_view) + self._camera_names.append((arena, right_view_name)) - for i in range(num_instances): + for i in range(self.num_instances): self._entities[i] = PairCameraView( left_list[i], right_list[i], config.left_to_right.cpu().numpy() ) @@ -341,14 +345,10 @@ def get_left_right_arena_pose(self) -> torch.Tensor: Returns: torch.Tensor: The local pose of the left camera with shape (num_envs, 4, 4). """ - from embodichain.lab.sim.utility import get_dexsim_arenas - - arenas = get_dexsim_arenas() - left_poses = [] right_poses = [] for i, entity in enumerate(self._entities): - arena_pose = arenas[i].get_root_node().get_local_pose() + arena_pose = self._arenas[i].get_root_node().get_local_pose() left_pose = entity._left_view.get_world_pose() left_pose[:2, 3] -= arena_pose[:2, 3] left_poses.append( diff --git a/embodichain/lab/sim/shapes.py b/embodichain/lab/sim/shapes.py index 34eb437e4..b9f55a0c9 100755 --- a/embodichain/lab/sim/shapes.py +++ b/embodichain/lab/sim/shapes.py @@ -16,13 +16,280 @@ from __future__ import annotations -from typing import List, Dict, Union, TYPE_CHECKING, Any +import math +import warnings from dataclasses import MISSING +from numbers import Integral +from typing import Any, Dict, List, Literal, Sequence, TYPE_CHECKING + +import numpy as np + from embodichain.utils import configclass, is_configclass, logger if TYPE_CHECKING: from embodichain.lab.sim.material import VisualMaterialCfg +__all__ = [ + "MeshCollisionApproximation", + "MeshCollisionCfg", + "LoadOption", + "ShapeCfg", + "MeshCfg", + "CubeCfg", + "SphereCfg", +] + + +MeshCollisionApproximation = Literal[ + "convex_hull", + "convex_decomposition", + "triangle_mesh", + "sdf", +] +"""Supported collision representations for a triangle mesh.""" + + +@configclass +class MeshCollisionCfg: + """Collision-geometry construction for :class:`MeshCfg`. + + The approximation is explicit. Strategy-specific fields are rejected when + they do not apply, so changing a numerical cooking value cannot silently + select a different collision representation. + """ + + approximation: MeshCollisionApproximation = "convex_hull" + """Collision representation built from the source triangle mesh.""" + + max_hulls: int | None = None + """Maximum hull count for ``convex_decomposition``; must be at least two.""" + + acd_method: Literal["coacd", "vhacd"] | None = None + """Approximate-convex-decomposition implementation.""" + + sdf_resolution: int | None = None + """Maximum SDF grid resolution; valid only for the ``sdf`` strategy.""" + + is_hydroelastic: bool | None = None + """Whether Newton uses the generated SDF for hydroelastic contact.""" + + sdf_narrow_band_range: tuple[float, float] | None = None + """Inner and outer signed-distance limits of the Newton SDF band [m].""" + + sdf_target_voxel_size: float | None = None + """Target Newton sparse-SDF voxel size [m], alternative to resolution.""" + + sdf_texture_format: Literal["uint16", "float32", "uint8"] | None = None + """Newton SDF voxel storage format.""" + + sdf_padding: float | None = None + """Extra padding used while Newton builds the mesh SDF [m].""" + + @property + def max_convex_hull_num(self) -> int: + """Deprecated compatibility view of :attr:`max_hulls`.""" + return self.max_hulls or 1 + + def __post_init__(self) -> None: + """Validate strategy-specific mesh-cooking fields.""" + supported = { + "convex_hull", + "convex_decomposition", + "triangle_mesh", + "sdf", + } + if self.approximation not in supported: + raise ValueError( + "MeshCollisionCfg.approximation must be one of " + f"{sorted(supported)}, got {self.approximation!r}." + ) + + if self.approximation == "convex_decomposition": + if ( + not isinstance(self.max_hulls, Integral) + or isinstance(self.max_hulls, bool) + or self.max_hulls < 2 + ): + raise ValueError( + "convex_decomposition requires max_hulls to be an integer " + "of at least 2." + ) + if self.acd_method not in (None, "coacd", "vhacd"): + raise ValueError("acd_method must be 'coacd' or 'vhacd'.") + elif self.max_hulls is not None or self.acd_method is not None: + raise ValueError( + "max_hulls and acd_method are valid only for convex_decomposition." + ) + + sdf_values = { + "sdf_resolution": self.sdf_resolution, + "is_hydroelastic": self.is_hydroelastic, + "sdf_narrow_band_range": self.sdf_narrow_band_range, + "sdf_target_voxel_size": self.sdf_target_voxel_size, + "sdf_texture_format": self.sdf_texture_format, + "sdf_padding": self.sdf_padding, + } + configured_sdf_fields = [ + name for name, value in sdf_values.items() if value is not None + ] + if self.approximation != "sdf" and configured_sdf_fields: + raise ValueError( + f"{configured_sdf_fields} are valid only for the sdf approximation." + ) + if self.sdf_resolution is not None and ( + not isinstance(self.sdf_resolution, Integral) + or isinstance(self.sdf_resolution, bool) + or self.sdf_resolution <= 0 + ): + raise ValueError("sdf_resolution must be a positive integer.") + if self.sdf_target_voxel_size is not None and ( + not math.isfinite(self.sdf_target_voxel_size) + or self.sdf_target_voxel_size <= 0.0 + ): + raise ValueError("sdf_target_voxel_size must be finite and positive.") + if self.sdf_resolution is not None and self.sdf_target_voxel_size is not None: + raise ValueError( + "Configure only one of sdf_resolution and sdf_target_voxel_size." + ) + if self.sdf_padding is not None and ( + not math.isfinite(self.sdf_padding) or self.sdf_padding < 0.0 + ): + raise ValueError("sdf_padding must be finite and non-negative.") + if self.is_hydroelastic is not None and not isinstance( + self.is_hydroelastic, bool + ): + raise TypeError("is_hydroelastic must be a boolean when configured.") + if self.sdf_texture_format not in (None, "uint16", "float32", "uint8"): + raise ValueError( + "sdf_texture_format must be 'uint16', 'float32', or 'uint8'." + ) + if self.sdf_narrow_band_range is not None: + if len(self.sdf_narrow_band_range) != 2: + raise ValueError("sdf_narrow_band_range must contain two values.") + inner, outer = (float(value) for value in self.sdf_narrow_band_range) + if not math.isfinite(inner) or not math.isfinite(outer): + raise ValueError("sdf_narrow_band_range values must be finite.") + if inner > outer: + raise ValueError( + "sdf_narrow_band_range inner value cannot exceed the outer value." + ) + + @classmethod + def from_dict(cls, init_dict: Dict[str, Any]) -> MeshCollisionCfg: + """Parse a mesh-collision mapping, including deprecated field names.""" + data = dict(init_dict) + legacy_fields = { + "max_convex_hull_num", + "force_sdf", + "sdf_max_resolution", + } + has_legacy_fields = bool(legacy_fields.intersection(data)) + if has_legacy_fields: + warnings.warn( + "Legacy mesh collision fields are deprecated; use an explicit " + "approximation with max_hulls or sdf_resolution.", + DeprecationWarning, + stacklevel=2, + ) + + legacy_max_hulls = data.pop("max_convex_hull_num", None) + legacy_force_sdf = data.pop("force_sdf", None) + legacy_sdf_resolution = data.pop("sdf_max_resolution", None) + if legacy_sdf_resolution is not None: + if "sdf_resolution" in data: + raise ValueError( + "sdf_max_resolution and sdf_resolution cannot both be configured." + ) + data["sdf_resolution"] = legacy_sdf_resolution + + if "approximation" not in data and has_legacy_fields: + sdf_requested = bool(legacy_force_sdf) or ( + data.get("sdf_resolution") is not None + and int(data["sdf_resolution"]) > 0 + ) + if sdf_requested: + data["approximation"] = "sdf" + data.pop("max_hulls", None) + data.pop("acd_method", None) + elif legacy_max_hulls is not None and int(legacy_max_hulls) > 1: + data["approximation"] = "convex_decomposition" + data["max_hulls"] = int(legacy_max_hulls) + else: + data["approximation"] = "convex_hull" + data.pop("acd_method", None) + elif legacy_max_hulls is not None: + if "max_hulls" in data: + raise ValueError( + "max_convex_hull_num and max_hulls cannot both be configured." + ) + data["max_hulls"] = int(legacy_max_hulls) + + if data.get("sdf_resolution") == 0: + data.pop("sdf_resolution") + return cls(**data) + + +_mesh_collision_cfg_init = MeshCollisionCfg.__init__ + + +def _mesh_collision_cfg_init_with_legacy_max_hulls( + self: MeshCollisionCfg, + approximation: MeshCollisionApproximation | None = None, + max_hulls: int | None = None, + acd_method: Literal["coacd", "vhacd"] | None = None, + sdf_resolution: int | None = None, + is_hydroelastic: bool | None = None, + sdf_narrow_band_range: tuple[float, float] | None = None, + sdf_target_voxel_size: float | None = None, + sdf_texture_format: Literal["uint16", "float32", "uint8"] | None = None, + sdf_padding: float | None = None, + *, + max_convex_hull_num: int | None = None, +) -> None: + """Initialize with the deprecated hull-count spelling at the API boundary.""" + if max_convex_hull_num is not None: + warnings.warn( + "max_convex_hull_num is deprecated; use max_hulls with an explicit " + "approximation.", + DeprecationWarning, + stacklevel=2, + ) + if max_hulls is not None: + raise ValueError( + "max_convex_hull_num and max_hulls cannot both be configured." + ) + if ( + not isinstance(max_convex_hull_num, Integral) + or isinstance(max_convex_hull_num, bool) + or max_convex_hull_num < 1 + ): + raise ValueError("max_convex_hull_num must be a positive integer.") + if approximation is None: + approximation = ( + "convex_decomposition" if max_convex_hull_num > 1 else "convex_hull" + ) + max_hulls = ( + None + if approximation == "convex_hull" and max_convex_hull_num == 1 + else max_convex_hull_num + ) + + _mesh_collision_cfg_init( + self, + approximation="convex_hull" if approximation is None else approximation, + max_hulls=max_hulls, + acd_method=acd_method, + sdf_resolution=sdf_resolution, + is_hydroelastic=is_hydroelastic, + sdf_narrow_band_range=sdf_narrow_band_range, + sdf_target_voxel_size=sdf_target_voxel_size, + sdf_texture_format=sdf_texture_format, + sdf_padding=sdf_padding, + ) + + +MeshCollisionCfg.__init__ = _mesh_collision_cfg_init_with_legacy_max_hulls + @configclass class LoadOption: @@ -70,13 +337,35 @@ def from_dict(cls, init_dict: Dict[str, Any]) -> ShapeCfg: """Initialize the configuration from a dictionary.""" from embodichain.utils.utility import get_class_instance - if "shape_type" not in init_dict: + data = dict(init_dict) + if "shape_type" not in data: logger.log_error("shape type must be specified in the configuration.") cfg = get_class_instance( - "embodichain.lab.sim.shapes", init_dict["shape_type"] + "Cfg" + "embodichain.lab.sim.shapes", data["shape_type"] + "Cfg" )() - for key, value in init_dict.items(): + legacy_mesh_fields = { + "max_convex_hull_num", + "acd_method", + "sdf_resolution", + } + if isinstance(cfg, MeshCfg): + configured_legacy = legacy_mesh_fields.intersection(data) + if configured_legacy: + if data.get("collision") is not None: + raise ValueError( + "MeshCfg collision cannot be combined with deprecated flat " + f"mesh fields {sorted(configured_legacy)}." + ) + legacy_collision = { + key: data.pop(key) for key in tuple(configured_legacy) + } + # Route through the legacy normalizer. Presence of this old hull + # name also makes the deprecation warning deterministic. + legacy_collision.setdefault("max_convex_hull_num", 1) + data["collision"] = legacy_collision + + for key, value in data.items(): if hasattr(cfg, key): attr = getattr(cfg, key) if key == "visual_material" and isinstance(value, dict): @@ -87,6 +376,15 @@ def from_dict(cls, init_dict: Dict[str, Any]) -> ShapeCfg: key, VisualMaterialCfg.from_dict(value), ) + elif key == "collision" and isinstance(cfg, MeshCfg): + if value is not None and not isinstance(value, MeshCollisionCfg): + if not isinstance(value, dict): + raise TypeError( + "MeshCfg.collision must be a mapping, " + "MeshCollisionCfg, or None." + ) + value = MeshCollisionCfg.from_dict(value) + setattr(cfg, key, value) elif is_configclass(attr): setattr(cfg, key, attr.from_dict(value)) else: @@ -104,8 +402,27 @@ class MeshCfg(ShapeCfg): shape_type: str = "Mesh" - fpath: str = MISSING - """File path to the shape mesh file.""" + fpath: str | None = None + """File path to the shape mesh file. + + Provide either this path or both :attr:`vertices` and :attr:`triangles`. + """ + + vertices: Sequence[Sequence[float]] | np.ndarray | None = None + """Optional array-backed mesh vertices with shape ``(N, 3)``. + + Array-backed meshes preserve vertex order, which is useful when per-node + deformable flags or kinematic trajectories refer to stable node indices. + """ + + triangles: Sequence[Sequence[int]] | np.ndarray | None = None + """Optional array-backed triangle indices with shape ``(M, 3)``.""" + + normals: Sequence[Sequence[float]] | np.ndarray | None = None + """Optional per-vertex normals with shape ``(N, 3)``.""" + + uv_coords: Sequence[Sequence[float]] | np.ndarray | None = None + """Optional per-vertex texture coordinates with shape ``(N, 2)``.""" load_option: LoadOption = LoadOption() """Options for loading and processing the shape.""" @@ -119,28 +436,12 @@ class MeshCfg(ShapeCfg): project_direction: List[float] = [1.0, 1.0, 1.0] """Direction to project the UV coordinates. Defaults to [1.0, 1.0, 1.0].""" - max_convex_hull_num: int = 1 - """The maximum number of convex hulls that will be created for the mesh. - - If set to larger than 1, the mesh will be decomposed into multiple convex hulls - using the approximate convex decomposition method specified by :attr:`acd_method`. - """ - - acd_method: str = "visacd" - """The method used for approximate convex decomposition (ACD) of the mesh. - - Defaults to ``"visacd"``. ``"visacd"``, ``"coacd"``, and ``"vhacd"`` are - supported. Only used when :attr:`max_convex_hull_num` is set to larger than - 1. ``"visacd"`` requires CUDA support. - """ - - sdf_resolution: int = 0 - """Resolution for the signed distance field (SDF) of the mesh. + collision: MeshCollisionCfg | None = None + """Optional collision representation and cooking parameters. - The spacing of the uniformly sampled SDF is equal to the largest AABB extent - of the mesh, divided by the resolution. If ``sdf_resolution`` is set to larger - than 0, an SDF will be generated for collision detection. SDF increases the - accuracy of collision, but also takes more time to initialize and simulate. + ``None`` uses a single convex hull. Mesh collision construction belongs to + the geometry because it cannot be applied meaningfully to primitive shapes + or to articulation links without a named source-shape overlay. """ diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 1bc8fdd9d..f5d558bfe 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -22,17 +22,28 @@ import queue import time import threading +from contextlib import contextmanager import dexsim import torch import numpy as np import warp as wp -from tqdm import tqdm from pathlib import Path from copy import deepcopy from datetime import datetime -from functools import cached_property -from typing import TYPE_CHECKING, Callable, Dict, List, Sequence, Union +from functools import cached_property, partial +from typing import ( + TYPE_CHECKING, + Any, + Callable, + Dict, + Iterator, + List, + Literal, + Mapping, + Sequence, + Union, +) from dataclasses import dataclass, asdict, field, MISSING # Global cache directories @@ -41,26 +52,28 @@ CONVEX_DECOMP_DIR = SIM_CACHE_DIR / "convex_decomposition" REACHABLE_XPOS_DIR = SIM_CACHE_DIR / "robot_reachable_xpos" + +def _is_usd_path(path: object | None) -> bool: + """Return whether a source path is a USD stage.""" + return path is not None and str(path).lower().endswith((".usd", ".usda", ".usdc")) + + from dexsim.types import ( + ActorType, Backend, ThreadMode, - PhysicalAttr, - ActorType, - RigidBodyShape, - RigidBodyGPUAPIReadType, - ArticulationGPUAPIReadType, ) from dexsim.core import TASK_RETURN -from dexsim.engine import CudaArray, Material +from dexsim.engine import Material, ObjectManipulator from dexsim.models import MeshObject -from dexsim.render import Light as _Light, LightType, Windows -from dexsim.engine import ObjectManipulator +from dexsim.render import LightType, Windows from embodichain.lab.sim.objects import ( RigidObject, RigidObjectGroup, - SoftObject, - ClothObject, + DeformableObject, + SurfaceDeformableObject, + VolumeDeformableObject, Articulation, Robot, Light, @@ -75,30 +88,55 @@ StereoCamera, ContactSensor, ) -from embodichain.lab.sim.sensors.attachment import resolve_parent_nodes from embodichain.lab.sim.cfg import ( RenderCfg, - PhysicsCfg, - MarkerCfg, + PhysicsBackendCfg, GPUMemoryCfg, + DefaultPhysicsCfg, + NewtonPhysicsCfg, + validate_physics_cfg, + MarkerCfg, WindowRecordCfg, WindowCameraPoseCfg, LightCfg, RigidObjectCfg, - SoftObjectCfg, - ClothObjectCfg, + DeformableObjectCfg, + SurfaceDeformableObjectCfg, + VolumeDeformableObjectCfg, RigidObjectGroupCfg, ArticulationCfg, + ArticulationRootPropertiesCfg, RobotCfg, + RobotPresetCfg, RigidConstraintCfg, ) +from embodichain.lab.sim.physics import make_physics_backend +from embodichain.lab.sim.spawn.descriptors import ( + articulation_desc_from_cfg, + configure_articulation_desc, + rigid_desc_from_cfg, + surface_deformable_desc_from_cfg, + volume_deformable_desc_from_cfg, +) +from embodichain.lab.sim.spawn.usd import ( + articulation_desc_from_usd, + rigid_desc_from_usd, +) +from embodichain.lab.sim.spawn.scene import SpawnScene from embodichain.lab.sim import VisualMaterial, VisualMaterialCfg from embodichain.lab.sim.profiler import Profiler, ProfilerCfg from embodichain.lab.visualization.cfg import VisualizationCfg from embodichain.utils import configclass, logger -from embodichain.utils.math import look_at_to_pose, matrix_from_quat, pose_inv +from embodichain.utils.math import ( + convert_quat, + look_at_to_pose, + matrix_from_quat, + pose_inv, +) if TYPE_CHECKING: + from dexsim.engine import PhysicsScene + from dexsim.scene import Scene from dexsim.interaction import EntityGizmoConfig, EntityGizmoManipulator from embodichain.lab.visualization import ( RuntimeHealth, @@ -111,6 +149,7 @@ __all__ = [ "SimulationManager", "SimulationManagerCfg", + "get_physics_scene", "SIM_CACHE_DIR", "MATERIAL_CACHE_DIR", "CONVEX_DECOMP_DIR", @@ -118,13 +157,145 @@ ] +@contextmanager +def _temporary_warp_kernel_log_suppression( + physics_cfg: PhysicsBackendCfg, +) -> Iterator[None]: + """Temporarily suppress informational Warp logs for Newton operations.""" + if not ( + isinstance(physics_cfg, NewtonPhysicsCfg) + and physics_cfg.suppress_warp_kernel_logs + ): + yield + return + + previous_log_level = wp.config.log_level + try: + # Warp emits its startup banner and module-load timers at INFO level. + # Keep warnings and errors visible. + wp.config.log_level = wp.LOG_WARNING + yield + finally: + wp.config.log_level = previous_log_level + + +def _initialize_warp_runtime(physics_cfg: PhysicsBackendCfg) -> None: + """Initialize Warp while honoring Newton startup-log suppression.""" + with _temporary_warp_kernel_log_suppression(physics_cfg): + wp.init() + + +# Deformable objects are Newton particle sets. The Default implementation is +# deliberately absent so stale soft/cloth configurations fail at declaration. +_DEFORMABLE_BACKEND_IMPLEMENTATIONS = { + "default": {}, + "newton": { + "volume": ( + VolumeDeformableObjectCfg, + VolumeDeformableObject, + volume_deformable_desc_from_cfg, + "soft_object", + ), + "surface": ( + SurfaceDeformableObjectCfg, + SurfaceDeformableObject, + surface_deformable_desc_from_cfg, + "cloth_object", + ), + }, +} + + @configclass class SimulationManagerCfg: """Global robot simulation configuration.""" + def __init__( + self, + width: int = 1920, + height: int = 1080, + headless: bool = False, + render_cfg: RenderCfg | None = None, + gpu_id: int = 0, + thread_mode: ThreadMode = ThreadMode.RENDER_SHARE_ENGINE, + cpu_num: int = 1, + num_envs: int = 1, + arena_space: float = 5.0, + enable_entity_gizmo: bool = True, + robot_ik_gizmo: GizmoCfg | dict | None = GizmoCfg(), + physics_dt: float | None = None, + device: str | torch.device | None = None, + physics_cfg: PhysicsBackendCfg | None = None, + sim_device: str | torch.device | None = None, + physics_config: PhysicsBackendCfg | None = None, + gpu_memory_config: GPUMemoryCfg | None = None, + profiler: ProfilerCfg | None = None, + visualization: VisualizationCfg | None = None, + window_record: WindowRecordCfg | None = None, + window_camera_pose: WindowCameraPoseCfg | None = None, + startup_summary: Literal["compact", "full", "off"] = "compact", + dexsim_startup_info: bool = False, + ) -> None: + self.startup_summary = startup_summary + self.dexsim_startup_info = dexsim_startup_info + self.width = width + self.height = height + self.headless = headless + self.render_cfg = RenderCfg() if render_cfg is None else render_cfg + self.gpu_id = gpu_id + self.thread_mode = thread_mode + self.cpu_num = cpu_num + self.num_envs = num_envs + self.arena_space = arena_space + self.enable_entity_gizmo = enable_entity_gizmo + self.robot_ik_gizmo = deepcopy(robot_ik_gizmo) + if physics_cfg is None: + physics_cfg = ( + DefaultPhysicsCfg() if physics_config is None else physics_config + ) + self.physics_cfg = physics_cfg + if gpu_memory_config is not None: + if not isinstance(self.physics_cfg, DefaultPhysicsCfg): + logger.log_error( + "gpu_memory_config is only supported by the default physics backend.", + ValueError, + ) + self.physics_cfg.gpu_memory = gpu_memory_config + self.profiler = profiler + self.visualization = ( + VisualizationCfg() if visualization is None else visualization + ) + self.window_record = ( + WindowRecordCfg() if window_record is None else window_record + ) + self.window_camera_pose = ( + WindowCameraPoseCfg() if window_camera_pose is None else window_camera_pose + ) + if physics_dt is not None: + self.physics_cfg.physics_dt = physics_dt + # ``None`` is an omission sentinel, not a request for the generic + # PhysicsBackendCfg default. Leave the concrete config untouched so + # NewtonPhysicsCfg's CUDA default remains authoritative. A non-None + # value is an intentional runtime override and is applied uniformly. + runtime_device = device if device is not None else sim_device + if runtime_device is not None: + self.physics_cfg.device = runtime_device + + self.__post_init__() + width: int = 1920 """The width of the simulation window.""" + startup_summary: Literal["compact", "full", "off"] = "compact" + """Startup table detail. ``off`` disables both simulation and Gym summaries.""" + + dexsim_startup_info: bool = False + """Show DexSim's native startup information in addition to our summary. + + Warnings and errors remain visible regardless of this setting. Requires + a DexSim build exposing ``WorldConfig.log_startup_info``. + """ + height: int = 1080 """The height of the simulation window.""" @@ -182,8 +353,13 @@ class SimulationManagerCfg: arena_space: float = 5.0 """The distance between each arena when building multiple arenas.""" - physics_dt: float = 1.0 / 100.0 - """The time step for the physics simulation.""" + physics_cfg: PhysicsBackendCfg = field(default_factory=DefaultPhysicsCfg) + """Physics backend configuration (type selects default vs Newton backend). + + The concrete config owns the default device: Default uses ``cpu`` and + Newton uses ``cuda:0``. The constructor's optional ``device``/``sim_device`` + arguments are applied only when explicitly provided. + """ profiler: ProfilerCfg | None = None """Optional simulation profiler. ``None`` disables profiling. @@ -193,14 +369,6 @@ class SimulationManagerCfg: profiler instance composes with the environment's step/reset hierarchy. """ - sim_device: Union[str, torch.device] = "cpu" - """The device for the physics simulation. Can be 'cpu', 'cuda', or a torch.device object.""" - - physics_config: PhysicsCfg = field(default_factory=PhysicsCfg) - """The physics configuration parameters.""" - gpu_memory_config: GPUMemoryCfg = field(default_factory=GPUMemoryCfg) - """The GPU memory configuration parameters.""" - window_record: WindowRecordCfg = field(default_factory=WindowRecordCfg) """Viewer window recording settings (hotkey, paths, FPS, memory budget).""" @@ -211,7 +379,10 @@ class SimulationManagerCfg: """Live browser visualization settings.""" def __post_init__(self) -> None: - """Apply visualization-dependent simulation defaults.""" + """Validate physics and apply visualization-dependent defaults.""" + if self.startup_summary not in ("compact", "full", "off"): + raise ValueError("startup_summary must be 'compact', 'full', or 'off'.") + validate_physics_cfg(self.physics_cfg) if isinstance(self.robot_ik_gizmo, dict): self.robot_ik_gizmo = GizmoCfg(**self.robot_ik_gizmo) if self.robot_ik_gizmo is not None and not isinstance( @@ -221,6 +392,58 @@ def __post_init__(self) -> None: if self.visualization.backend == "viser": self.headless = True + @property + def physics_dt(self) -> float: + """The time step for the physics simulation.""" + return self.physics_cfg.physics_dt + + @physics_dt.setter + def physics_dt(self, value: float) -> None: + self.physics_cfg.physics_dt = value + + @property + def device(self) -> str | torch.device: + """The device for the physics simulation.""" + return self.physics_cfg.device + + @device.setter + def device(self, value: str | torch.device) -> None: + self.physics_cfg.device = value + + @property + def sim_device(self) -> str | torch.device: + """Legacy alias for :attr:`device`.""" + return self.device + + @sim_device.setter + def sim_device(self, value: str | torch.device) -> None: + self.device = value + + @property + def physics_config(self) -> PhysicsBackendCfg: + """Legacy alias for :attr:`physics_cfg`.""" + return self.physics_cfg + + @physics_config.setter + def physics_config(self, value: PhysicsBackendCfg) -> None: + validate_physics_cfg(value) + self.physics_cfg = value + + @property + def gpu_memory_config(self) -> GPUMemoryCfg | None: + """Legacy alias for the default backend GPU-memory configuration.""" + if not isinstance(self.physics_cfg, DefaultPhysicsCfg): + return None + return self.physics_cfg.gpu_memory + + @gpu_memory_config.setter + def gpu_memory_config(self, value: GPUMemoryCfg) -> None: + if not isinstance(self.physics_cfg, DefaultPhysicsCfg): + raise AttributeError( + "gpu_memory_config is unavailable for the Newton physics backend." + ) + self.physics_cfg.gpu_memory = value + @dataclass class _WindowRecordState: @@ -267,6 +490,8 @@ class SimulationManager: Args: sim_config (SimulationManagerCfg, optional): simulation configuration. Defaults to SimulationManagerCfg(). + defer_startup_summary: Let an owning environment emit one combined table + after task and manager initialization. Standalone callers keep False. """ _instances = {} @@ -280,7 +505,12 @@ class SimulationManager: "ContactSensor": ContactSensor, } - def __new__(cls, sim_config: SimulationManagerCfg = SimulationManagerCfg()): + def __new__( + cls, + sim_config: SimulationManagerCfg = SimulationManagerCfg(), + *, + defer_startup_summary: bool = False, + ): """Create or return the instance based on instance_id.""" n_instance = len(list(cls._instances.keys())) instance = super(SimulationManager, cls).__new__(cls) @@ -291,8 +521,21 @@ def __new__(cls, sim_config: SimulationManagerCfg = SimulationManagerCfg()): return instance def __init__( - self, sim_config: SimulationManagerCfg = SimulationManagerCfg() + self, + sim_config: SimulationManagerCfg = SimulationManagerCfg(), + *, + defer_startup_summary: bool = False, ) -> None: + self._defer_startup_summary = defer_startup_summary + self._startup_summary_logged = False + self._scene_summary_logged = False + self._requested_renderer = sim_config.render_cfg.renderer + solver_cfg = getattr(sim_config.physics_cfg, "solver_cfg", None) + self._requested_solver = str( + solver_cfg.get("solver_type", solver_cfg.get("class_type", "auto")) + if isinstance(solver_cfg, Mapping) + else getattr(solver_cfg, "solver_type", "auto") + ) instance_id = SimulationManager.get_instance_num() - 1 # Mark as initialized @@ -316,11 +559,28 @@ def __init__( self.sim_config = sim_config self.device = torch.device("cpu") + # Initialize physics backend (selected by the type of physics_cfg). + # The backend is held as an instance member; SimulationManager delegates + # all backend-specific lifecycle/scene/capability logic to it instead of + # branching on a backend name throughout the manager. + self.physics = make_physics_backend(sim_config.physics_cfg, self) + world_config = self._convert_sim_config(sim_config) + self._world_config = world_config self.profiler = Profiler(sim_config.profiler, self.device) - # Initialize warp runtime context before creating the world. - wp.init() + # Initialize Warp before creating the world. For Newton, honor the + # configured startup/kernel-log suppression from the very first init. + _initialize_warp_runtime(sim_config.physics_cfg) + self._render_device_name: str | None = None + if sim_config.startup_summary != "off": + # Warp already enumerated these devices during initialization. Read + # its cached names instead of lazily initializing PyTorch CUDA just + # to print diagnostics for CPU-physics/explicit-renderer programs. + for render_device in wp.get_cuda_devices(): + if render_device.ordinal == sim_config.gpu_id: + self._render_device_name = render_device.name + break self._world: dexsim.World = dexsim.World(world_config) # The caller owns physics time, including while the scene is assembled. self._world.set_manual_update(True) @@ -350,14 +610,11 @@ def __init__( ) self._window_camera_pose_input_control: ObjectManipulator | None = None - self._world.set_delta_time(sim_config.physics_dt) + self._world.set_delta_time(sim_config.physics_cfg.physics_dt) self._world.show_coordinate_axis(False) - dexsim.set_physics_config(**sim_config.physics_config.to_dexsim_args()) - dexsim.set_physics_gpu_memory_config(**sim_config.gpu_memory_config.to_dict()) - - self._is_initialized_gpu_physics = False - self._ps = self._world.get_physics_scene() + # Activate the physics backend now that the dexsim World exists. + self.physics.activate(sim_config) # activate physics self.enable_physics(True) @@ -381,13 +638,23 @@ def __init__( self._rigid_objects: Dict[str, RigidObject] = dict() self._constraints: Dict[str, RigidConstraint] = dict() self._rigid_object_groups: Dict[str, RigidObjectGroup] = dict() - self._soft_objects: Dict[str, SoftObject] = dict() - self._cloth_objects: Dict[str, ClothObject] = dict() + self._deformable_objects: Dict[str, DeformableObject] = dict() self._articulations: Dict[str, Articulation] = dict() self._robots: Dict[str, Robot] = dict() self._sensors: Dict[str, BaseSensor] = dict() - self._lights: Dict[str, _Light] = dict() + self._lights: Dict[str, Light] = dict() + + self._spawn_scene = SpawnScene( + self._world, + num_envs=sim_config.num_envs, + spacing=(sim_config.arena_space, sim_config.arena_space, 0.0), + ) + self._arenas = list(self._spawn_scene.builder.prepare_arenas()) + self._prepared_spawn_topology_revision = -1 + self._ready_spawn_topology_revision = -1 + self._synced_spawn_render_topology_revision = -1 + self._camera_attachment_topology_revision = -1 self._visualization_runtime = None self._visualization_overlays: SceneOverlays | None = None @@ -406,19 +673,60 @@ def __init__( self._init_sim_resources() - self._create_default_plane() + # The plane material and visibility are authored before declaration so + # both eager Default loading and deferred Newton loading see them. + self._spawn_default_plane_visibility = True + self._default_plane = None self.set_default_background() + self._declare_spawn_default_plane() self.set_default_global_lighting() - self._build_multiple_arenas(sim_config.num_envs) + # SpawnScene has already prepared the configured Arenas. Start the + # optional browser runtime after default resources are declared. self.start_visualization() - if sim_config.headless is False: self._window = self._world.get_windows() self.is_window_opened = self._window is not None self._enable_default_entity_gizmo() self._is_constructed = True + if not self._defer_startup_summary: + self._log_startup_summary() + + def _log_startup_summary(self) -> None: + """Emit the engine snapshot once, after successful construction.""" + if self.sim_config.startup_summary == "off" or self._startup_summary_logged: + return + from ._startup_summary import format_summary, simulation_rows + + logger.log_info( + format_summary("Simulation initialized", simulation_rows(self)), + prefix=False, + ) + self._startup_summary_logged = True + + def _log_scene_summary(self) -> None: + """Emit the first usable scene snapshot, never a partial prepare.""" + if ( + not getattr(self, "_is_constructed", False) + or getattr(self, "_defer_startup_summary", True) + or getattr(self, "_scene_summary_logged", True) + or self.sim_config.startup_summary == "off" + or self.spawn_result is None + ): + return + from ._startup_summary import format_summary, scene_is_ready, scene_rows + + if not scene_is_ready(self): + return + if ( + self.physics.name == "newton" + and self.physics.cuda_graph_status == "pending" + ): + return + + logger.log_info(format_summary("Scene ready", scene_rows(self)), prefix=False) + self._scene_summary_logged = True @classmethod def get_instance(cls, instance_id: int = 0) -> SimulationManager: @@ -514,13 +822,66 @@ def num_envs(self) -> int: Returns: int: number of arenas. """ - return len(self._arenas) if len(self._arenas) > 0 else 1 + return self.sim_config.num_envs + + @property + def spawn_result(self) -> "Scene | None": + """Return the finalized Scene, or ``None`` before first prepare.""" + spawn_scene = getattr(self, "_spawn_scene", None) + if spawn_scene is None or not spawn_scene.builder.is_finalized: + return None + return spawn_scene.builder.result @property def is_use_gpu_physics(self) -> bool: - """Check if the physics simulation is using GPU.""" + """Whether the active physics backend is running on GPU.""" return self.device.type == "cuda" + @property + def physics_backend(self) -> str: + """Return the active physics backend name.""" + return self.physics.name + + @property + def is_default_backend(self) -> bool: + """Whether the Default physics backend is active.""" + return self.physics.name == "default" + + @property + def is_newton_backend(self) -> bool: + """Whether the Newton physics backend is active.""" + return self.physics.name == "newton" + + @property + def _active_newton_solver_type(self) -> str | None: + """Return the active backend's resolved solver type, when available.""" + return self.physics.solver_type if self.is_newton_backend else None + + @property + def newton_manager(self): + """Compatibility accessor for the removed NewtonManager API. + + A non-Newton backend still returns ``None``. The Newton backend raises + an actionable error because Spawn owns its World-level runtime and no + independent NewtonManager exists. + """ + return self.physics.newton_manager + + @property + def differentiable_runtime(self): + """Return the differentiable facade over the Spawn-owned Newton runtime.""" + runtime = self.physics.differentiable_runtime + if runtime is None: + raise RuntimeError( + f"The {self.physics.name} physics backend does not expose a " + "differentiable runtime." + ) + return runtime + + @property + def is_physics_manually_update(self) -> bool: + return self._world.is_physics_manually_update() + @property def asset_uids(self) -> List[str]: """Get all assets uid in the simulation. @@ -536,8 +897,7 @@ def asset_uids(self) -> List[str]: uid_list.extend(list(self._robots.keys())) uid_list.extend(list(self._rigid_objects.keys())) uid_list.extend(list(self._rigid_object_groups.keys())) - uid_list.extend(list(self._soft_objects.keys())) - uid_list.extend(list(self._cloth_objects.keys())) + uid_list.extend(list(self._deformable_objects.keys())) uid_list.extend(list(self._articulations.keys())) return uid_list @@ -601,6 +961,8 @@ def start_visualization(self) -> VisualizationRuntime | None: """Start the configured live visualizer and publish the current scene.""" if self.sim_config.visualization.backend == "none": return None + if getattr(self, "_spawn_scene", None) is not None: + self.prepare() if getattr(self, "is_window_opened", False): raise RuntimeError( "Cannot start the Viser backend while the native DexSim window " @@ -736,6 +1098,7 @@ def _convert_sim_config( self, sim_config: SimulationManagerCfg ) -> dexsim.WorldConfig: world_config = dexsim.WorldConfig() + world_config.log_startup_info = sim_config.dexsim_startup_info win_config = dexsim.WindowsConfig() win_config.width = sim_config.width win_config.height = sim_config.height @@ -746,8 +1109,6 @@ def _convert_sim_config( world_config.backend = Backend.VULKAN world_config.thread_mode = sim_config.thread_mode world_config.cache_path = str(self._material_cache_dir) - world_config.length_tolerance = sim_config.physics_config.length_tolerance - world_config.speed_tolerance = sim_config.physics_config.speed_tolerance if sim_config.render_cfg.renderer == "auto": from embodichain.lab.sim.utility.render_utils import ( @@ -755,22 +1116,19 @@ def _convert_sim_config( ) resolved_renderer = select_default_renderer(sim_config.gpu_id) - logger.log_info( + logger.log_debug( f"Auto-selected '{resolved_renderer}' renderer for gpu_id={sim_config.gpu_id}." ) sim_config.render_cfg.renderer = resolved_renderer sim_config.render_cfg.apply_to_dexsim_config(world_config) - if type(sim_config.sim_device) is str: - self.device = torch.device(sim_config.sim_device) + if type(sim_config.device) is str: + self.device = torch.device(sim_config.device) else: - self.device = sim_config.sim_device + self.device = sim_config.device if self.device.type == "cuda": - world_config.enable_gpu_sim = True - world_config.direct_gpu_api = True - if self.device.index is not None and sim_config.gpu_id != self.device.index: logger.log_warning( f"Conflict gpu_id {sim_config.gpu_id} and device index {self.device.index}. Using device index." @@ -781,6 +1139,10 @@ def _convert_sim_config( world_config.gpu_id = sim_config.gpu_id + # Apply backend-specific WorldConfig fields (default tolerances/GPU flags + # or the Newton cfg) via the active backend. + self.physics.configure_world(world_config, sim_config) + return world_config def _init_sim_resources(self) -> None: @@ -789,6 +1151,438 @@ def _init_sim_resources(self) -> None: self._default_resources = SimResources() + def register_kinematic_joint_trajectory( + self, + uid: str, + joint_positions: torch.Tensor | np.ndarray, + *, + fps: float | None = None, + root_poses: torch.Tensor | np.ndarray | None = None, + ) -> None: + """Register a Newton kinematic joint trajectory for every arena. + + The leading trajectory dimension follows EmbodiChain's batched arena + layout. Each arena row is lowered to one DexSim runtime control with + the corresponding concrete Spawn articulation path. Row zero is the + initial sample; when ``fps`` is omitted, each call to :meth:`update` + advances to the next sample. + + .. attention:: + Declare the target robot or articulation first, then call this + method before :meth:`prepare`. Runtime controls are part of the + finalized Newton simulation pipeline and cannot be added later. + + Args: + uid: UID of a robot or articulation declared on this manager. + joint_positions: Batched positions in the articulation's public + qpos order with shape ``(num_envs, frames, dof)``. + fps: Optional trajectory sample rate. When omitted, samples advance + once per EmbodiChain physics frame. + root_poses: Optional batched world-space root transforms with shape + ``(num_envs, frames, 4, 4)``. + + Raises: + RuntimeError: If the active backend is not Newton, the Spawn scene + is already finalized, or its arena count is inconsistent. + KeyError: If ``uid`` is not a declared robot or articulation. + ValueError: If an input has an invalid batch shape or non-finite + values. + """ + if not isinstance(uid, str) or not uid: + raise ValueError("uid must be a non-empty string.") + if not self.is_newton_backend: + raise RuntimeError( + "Kinematic joint trajectory controls require the Newton backend." + ) + if uid not in self._robots and uid not in self._articulations: + raise KeyError(f"Robot or articulation {uid!r} is not declared.") + + scene = self._spawn_scene + if scene.builder.is_finalized: + raise RuntimeError( + "Kinematic joint trajectories must be registered before " + "SimulationManager.prepare()." + ) + + if isinstance(joint_positions, torch.Tensor): + positions = joint_positions.detach().cpu().numpy() + else: + positions = np.asarray(joint_positions) + positions = np.asarray(positions, dtype=np.float32) + expected_prefix = (self.num_envs,) + if ( + positions.ndim != 3 + or positions.shape[:1] != expected_prefix + or positions.shape[1] == 0 + or positions.shape[2] == 0 + ): + raise ValueError( + "joint_positions must have non-empty shape " + f"({self.num_envs}, frames, dof); got {positions.shape}." + ) + if not np.isfinite(positions).all(): + raise ValueError("joint_positions must contain only finite values.") + + poses: np.ndarray | None = None + if root_poses is not None: + if isinstance(root_poses, torch.Tensor): + poses = root_poses.detach().cpu().numpy() + else: + poses = np.asarray(root_poses) + poses = np.asarray(poses, dtype=np.float32) + expected_shape = (self.num_envs, positions.shape[1], 4, 4) + if poses.shape != expected_shape: + raise ValueError( + f"root_poses must have shape {expected_shape}; got {poses.shape}." + ) + if not np.isfinite(poses).all(): + raise ValueError("root_poses must contain only finite values.") + + arena_names = scene.arena_names + if len(arena_names) != self.num_envs: + raise RuntimeError( + "Spawn arena count does not match SimulationManager.num_envs: " + f"{len(arena_names)} != {self.num_envs}." + ) + + from dexsim.engine.newton_physics import KinematicJointTrajectoryControl + + controls = tuple( + KinematicJointTrajectoryControl( + f"{arena_name}/{uid}", + positions[env_index], + fps=fps, + root_poses=None if poses is None else poses[env_index], + ) + for env_index, arena_name in enumerate(arena_names) + ) + for control in controls: + scene.builder.add_runtime_control(control) + + def register_contact_material_schedule( + self, + uid: str, + keyframes: Mapping[str, Sequence[Sequence[float]]], + *, + link_names: Sequence[str] | None = None, + ) -> None: + """Register time-varying Newton contact properties for an asset. + + The manager expands the declared UID to the concrete Spawn path in + every Arena. Supported keyframe tracks are ``dynamic_friction``, + ``stiffness``, and ``damping``; each track contains ``(time, value)`` + pairs and is sampled piecewise-constantly in simulation time. + + .. attention:: + Declare the rigid object, robot, or articulation first and call + this method before :meth:`prepare`. Runtime controls are part of + the finalized Newton pipeline and cannot be added later. + + Args: + uid: UID of a declared rigid object, robot, or articulation. + keyframes: Contact-property tracks keyed by property name. + link_names: Optional articulation-link names to update. ``None`` + updates every collision shape belonging to the target. + + Raises: + RuntimeError: If the backend is not Newton, the scene is already + finalized, or its Arena count is inconsistent. + KeyError: If ``uid`` does not identify a declared supported asset. + ValueError: If the UID or keyframes are invalid. + """ + if not isinstance(uid, str) or not uid: + raise ValueError("uid must be a non-empty string.") + if not self.is_newton_backend: + raise RuntimeError("Contact material schedules require the Newton backend.") + if ( + uid not in self._rigid_objects + and uid not in self._robots + and uid not in self._articulations + ): + raise KeyError( + f"Rigid object, robot, or articulation {uid!r} is not declared." + ) + + scene = self._spawn_scene + if scene.builder.is_finalized: + raise RuntimeError( + "Contact material schedules must be registered before " + "SimulationManager.prepare()." + ) + arena_names = scene.arena_names + if len(arena_names) != self.num_envs: + raise RuntimeError( + "Spawn arena count does not match SimulationManager.num_envs: " + f"{len(arena_names)} != {self.num_envs}." + ) + + from dexsim.engine.newton_physics import ContactMaterialSchedule + + controls = tuple( + ContactMaterialSchedule( + f"{arena_name}/{uid}", + keyframes, + link_names=link_names, + ) + for arena_name in arena_names + ) + for control in controls: + scene.builder.add_runtime_control(control) + + def register_particle_contact_material_schedule( + self, + keyframes: Mapping[str, Sequence[Sequence[float]]], + ) -> None: + """Register time-varying Newton particle contact properties. + + Supported tracks are ``dynamic_friction``, ``stiffness``, and + ``damping``. Values apply scene-wide to particle-versus-rigid contacts + and are sampled piecewise-constantly in simulation time. + + .. attention:: + Register this control before :meth:`prepare`. This host-side + control requires direct Newton stepping and therefore disables + CUDA Graph replay for the finalized simulation. + + Args: + keyframes: Particle contact-property tracks containing ``(time, + value)`` pairs. + + Raises: + RuntimeError: If the backend is not Newton or the scene is already + finalized. + ValueError: If the keyframes are invalid. + """ + if not self.is_newton_backend: + raise RuntimeError( + "Particle contact material schedules require the Newton backend." + ) + scene = self._spawn_scene + if scene.builder.is_finalized: + raise RuntimeError( + "Particle contact material schedules must be registered before " + "SimulationManager.prepare()." + ) + + from dexsim.engine.newton_physics import ParticleContactMaterialSchedule + + scene.builder.add_runtime_control(ParticleContactMaterialSchedule(keyframes)) + + def register_kinematic_nodal_trajectory( + self, + uid: str, + node_indices: torch.Tensor | np.ndarray | Sequence[int], + position_offsets: torch.Tensor | np.ndarray, + *, + fps: float | None = None, + rebuild_self_contact_bvh: bool = False, + ) -> None: + """Register a Newton trajectory for selected deformable nodes. + + Each selected node is fixed during Newton model construction and then + moved relative to the world position captured when the runtime control + initializes. The manager expands the batched offsets to one control per + Arena without exposing the private Spawn scene. + + When ``fps`` is provided, samples are linearly interpolated at Newton + substep times. Otherwise one sample is consumed per substep. The final + sample is held after the trajectory ends. + + .. attention:: + Declare the deformable first and clear the Newton ``ACTIVE`` bit in + its ``particle_flags`` for every selected node. Then register this + control before :meth:`prepare`. This host-side control makes Newton + use direct substep launches instead of CUDA Graph replay. Surface + node indices can follow an array-backed mesh directly; volume node + indices refer to the generated tetrahedral simulation particles, + not source-mesh vertices. + + Args: + uid: UID of a deformable declared on this manager. + node_indices: Shared one-dimensional simulation-particle indices. + position_offsets: Batched world-frame position offsets with shape + ``(num_envs, samples, selected_nodes, 3)``. + fps: Optional trajectory sample rate in samples per second. + rebuild_self_contact_bvh: Whether to request a full solver BVH + rebuild at the start of every physics frame when supported. + + Raises: + RuntimeError: If the active backend is not Newton, the Spawn scene + is already finalized, or its Arena count is inconsistent. + KeyError: If ``uid`` is not a declared deformable. + TypeError: If node indices, ``fps``, or the BVH option have invalid + types. + ValueError: If an input has an invalid shape or value, or selected + nodes were not configured as inactive particles. + """ + if not isinstance(uid, str) or not uid: + raise ValueError("uid must be a non-empty string.") + if not self.is_newton_backend: + raise RuntimeError( + "Kinematic nodal trajectory controls require the Newton backend." + ) + if uid not in self._deformable_objects: + raise KeyError(f"Deformable object {uid!r} is not declared.") + + scene = self._spawn_scene + if scene.builder.is_finalized: + raise RuntimeError( + "Kinematic nodal trajectories must be registered before " + "SimulationManager.prepare()." + ) + + if isinstance(node_indices, torch.Tensor): + raw_indices = node_indices.detach().cpu().numpy() + else: + raw_indices = np.asarray(node_indices) + if raw_indices.ndim != 1 or raw_indices.size == 0: + raise ValueError("node_indices must be a non-empty one-dimensional array.") + if raw_indices.dtype.kind not in "iu": + raise TypeError("node_indices must contain integers.") + if np.any(raw_indices < 0): + raise ValueError("node_indices must be non-negative.") + if np.any(raw_indices > np.iinfo(np.int32).max): + raise ValueError("node_indices exceed the supported int32 range.") + indices = np.asarray(raw_indices, dtype=np.int32) + if len(np.unique(indices)) != len(indices): + raise ValueError("node_indices must not contain duplicates.") + + configured_flags = self._deformable_objects[uid].cfg.particle_flags + if configured_flags is None: + raise ValueError( + "Kinematic nodes require particle_flags with the Newton ACTIVE " + "bit cleared before model construction." + ) + flags = np.asarray(configured_flags) + if flags.ndim == 0: + selected_flags = np.full(len(indices), int(flags), dtype=np.int64) + elif flags.ndim == 1: + if int(indices.max()) >= len(flags): + raise ValueError( + "node_indices exceed the configured particle_flags length: " + f"max index {int(indices.max())}, length {len(flags)}." + ) + selected_flags = flags[indices] + else: + raise ValueError( + "Configured particle_flags must be scalar or one-dimensional." + ) + newton_active_particle_flag = 1 + if np.any( + np.asarray(selected_flags, dtype=np.int64) & newton_active_particle_flag + ): + raise ValueError( + "Every kinematic node must have the Newton ACTIVE particle flag cleared." + ) + + if isinstance(position_offsets, torch.Tensor): + offsets = position_offsets.detach().cpu().numpy() + else: + offsets = np.asarray(position_offsets) + offsets = np.asarray(offsets, dtype=np.float32) + if ( + offsets.ndim != 4 + or offsets.shape[0] != self.num_envs + or offsets.shape[1] == 0 + or offsets.shape[2:] != (len(indices), 3) + ): + raise ValueError( + "position_offsets must have non-empty shape " + f"({self.num_envs}, samples, {len(indices)}, 3); got " + f"{offsets.shape}." + ) + if not np.isfinite(offsets).all(): + raise ValueError("position_offsets must contain only finite values.") + + if fps is not None: + if isinstance(fps, bool) or not isinstance( + fps, (int, float, np.integer, np.floating) + ): + raise TypeError("fps must be a finite positive number or None.") + fps = float(fps) + if not np.isfinite(fps) or fps <= 0.0: + raise ValueError("fps must be a finite positive number.") + if not isinstance(rebuild_self_contact_bvh, bool): + raise TypeError("rebuild_self_contact_bvh must be a bool.") + + arena_names = scene.arena_names + if len(arena_names) != self.num_envs: + raise RuntimeError( + "Spawn arena count does not match SimulationManager.num_envs: " + f"{len(arena_names)} != {self.num_envs}." + ) + + from embodichain.lab.sim._runtime_controls import ( + _KinematicNodalTrajectoryControl, + ) + + controls = tuple( + _KinematicNodalTrajectoryControl( + f"{arena_name}/{uid}", + indices, + offsets[env_index], + fps=fps, + rebuild_self_contact_bvh=rebuild_self_contact_bvh, + ) + for env_index, arena_name in enumerate(arena_names) + ) + for control in controls: + scene.builder.add_runtime_control(control) + + def prepare(self) -> None: + """Materialize declarations, bind state, and restore camera parents.""" + self._ready_spawn_topology_revision = -1 + scene = self._spawn_scene + result = scene.builder.result + if ( + not scene.builder.is_finalized + or result is None + or result.needs_rebuild + or scene.builder.has_pending_changes + ): + result = scene.commit() + self._env = result.get_arena("default") + self._arenas = [result.get_arena(name) for name in scene.arena_names] + self.__dict__.pop("arena_offsets", None) + if self._default_plane is None: + self._bind_default_plane(scene.handles("default_plane")[0]) + + # Runtime readiness belongs to the SimulationManager. Keep this and + # facade binding outside the topology-change branch so a failed call + # remains retryable without rematerializing the scene. + scene.prepare_runtime_config(result) + self._prepare_spawn_runtime(result) + scene.bind() + self._sync_spawn_render_state(result) + + topology_revision = int(result.topology_revision) + if ( + getattr(self, "_camera_attachment_topology_revision", -1) + != topology_revision + ): + self._attach_parented_cameras() + self._camera_attachment_topology_revision = topology_revision + self._ready_spawn_topology_revision = topology_revision + + def _prepare_spawn_runtime(self, result: Scene) -> None: + """Prepare backend runtime buffers for one Spawn topology revision.""" + topology_revision = int(result.topology_revision) + if getattr(self, "_prepared_spawn_topology_revision", -1) == topology_revision: + return + self.physics.prepare_spawn_runtime(result) + self._prepared_spawn_topology_revision = topology_revision + + def _sync_spawn_render_state(self, result: Scene) -> None: + """Publish newly bound state once for each Spawn topology revision.""" + topology_revision = int(result.topology_revision) + if ( + getattr(self, "_synced_spawn_render_topology_revision", -1) + == topology_revision + ): + return + self.physics.sync_render_state(result) + self._synced_spawn_render_topology_revision = topology_revision + def enable_physics(self, enable: bool) -> None: """Enable or disable physics simulation. @@ -797,29 +1591,40 @@ def enable_physics(self, enable: bool) -> None: """ self._world.enable_physics(enable) - def init_gpu_physics(self) -> None: - """Initialize the GPU physics simulation.""" - if self.device.type != "cuda": - logger.log_warning( - "The simulation device is not cuda, cannot initialize GPU physics." + def set_manual_update(self, enable: bool) -> None: + """Retain the explicit-update compatibility switch. + + Physics is always advanced by :meth:`update`; background stepping is + no longer supported. Existing callers may still assert manual mode. + + Args: + enable: Must be ``True``. + + Raises: + ValueError: If automatic physics updates are requested. + """ + if not enable: + raise ValueError( + "Automatic physics updates are not supported; call update() " + "to advance the simulation explicitly." ) - return + self._world.set_manual_update(True) - if self._is_initialized_gpu_physics: - return + def init_gpu_physics(self) -> None: + """Prepare the Spawn-owned physics runtime. - for art in self._articulations.values(): - art.reallocate_body_data() - for robot in self._robots.values(): - robot.reallocate_body_data() + This backwards-compatible alias now has the same backend-neutral + behavior as :meth:`prepare`. + """ + self.prepare() - # Re-establish rigid object positions after articulation resets, ensuring - # no articulation kinematics step has inadvertently corrupted the broadphase - # state for rigid bodies. - for rigid_obj in self._rigid_objects.values(): - rigid_obj.reset() + def finalize_newton_physics(self) -> None: + """Prepare the Spawn-owned physics runtime. - self._is_initialized_gpu_physics = True + This backwards-compatible alias now has the same backend-neutral + behavior as :meth:`prepare`. + """ + self.prepare() def render_camera_group(self, group_ids: list[int]) -> None: """Render all camera group in the simulation. @@ -831,6 +1636,7 @@ def render_camera_group(self, group_ids: list[int]) -> None: """ self._world.render_camera_group(group_ids) + self._log_scene_summary() def update(self, physics_dt: float | None = None, step: int = 10) -> None: """Advance physics explicitly and publish the resulting simulation state. @@ -841,17 +1647,11 @@ def update(self, physics_dt: float | None = None, step: int = 10) -> None: Args: physics_dt (float | None, optional): the time step for physics simulation. Defaults to None. - step (int, optional): the number of steps to update physics. Defaults to 10. + step (int, optional): the number of :meth:`World.update` calls per invocation. Defaults to 10. """ with self.profiler.section("sim_update", is_root=True): with self.profiler.section("gpu_physics_check"): - if self.is_use_gpu_physics and not self._is_initialized_gpu_physics: - logger.log_warning( - "Using GPU physics, but not initialized yet. " - "Forcing initialization." - ) - with self.profiler.section("gpu_physics_init"): - self.init_gpu_physics() + self.prepare() with self.profiler.section("physics_steps"): if physics_dt is None: @@ -861,7 +1661,10 @@ def update(self, physics_dt: float | None = None, step: int = 10) -> None: with self.profiler.section("gizmo_update"): self.update_gizmos() with self.profiler.section("world_update"): - self._world.update(physics_dt) + with _temporary_warp_kernel_log_suppression( + self.sim_config.physics_cfg + ): + self._world.update(physics_dt) self._visualization_sim_step += 1 self._visualization_sim_time += physics_dt if ( @@ -878,6 +1681,9 @@ def update(self, physics_dt: float | None = None, step: int = 10) -> None: capture_camera_images=i == step - 1 ) + if step > 0: + self._log_scene_summary() + def get_env(self, arena_index: int = -1) -> dexsim.environment.Arena: """Get the arena or env by index. @@ -976,6 +1782,14 @@ def visualize_point_cloud( def get_world(self) -> dexsim.World: return self._world + def get_physics_scene(self) -> "PhysicsScene": + """Return the Default backend's compatibility scene after Spawn preparation. + + Newton has no ``PhysicsScene`` facade and raises with guidance to use + :attr:`spawn_result` instead. + """ + return self.physics.get_scene() + def can_open_native_window(self) -> bool: """Return whether the native DexSim window may be opened. @@ -1025,6 +1839,7 @@ def open_window(self) -> bool: ): self.enable_window_camera_pose_hotkey(**self._window_camera_pose_hotkey_cfg) self.is_window_opened = True + self._log_scene_summary() return True def close_window(self) -> None: @@ -1039,32 +1854,6 @@ def close_window(self) -> None: self._window_camera_pose_input_control = None self.is_window_opened = False - def _build_multiple_arenas(self, num: int, space: float | None = None) -> None: - """Build multiple arenas in a grid pattern. - - This interface is used for vectorized simulation. - - Args: - num (int): number of arenas to build. - space (float | None, optional): The distance between each arena. Defaults to the arena_space in sim_config. - """ - - if space is None: - space = self.sim_config.arena_space - - if num <= 0: - logger.log_warning("Number of arenas must be greater than 0.") - return - - scene_grid_length = int(np.ceil(np.sqrt(num))) - - for i in range(num): - arena = self._env.add_arena(f"arena_{i}") - - id_x, id_y = i % scene_grid_length, i // scene_grid_length - arena.set_root_node_position([id_x * space, id_y * space, 0]) - self._arenas.append(arena) - def set_indirect_lighting(self, name: str) -> None: """Set indirect lighting. @@ -1094,23 +1883,67 @@ def set_emission_light( if intensity is not None: self._env.set_env_light_intensity(intensity) - def _create_default_plane(self): - default_length = 1000 - repeat_uv_size = int(default_length / 2) - self._default_plane = self._env.create_plane( - 0, default_length, repeat_uv_size, repeat_uv_size + def _declare_spawn_default_plane(self) -> None: + """Declare the global ground in the World's Spawn scene.""" + + from dexsim.spawn import ( + CollisionApproximation, + CollisionDesc, + DexsimCollisionDesc, + GeometryDesc, + NewtonCollisionDesc, + ObjectDesc, + RenderDesc, + RigidBodyPhysicsDesc, + ) + + default_length = 1000.0 + geometry = GeometryDesc.plane(default_length) + repeat_uv_size = default_length / 2.0 + render = RenderDesc.from_geometry( + geometry, + material=self._spawn_default_plane_material, + ) + render.uv_coords = np.asarray( + [ + [0.0, 0.0], + [repeat_uv_size, 0.0], + [repeat_uv_size, repeat_uv_size], + [0.0, repeat_uv_size], + ], + dtype=np.float32, + ) + collision = CollisionDesc.from_geometry( + geometry, + approximation=CollisionApproximation.NONE, + ) + collision.dexsim = DexsimCollisionDesc( + dynamic_friction=0.5, + static_friction=0.5, + ) + collision.newton = NewtonCollisionDesc(mu=0.5) + collision.render_source_index = 0 + descriptor = ObjectDesc( + name="default_plane", + renders=[render], + collisions=[collision], + physics=RigidBodyPhysicsDesc.static(), + per_env=False, ) - self._default_plane.set_name("default_plane") - plane_collision = self._env.create_cube( - default_length, default_length, default_length / 10 + + self._spawn_scene.declare( + "rigid_object", + "default_plane", + descriptor, ) - plane_collision.set_visible(False) - plane_collision_pose = np.eye(4, dtype=float) - plane_collision_pose[2, 3] = -default_length / 20 - 0.001 - plane_collision.set_local_pose(plane_collision_pose) - plane_collision.add_rigidbody(ActorType.KINEMATIC, RigidBodyShape.CONVEX) + handles = self._spawn_scene.handles("default_plane") + if handles: + self._bind_default_plane(handles[0]) - # TODO: add default physics attributes for the plane. + def _bind_default_plane(self, plane: Any) -> None: + """Retain the spawned ground plane and apply its visibility.""" + self._default_plane = plane + plane.set_visible(self._spawn_default_plane_visibility) def set_default_global_lighting(self) -> None: """Set default global lighting for the scene. @@ -1127,7 +1960,6 @@ def set_default_background(self) -> None: """Set default background.""" mat_name = "plane_mat" - mat = None mat_path = self._default_resources.get_material_path("PlaneDark") color_texture = os.path.join(mat_path, "PlaneDark_2K_Color.jpg") roughness_texture = os.path.join(mat_path, "PlaneDark_2K_Roughness.jpg") @@ -1140,7 +1972,11 @@ def set_default_background(self) -> None: ) ) - self._default_plane.set_material(mat.get_instance("plane_mat").mat) + material = mat.get_instance("plane_mat").mat + # Consumed by _declare_spawn_default_plane(). Keeping the native + # material in the descriptor preserves the VisualMaterial registry + # used by visual randomization without forcing finalization. + self._spawn_default_plane_material = material self._visual_materials[mat_name] = mat def set_ground_plane_visibility(self, visible: bool) -> None: @@ -1149,10 +1985,10 @@ def set_ground_plane_visibility(self, visible: bool) -> None: Args: visible (bool): _description_ """ - if visible: - self._default_plane.set_visible(True) - else: - self._default_plane.set_visible(False) + self._spawn_default_plane_visibility = bool(visible) + if self._default_plane is None: + return + self._default_plane.set_visible(bool(visible)) def set_texture_cache( self, key: str, texture: Union[torch.Tensor, List[torch.Tensor]] @@ -1186,16 +2022,26 @@ def get_texture_cache( def get_asset( self, uid: str - ) -> Light | BaseSensor | Robot | RigidObject | Articulation | None: + ) -> ( + Light + | BaseSensor + | Robot + | RigidObject + | RigidObjectGroup + | DeformableObject + | Articulation + | None + ): """Get an asset by its UID. - The asset can be a light, sensor, robot, rigid object or articulation. + The asset can be a light, sensor, robot, rigid object, deformable, or + articulation. Args: uid (str): The UID of the asset. Returns: - Light | BaseSensor | Robot | RigidObject | Articulation | None: The asset instance if found, otherwise None. + The asset instance if found, otherwise ``None``. """ if uid in self._lights: return self._lights[uid] @@ -1207,17 +2053,14 @@ def get_asset( return self._rigid_objects[uid] if uid in self._rigid_object_groups: return self._rigid_object_groups[uid] - if uid in self._soft_objects: - return self._soft_objects[uid] - if uid in self._cloth_objects: - return self._cloth_objects[uid] + if uid in self._deformable_objects: + return self._deformable_objects[uid] if uid in self._articulations: return self._articulations[uid] logger.log_warning(f"Asset {uid} not found.") return None - # Light type string → dexsim LightType enum mapping _LIGHT_TYPE_MAP: dict[str, LightType] = { "point": LightType.POINT, "sun": LightType.SUN, @@ -1226,8 +2069,6 @@ def get_asset( "rect": LightType.RECT, "mesh": LightType.MESH, } - - # Light types that are created as a single global scene light (not per-environment). _GLOBAL_LIGHT_TYPES: tuple[str, ...] = ("sun", "direction") def add_light(self, cfg: LightCfg) -> Light: @@ -1252,7 +2093,7 @@ def add_light(self, cfg: LightCfg) -> Light: Light: The created light instance. Raises: - RuntimeError: If ``cfg.light_type`` is not one of the supported types. + ValueError: If ``cfg.light_type`` is not supported. """ if cfg.uid is None: uid = "light" @@ -1263,45 +2104,41 @@ def add_light(self, cfg: LightCfg) -> Light: if uid in self._lights: logger.log_error(f"Light {uid} already exists.") - light_type_str = cfg.light_type - light_type = self._LIGHT_TYPE_MAP.get(light_type_str) + light_type = self._LIGHT_TYPE_MAP.get(cfg.light_type) if light_type is None: - supported = ", ".join(self._LIGHT_TYPE_MAP.keys()) - logger.log_error( - f"Unsupported light type: '{light_type_str}'. " + supported = ", ".join(self._LIGHT_TYPE_MAP) + raise ValueError( + f"Unsupported light type {cfg.light_type!r}. " f"Supported types: {supported}." ) - # Validation warnings for type-specific constraints - if light_type_str == "mesh" and not cfg.mesh_path: + if cfg.light_type == "mesh" and not cfg.mesh_path: logger.log_warning( f"Mesh light '{uid}' has no mesh_path set. " f"Use set_mesh() to assign a MeshObject." ) - if light_type_str == "rect" and (cfg.rect_width <= 0 or cfg.rect_height <= 0): + if cfg.light_type == "rect" and (cfg.rect_width <= 0 or cfg.rect_height <= 0): logger.log_warning( f"Rect light '{uid}' has zero or negative dimensions " f"(width={cfg.rect_width}, height={cfg.rect_height})." ) if cfg.light_type in self._GLOBAL_LIGHT_TYPES: - # Global scene light: create a single instance on the root - # environment. Infinite-distance lights (sun, direction) are - # physically scene-global and should not be duplicated per arena. - light = self._env.create_light(uid, light_type) - batch_lights = Light(cfg=cfg, entities=[light]) + batch_lights = Light( + cfg=cfg, + entities=[self._env.create_light(uid, light_type)], + ) else: - # Per-environment batched light: one instance per arena. - env_list = [self._env] if len(self._arenas) == 0 else self._arenas - light_list = [] - for i, env in enumerate(env_list): - light_name = f"{uid}_{i}" - light = env.create_light(light_name, light_type) - light_list.append(light) - batch_lights = Light(cfg=cfg, entities=light_list) + batch_lights = Light( + cfg=cfg, + entities=[ + arena.create_light(f"{uid}_{index}", light_type) + for index, arena in enumerate(self._arenas) + ], + ) self._lights[uid] = batch_lights - + self.notify_visualization_topology_changed() return batch_lights def get_light(self, uid: str) -> Light | None: @@ -1326,107 +2163,281 @@ def get_light_uid_list(self) -> List[str]: """ return list(self._lights.keys()) - def add_rigid_object( + def add_usd( self, - cfg: RigidObjectCfg, - ) -> RigidObject: - """Add a rigid object to the scene. + name: str, + file_path: str, + *, + pose: np.ndarray | None = None, + robot_cfgs: dict[str, RobotCfg] | None = None, + ) -> dict[str, RigidObject | Articulation | Robot]: + """Declare the supported entities in a USD scene. + + The returned facades are keyed by their USD prim paths. They remain in + declared state until :meth:`prepare` finalizes the shared Spawn scene, + then bind in place to the resulting DexSim handles. + + USD does not identify which articulations should expose EmbodiChain's + robot interface. Pass those explicitly through ``robot_cfgs``; all + other articulation descriptions become :class:`Articulation` objects. Args: - cfg (RigidObjectCfg): Configuration for the rigid object. + name: Name passed to DexSim's USD scene parser. + file_path: USD, USDA, or USDC file path. + pose: Optional scene-root transform. + robot_cfgs: Robot configurations keyed by USD prim path. These + provide robot-side metadata while physics remains authored by + the USD scene. Returns: - RigidObject: The added rigid object instance handle. + Supported EmbodiChain facades keyed by USD prim path. + + Raises: + RuntimeError: If called after the Spawn scene was finalized. """ - from embodichain.lab.sim.utility.sim_utils import ( - load_mesh_objects_from_cfg, - ) + if self.spawn_result is not None: + raise RuntimeError( + "add_usd() must be called before SimulationManager.prepare()." + ) - uid = cfg.uid - if uid is None: - logger.log_error("Rigid object uid must be specified.") - if uid in self._rigid_objects: - logger.log_error(f"Rigid object {uid} already exists.") + from dexsim.spawn import ArticulationDesc, MeshObjectDesc - env_list = [self._env] if len(self._arenas) == 0 else self._arenas - obj_list = load_mesh_objects_from_cfg( - cfg=cfg, - env_list=env_list, - cache_dir=self._convex_decomp_dir, + descriptors = self._spawn_scene.builder.add_usd( + name, + file_path, + pose=pose, + per_env=True, ) + assets: dict[str, RigidObject | Articulation | Robot] = {} + robot_cfgs = robot_cfgs or {} + + for descriptor in descriptors: + source_path = ( + descriptor.usd.prim_path + if descriptor.usd is not None and descriptor.usd.prim_path + else descriptor.name + ) - rigid_obj = RigidObject(cfg=cfg, entities=obj_list, device=self.device) + if type(descriptor) is MeshObjectDesc: + body_type = "static" + if descriptor.physics is not None: + body_type = { + ActorType.DYNAMIC: "dynamic", + ActorType.KINEMATIC: "kinematic", + ActorType.STATIC: "static", + }[descriptor.physics.actor_type] + cfg = RigidObjectCfg( + uid=descriptor.name, + init_local_pose=descriptor.pose.copy(), + body_type=body_type, + body_scale=tuple(float(value) for value in descriptor.body_scale), + asset_physics_mode="preserve", + ) + facade = RigidObject( + cfg=cfg, + device=self.device, + ) - if cfg.shape.visual_material: - mat = self.create_visual_material(cfg.shape.visual_material) - rigid_obj.set_visual_material(mat, update_default=True) + self._spawn_scene.track( + "rigid_object", + descriptor.name, + descriptor, + facade=facade, + ) + self._rigid_objects[descriptor.name] = facade + assets[source_path] = facade + continue - self._rigid_objects[uid] = rigid_obj - self.notify_visualization_topology_changed() + if isinstance(descriptor, ArticulationDesc): + robot_cfg = robot_cfgs.get(source_path) + facade_type: type[Articulation] = ( + Robot if robot_cfg is not None else Articulation + ) + cfg = ( + deepcopy(robot_cfg) + if robot_cfg is not None + else ArticulationCfg(uid=descriptor.name) + ) + cfg.uid = descriptor.name + cfg.fpath = file_path + cfg.init_local_pose = descriptor.pose.copy() + cfg.asset_physics_mode = "preserve" + if robot_cfg is None: + cfg.root_props = ArticulationRootPropertiesCfg() + else: + cfg.root_props = cfg.root_props.copy() + cfg.root_props.fixed_base = bool(descriptor.fixed_base) + cfg.root_props.self_collision_enabled = descriptor.enable_self_collision + cfg.body_scale = tuple(float(value) for value in descriptor.body_scale) + cfg.build_pk_chain = False + facade = facade_type( + cfg=cfg, + device=self.device, + ) - return rigid_obj + self._spawn_scene.track( + "articulation", + descriptor.name, + descriptor, + facade=facade, + ) + registry = ( + self._robots if robot_cfg is not None else self._articulations + ) + registry[descriptor.name] = facade + assets[source_path] = facade + + self.notify_visualization_topology_changed() + return assets - def add_soft_object(self, cfg: SoftObjectCfg) -> SoftObject: - """Add a soft object to the scene. + def add_rigid_object( + self, + cfg: RigidObjectCfg, + ) -> RigidObject: + """Add a rigid object to the scene. Args: - cfg (SoftObjectCfg): Configuration for the soft object. + cfg (RigidObjectCfg): Configuration for the rigid object. Returns: - SoftObject: The added soft object instance handle. + RigidObject: The added rigid object instance handle. """ - if not self.is_use_gpu_physics: - logger.log_error("Soft object requires GPU physics to be enabled.") - - from embodichain.lab.sim.utility import ( - load_soft_object_from_cfg, - ) - uid = cfg.uid if uid is None: - logger.log_error("Soft object uid must be specified.") + raise ValueError("Rigid object uid must be specified.") + if uid in self._rigid_objects: + raise ValueError(f"Rigid object {uid!r} already exists.") + source_path = getattr(cfg.shape, "fpath", None) + if _is_usd_path(source_path): + descriptor, materials = rigid_desc_from_usd( + cfg, + per_env=True, + newton_solver_type=self._active_newton_solver_type, + ) + else: + descriptor, materials = rigid_desc_from_cfg( + cfg, + per_env=True, + newton_solver_type=self._active_newton_solver_type, + ) + self._spawn_scene.builder.materials.update(materials) - env_list = [self._env] if len(self._arenas) == 0 else self._arenas - obj_list = load_soft_object_from_cfg( + rigid_obj = RigidObject( cfg=cfg, - env_list=env_list, + device=self.device, ) - soft_obj = SoftObject(cfg=cfg, entities=obj_list, device=self.device) - self._soft_objects[uid] = soft_obj + was_materialized = self.spawn_result is not None + self._spawn_scene.declare( + "rigid_object", + uid, + descriptor, + facade=rigid_obj, + ) + self._rigid_objects[uid] = rigid_obj self.notify_visualization_topology_changed() - return soft_obj - def add_cloth_object(self, cfg: ClothObjectCfg) -> ClothObject: - """Add a cloth object to the scene. + # Preserve the legacy immediate-availability behavior for runtime + # additions. Initial environment construction still batches all + # declarations into one finalize at BaseEnv's prepare boundary. + if was_materialized: + self.prepare() + return rigid_obj + + def add_deformable_object(self, cfg: DeformableObjectCfg) -> DeformableObject: + """Declare a volume or surface deformable in the scene. + + Deformables are DexSim 0.5 typed particle sets owned by the Newton + Spawn scene. The Default backend is intentionally unsupported. Args: - cfg (ClothObjectCfg): Configuration for the cloth object. + cfg: Volume- or surface-deformable configuration. Returns: - ClothObject: The added cloth object instance handle. - """ - if not self.is_use_gpu_physics: - logger.log_error("Cloth object requires GPU physics to be enabled.") + The declared deformable facade. - from embodichain.lab.sim.utility import ( - load_cloth_object_from_cfg, - ) + Raises: + NotImplementedError: If the active backend or device cannot host + the requested deformable type. + ValueError: If the discriminator or UID is invalid. + """ + deformable_type = cfg.deformable_type + if deformable_type == "volume": + supported = self.physics.supports_volume_deformables + elif deformable_type == "surface": + supported = self.physics.supports_surface_deformables + else: + raise ValueError( + f"Unsupported deformable_type {deformable_type!r}; expected " + "'volume' or 'surface'." + ) + if not supported: + raise NotImplementedError( + "EmbodiChain deformable objects require the Newton backend; " + f"the {self.physics.name} backend does not support them." + ) + if self.device.type != "cuda": + raise NotImplementedError( + "Newton deformable particle sets currently require a CUDA device." + ) + solver_type = self._active_newton_solver_type + supported_solvers = {"auto", "dexuni", "xpbd", "semi_implicit", "vbd"} + if solver_type not in supported_solvers: + raise NotImplementedError( + f"Newton solver {solver_type!r} does not support deformable " + "particle sets; select one of 'auto', 'dexuni', 'xpbd', " + "'semi_implicit', or 'vbd'." + ) + physics_cfg = self.sim_config.physics_cfg + if isinstance(physics_cfg, NewtonPhysicsCfg) and physics_cfg.requires_grad: + raise NotImplementedError( + "Newton deformable state mutation is unavailable when " + "requires_grad=True." + ) + if self.spawn_result is not None: + raise NotImplementedError( + "DexSim Spawn does not yet support adding deformables after " + "finalization." + ) uid = cfg.uid if uid is None: - logger.log_error("Cloth object uid must be specified.") + raise ValueError("Deformable object uid must be specified.") + if uid in self._deformable_objects: + raise ValueError(f"Deformable object {uid!r} already exists.") - env_list = [self._env] if len(self._arenas) == 0 else self._arenas - obj_list = load_cloth_object_from_cfg( - cfg=cfg, - env_list=env_list, + backend_implementations = _DEFORMABLE_BACKEND_IMPLEMENTATIONS.get( + self.physics.name ) + if not backend_implementations: + raise NotImplementedError( + f"No deformable implementation is registered for the " + f"{self.physics.name} backend." + ) - cloth_obj = ClothObject(cfg=cfg, entities=obj_list, device=self.device) - self._cloth_objects[uid] = cloth_obj + config_cls, object_cls, descriptor_factory, spawn_kind = ( + backend_implementations[deformable_type] + ) + if not isinstance(cfg, config_cls): + raise TypeError( + f"A {deformable_type} deformable requires " + f"{config_cls.__name__}, got {type(cfg).__name__}." + ) + descriptor, materials = descriptor_factory(cfg, per_env=True) + self._spawn_scene.builder.materials.update(materials) + deformable = object_cls( + cfg, + device=self.device, + ) + self._spawn_scene.declare( + spawn_kind, + uid, + descriptor, + facade=deformable, + ) + self._deformable_objects[uid] = deformable self.notify_visualization_topology_changed() - return cloth_obj + return deformable def get_rigid_object(self, uid: str) -> RigidObject | None: """Get a rigid object by its unique ID. @@ -1442,33 +2453,12 @@ def get_rigid_object(self, uid: str) -> RigidObject | None: return None return self._rigid_objects[uid] - def get_soft_object(self, uid: str) -> SoftObject | None: - """Get a soft object by its unique ID. - - Args: - uid (str): The unique ID of the soft object. - - Returns: - SoftObject | None: The soft object instance if found, otherwise None. - """ - if uid not in self._soft_objects: - logger.log_warning(f"Soft object {uid} not found.") - return None - return self._soft_objects[uid] - - def get_cloth_object(self, uid: str) -> ClothObject | None: - """Get a cloth object by its unique ID. - - Args: - uid (str): The unique ID of the cloth object. - - Returns: - ClothObject | None: The cloth object instance if found, otherwise None. - """ - if uid not in self._cloth_objects: - logger.log_warning(f"Cloth object {uid} not found.") + def get_deformable_object(self, uid: str) -> DeformableObject | None: + """Get a deformable object by its unique ID.""" + if uid not in self._deformable_objects: + logger.log_warning(f"Deformable object {uid} not found.") return None - return self._cloth_objects[uid] + return self._deformable_objects[uid] def get_rigid_object_uid_list(self) -> List[str]: """Get current rigid body uid list @@ -1485,20 +2475,7 @@ def _broadcast_frame( env_ids: Sequence[int], name: str, ) -> list[np.ndarray]: - """Broadcast a local-frame spec to one matrix per target env. - - Args: - frame: None -> identity; (4,4) -> repeated; (N,4,4) -> indexed per env. - num_envs: Total number of arenas (used to validate (N,4,4)). - env_ids: Target env indices to produce frames for. - name: Constraint name (for error messages). - - Returns: - A list of (4,4) numpy arrays, one per env in env_ids. - - Raises: - RuntimeError: If an (N,4,4) frame's N != num_envs, or shape is invalid. - """ + """Broadcast a local constraint frame to the selected environments.""" if frame is None: identity = np.eye(4, dtype=np.float32) return [identity for _ in env_ids] @@ -1548,15 +2525,11 @@ def create_rigid_constraint( cfg: RigidConstraintCfg, env_ids: Sequence[int] | torch.Tensor | None = None, ) -> RigidConstraint: - """Create a fixed constraint between two RigidObjects. + """Create a fixed constraint between two rigid objects. - Binds ``rigid_object_a``'s entity[i] to ``rigid_object_b``'s entity[i] - within arena[i], for each env in ``env_ids``. Local frames default to - welding the objects at their *current* relative pose: - ``local_frame_a`` defaults to identity (object A's origin) and - ``local_frame_b`` defaults to ``inv(pose_B) @ pose_A`` (computed per env), - so the offset is preserved rather than the two origins being pulled - together. Pass explicit frames to define a specific joint frame. + Constraints are native Default-backend resources owned by each Arena. + Spawn owns the two actors; this method only borrows their native actor + handles while creating the constraint. Args: cfg: The constraint configuration. @@ -1564,20 +2537,17 @@ def create_rigid_constraint( the :class:`EventManager`) or a sequence of ints. None -> all arenas. Returns: - The created :class:`RigidConstraint`. - - Raises: - RuntimeError: If either object is missing, the name is already in use, - a frame shape is invalid, or dexsim fails to create a handle. + The created constraint batch. """ - # validate constraint type (only fixed supported in v1) + if hasattr(self, "physics") and not self.physics.supports_rigid_constraints: + raise NotImplementedError( + f"The {self.physics.name} backend does not support rigid constraints." + ) if cfg.constraint_type != "fixed": logger.log_error( f"Constraint '{cfg.name}' has unsupported type " - f"'{cfg.constraint_type}'. Only 'fixed' is supported in v1." + f"'{cfg.constraint_type}'. Only 'fixed' is supported." ) - - # resolve objects if cfg.rigid_object_a_uid not in self._rigid_objects: logger.log_error( f"RigidObject '{cfg.rigid_object_a_uid}' not found for constraint " @@ -1588,16 +2558,16 @@ def create_rigid_constraint( f"RigidObject '{cfg.rigid_object_b_uid}' not found for constraint " f"'{cfg.name}'. Available: {list(self._rigid_objects.keys())}." ) - rigid_object_a = self._rigid_objects[cfg.rigid_object_a_uid] - rigid_object_b = self._rigid_objects[cfg.rigid_object_b_uid] - - # validate duplicate name if cfg.name in self._constraints: logger.log_error( f"Constraint '{cfg.name}' already exists. Remove it before recreating." ) - # validate object entity counts match num_envs + rigid_object_a = self._rigid_objects[cfg.rigid_object_a_uid] + rigid_object_b = self._rigid_objects[cfg.rigid_object_b_uid] + if hasattr(self, "_spawn_scene"): + self.prepare() + num_envs = self.num_envs if rigid_object_a.num_instances != num_envs: logger.log_error( @@ -1610,50 +2580,52 @@ def create_rigid_constraint( f"{rigid_object_b.num_instances} instances but num_envs is {num_envs}." ) - # resolve target env_ids (accepts None / tensor / sequence) target_env_ids = self._normalize_env_ids(env_ids, num_envs) - - # broadcast local frames. - # local_frame_a defaults to identity (object A's origin). - # local_frame_b defaults to the current relative pose of A w.r.t. B - # (inv(pose_B) @ pose_A), so that with both frames left as None the - # constraint welds the objects at their *current* relative pose instead - # of pulling their origins together. frames_a = self._broadcast_frame( cfg.local_frame_a, num_envs, target_env_ids, cfg.name ) if cfg.local_frame_b is None: pose_a = rigid_object_a.get_local_pose(to_matrix=True) pose_b = rigid_object_b.get_local_pose(to_matrix=True) - frame_b = torch.bmm(pose_inv(pose_b), pose_a) # (N, 4, 4) - frame_b = frame_b.cpu().numpy().astype(np.float32) + frame_b = ( + torch.bmm(pose_inv(pose_b), pose_a).cpu().numpy().astype(np.float32) + ) frames_b = [frame_b[i] for i in target_env_ids] else: frames_b = self._broadcast_frame( cfg.local_frame_b, num_envs, target_env_ids, cfg.name ) - # pre-size handles list with None, fill target envs handles: list = [None] * num_envs try: - for idx, env_id in enumerate(target_env_ids): + for index, env_id in enumerate(target_env_ids): + actor_a = rigid_object_a._entities[env_id] + actor_b = rigid_object_b._entities[env_id] + if getattr(rigid_object_a, "is_spawn_bound", False) is True: + actor_a = actor_a.native + if getattr(rigid_object_b, "is_spawn_bound", False) is True: + actor_b = actor_b.native + if actor_a is None or actor_b is None: + logger.log_error( + f"Constraint '{cfg.name}' references a released Spawn actor " + f"in environment {env_id}." + ) + arena = self.get_env(env_id) - name_i = cfg.name if num_envs <= 1 else f"{cfg.name}_{env_id}" + name = cfg.name if num_envs <= 1 else f"{cfg.name}_{env_id}" handle = arena.create_fixed_constraint( - name_i, - rigid_object_a._entities[env_id], - rigid_object_b._entities[env_id], - frames_a[idx], - frames_b[idx], + name, + actor_a, + actor_b, + frames_a[index], + frames_b[index], ) if handle is None: logger.log_error( - f"Failed to create constraint '{name_i}' in arena {env_id}." + f"Failed to create constraint '{name}' in arena {env_id}." ) handles[env_id] = handle except Exception: - # Ensure partially created per-arena constraints are removed if a later - # arena fails, so create/remove semantics stay consistent. RigidConstraint( cfg=cfg, constraint_handles=handles, @@ -1673,21 +2645,9 @@ def create_rigid_constraint( self._constraints[cfg.name] = constraint return constraint - def get_soft_object_uid_list(self) -> List[str]: - """Get current soft body uid list - - Returns: - List[str]: list of soft body uid. - """ - return list(self._soft_objects.keys()) - - def get_cloth_object_uid_list(self) -> List[str]: - """Get current cloth body uid list - - Returns: - List[str]: list of cloth body uid. - """ - return list(self._cloth_objects.keys()) + def get_deformable_object_uid_list(self) -> List[str]: + """Return all deformable object UIDs in declaration order.""" + return list(self._deformable_objects.keys()) def remove_rigid_constraint( self, @@ -1749,43 +2709,72 @@ def add_rigid_object_group(self, cfg: RigidObjectGroupCfg) -> RigidObjectGroup: Args: cfg (RigidObjectGroupCfg): Configuration for the rigid object group. - """ - from embodichain.lab.sim.utility.sim_utils import ( - load_mesh_objects_from_cfg, - ) + Returns: + The stable Group facade. During initial scene construction it is + bound to Spawn handles by :meth:`prepare`. + """ + if not self.physics.supports_rigid_object_group: + raise NotImplementedError( + f"The {self.physics.name} backend does not support rigid object groups." + ) uid = cfg.uid if uid is None: - logger.log_error("Rigid object group uid must be specified.") + raise ValueError("Rigid object group uid must be specified.") if uid in self._rigid_object_groups: - logger.log_error(f"Rigid object group {uid} already exists.") - + raise ValueError(f"Rigid object group {uid!r} already exists.") if cfg.body_type == "static": - logger.log_error("Rigid object group cannot be static.") - - env_list = [self._env] if len(self._arenas) == 0 else self._arenas - - obj_group_list = [] - for key, rigid_cfg in tqdm( - cfg.rigid_objects.items(), desc="Loading rigid objects" - ): - obj_list = load_mesh_objects_from_cfg( - cfg=rigid_cfg, - env_list=env_list, - cache_dir=self._convex_decomp_dir, - ) - obj_group_list.append(obj_list) + raise ValueError("Rigid object group cannot be static.") + if not cfg.rigid_objects: + raise ValueError("Rigid object group must contain at least one object.") + + actor_type = { + "dynamic": ActorType.DYNAMIC, + "kinematic": ActorType.KINEMATIC, + }[cfg.body_type] + descriptors = [] + for index, member in enumerate(cfg.rigid_objects.values()): + member_cfg = deepcopy(member) + member_cfg.uid = f"{uid}__member_{index}" + member_cfg.body_type = cfg.body_type + source_path = getattr(member_cfg.shape, "fpath", None) + if _is_usd_path(source_path): + descriptor, materials = rigid_desc_from_usd( + member_cfg, + per_env=True, + newton_solver_type=self._active_newton_solver_type, + ) + else: + descriptor, materials = rigid_desc_from_cfg( + member_cfg, + per_env=True, + newton_solver_type=self._active_newton_solver_type, + ) + if descriptor.physics is None: + raise ValueError( + f"Rigid object group member {index} has no rigid-body physics." + ) + descriptor.physics.actor_type = actor_type + self._spawn_scene.builder.materials.update(materials) + descriptors.append(descriptor) - # Convert [a1, a2, ...], [b1, b2, ...] to [(a1, b1, ...), (a2, b2, ...), ...] - obj_group_list = list(zip(*obj_group_list)) - rigid_obj_group = RigidObjectGroup( - cfg=cfg, entities=obj_group_list, device=self.device + group = RigidObjectGroup( + cfg, + device=self.device, ) - self._rigid_object_groups[uid] = rigid_obj_group + was_materialized = self.spawn_result is not None + self._spawn_scene.declare( + "rigid_object_group", + uid, + tuple(descriptors), + facade=group, + ) + self._rigid_object_groups[uid] = group self.notify_visualization_topology_changed() - - return rigid_obj_group + if was_materialized: + self.prepare() + return group def get_rigid_object_group(self, uid: str) -> RigidObjectGroup | None: """Get a rigid object group by its unique ID. @@ -1854,54 +2843,23 @@ def add_articulation( Returns: Articulation: The added articulation instance handle. """ - uid = cfg.uid if uid is None: + if cfg.fpath is None: + raise ValueError( + "Articulation configuration must provide fpath when uid " + "is not specified." + ) uid = os.path.splitext(os.path.basename(cfg.fpath))[0] cfg.uid = uid if uid in self._articulations: - logger.log_error(f"Articulation {uid} already exists.") - - env_list = [self._env] if len(self._arenas) == 0 else self._arenas - obj_list = [] - - is_usd = cfg.fpath.endswith((".usd", ".usda", ".usdc")) - if is_usd: - # TODO: Currently add checking for num_envs when file is USD. After we support spawn via cloning, we can remove this. - if len(env_list) > 1: - logger.log_error(f"Currently not supporting multiple arenas for USD.") - env = self._env - results = env.import_from_usd_file( - cfg.fpath, return_object=True, cache_dir=self._convex_decomp_dir - ) - # print("USD import results:", results) - - articulations_found = [] - for key, value in results.items(): - if isinstance(value, dexsim.engine.Articulation): - articulations_found.append(value) - - if len(articulations_found) == 0: - logger.log_error(f"No articulation found in USD file {cfg.fpath}.") - elif len(articulations_found) > 1: - logger.log_error( - f"Multiple articulations found in USD file {cfg.fpath}. " - ) - elif len(articulations_found) == 1: - obj_list.append(articulations_found[0]) - else: - # non-usd file does not support this option, will be forced set False to avoid potential issues. - cfg.use_usd_properties = False - - for env in env_list: - art = env.load_urdf(cfg.fpath) - obj_list.append(art) - - articulation = Articulation(cfg=cfg, entities=obj_list, device=self.device) + raise ValueError(f"Articulation {uid!r} already exists.") + was_materialized = self.spawn_result is not None + articulation = self._declare_spawn_articulation(cfg, Articulation) self._articulations[uid] = articulation - self.notify_visualization_topology_changed() - + if was_materialized: + self.prepare() return articulation def get_articulation(self, uid: str) -> Articulation | None: @@ -1926,15 +2884,29 @@ def get_articulation_uid_list(self) -> List[str]: """ return list(self._articulations.keys()) - def add_robot(self, cfg: RobotCfg) -> Robot | None: + def add_robot(self, cfg: RobotCfg | RobotPresetCfg) -> Robot | None: """Add a Robot to the scene. Args: - cfg (RobotCfg): Configuration for the robot. + cfg: A concrete robot configuration or a replace-only backend + preset. Presets are resolved from ``physics_cfg`` before the + robot is declared. Returns: Robot | None: The added robot instance handle, or None if failed. """ + if not self.physics.supports_robot: + logger.log_error( + f"Robot support is not enabled for the " + f"{self.physics.name} backend yet.", + error_type=NotImplementedError, + ) + + if isinstance(cfg, RobotPresetCfg): + cfg = cfg.resolve( + self.sim_config.physics_cfg, + newton_solver_type=self._active_newton_solver_type, + ) uid = cfg.uid if cfg.fpath is None: @@ -1959,45 +2931,59 @@ def add_robot(self, cfg: RobotCfg) -> Robot | None: logger.log_error(f"Robot {uid} already exists.") return self._robots[uid] - env_list = [self._env] if len(self._arenas) == 0 else self._arenas - obj_list = [] - - is_usd = cfg.fpath.endswith((".usd", ".usda", ".usdc")) - if is_usd: - # TODO: Currently add checking for num_envs when file is USD. After we support spawn via cloning, we can remove this. - if len(env_list) > 1: - logger.log_error(f"Currently not supporting multiple arenas for USD.") - env = self._env - results = env.import_from_usd_file(cfg.fpath, return_object=True) - # print("USD import results:", results) - - articulations_found = [] - for key, value in results.items(): - if isinstance(value, dexsim.engine.Articulation): - articulations_found.append(value) - - if len(articulations_found) == 0: - logger.log_error(f"No articulation found in USD file {cfg.fpath}.") - elif len(articulations_found) > 1: - logger.log_error( - f"Multiple articulations found in USD file {cfg.fpath}. " - ) - elif len(articulations_found) == 1: - obj_list.append(articulations_found[0]) - else: - # non-usd file does not support this option, will be forced set False to avoid potential issues. - cfg.use_usd_properties = False + was_materialized = self.spawn_result is not None + robot = self._declare_spawn_articulation(cfg, Robot) + self._robots[uid] = robot + if was_materialized: + self.prepare() + return robot + + def _declare_spawn_articulation( + self, + cfg: ArticulationCfg, + facade_type: type[Articulation], + ) -> Articulation: + """Declare an articulation facade and bind its Batch after finalize. - for env in env_list: - art = env.load_urdf(cfg.fpath) - obj_list.append(art) + DexSim remains the sole articulation source loader. EmbodiChain applies + regex/group configuration to the resolved descriptor before either + backend materializes it. Runtime Batch data is created at the shared + prepare boundary. + """ + if _is_usd_path(cfg.fpath): + descriptor, materials = articulation_desc_from_usd( + cfg, + per_env=True, + newton_solver_type=self._active_newton_solver_type, + ) + self._spawn_scene.builder.materials.update(materials) + else: + descriptor = articulation_desc_from_cfg( + cfg, + per_env=True, + newton_solver_type=self._active_newton_solver_type, + ) + if cfg.uid is None: + cfg.uid = descriptor.name - robot = Robot(cfg=cfg, entities=obj_list, device=self.device) + facade = facade_type( + cfg=cfg, + device=self.device, + ) - self._robots[uid] = robot + self._spawn_scene.declare( + "articulation", + descriptor.name, + descriptor, + facade=facade, + configure_source=partial( + configure_articulation_desc, + cfg=cfg, + newton_solver_type=self._active_newton_solver_type, + ), + ) self.notify_visualization_topology_changed() - - return robot + return facade def get_robot(self, uid: str) -> Robot | None: """Get a Robot by its unique ID. @@ -2051,7 +3037,7 @@ def enable_entity_gizmo( result = controller.register_external_target( self._DEFAULT_PLANE_GIZMO_TARGET_ID, dexsim.interaction.EntityGizmoTargetType.RIGID_BODY, - default_plane, + default_plane.native(), ActorType.STATIC, ) if result != dexsim.interaction.EntityGizmoResult.SUCCESS: @@ -2274,17 +3260,20 @@ def process_visualization_commands(self) -> int: device=self.device, ) position = position - self.arena_offsets[0] - wxyz = torch.as_tensor( - command.wxyz, - dtype=torch.float32, - device=self.device, + xyzw = convert_quat( + torch.as_tensor( + command.wxyz, + dtype=torch.float32, + device=self.device, + ), + to="xyzw", ).unsqueeze(0) pose = torch.eye( 4, dtype=torch.float32, device=self.device, ).unsqueeze(0) - pose[0, :3, :3] = matrix_from_quat(wxyz)[0] + pose[0, :3, :3] = matrix_from_quat(xyzw)[0] pose[0, :3, 3] = position if not gizmo.request_local_pose(pose, source_id=source_id): continue @@ -2426,7 +3415,13 @@ def set_gizmo_visibility( gizmo.set_visible(visible) def add_sensor(self, sensor_cfg: SensorCfg) -> BaseSensor: - """General interface to add a sensor to the scene and returns a handle. + """Create a sensor on the pre-created simulation Arenas. + + Cameras keep EmbodiChain's native CameraGroup implementation. A camera + attached to an articulation link is created immediately and attached + after the physical Spawn scene is prepared. Contact sensors are created + after preparation and query contacts through the backend-neutral Spawn + Scene API. Args: sensor_cfg (SensorCfg): configuration for the sensor. @@ -2435,39 +3430,128 @@ def add_sensor(self, sensor_cfg: SensorCfg) -> BaseSensor: BaseSensor: The added sensor instance handle. """ sensor_type = sensor_cfg.sensor_type - if sensor_type not in self.SUPPORTED_SENSOR_TYPES: - logger.log_warning(f"Unsupported sensor type: {sensor_type}") - return None - - sensor_uid = sensor_cfg.uid - if sensor_uid is None: - sensor_uid = f"{sensor_type.lower()}_{len(self._sensors)}" - sensor_cfg.uid = sensor_uid - - if sensor_uid in self._sensors: - logger.log_warning(f"Sensor {sensor_uid} already exists.") - return None + uid = sensor_cfg.uid + if uid is None: + uid = f"{sensor_type.lower()}_{len(self._sensors)}" + sensor_cfg.uid = uid + if uid in self._sensors: + raise ValueError(f"Sensor {uid!r} already exists.") - parent_nodes = None - if ( - isinstance(sensor_cfg, CameraCfg) - and sensor_cfg.extrinsics.parent is not None + sensor_factory = self.SUPPORTED_SENSOR_TYPES.get(sensor_type) + if sensor_factory is None: + raise ValueError( + f"Unsupported sensor type {sensor_type!r}. Supported types: " + f"{sorted(self.SUPPORTED_SENSOR_TYPES)}." + ) + if isinstance(sensor_factory, type) and issubclass(sensor_factory, Camera): + if len(self._arenas) != self.num_envs: + raise RuntimeError( + "Camera creation requires all Spawn Arenas to be " + f"prepared ({len(self._arenas)} of {self.num_envs} ready)." + ) + sensor = sensor_factory( + sensor_cfg, + self.device, + owner=self, + ) + if sensor_cfg.extrinsics.parent is not None: + if self._spawn_scene.builder.result is not None: + self._attach_camera_parent(sensor) + elif isinstance(sensor_factory, type) and issubclass( + sensor_factory, ContactSensor ): - parent_nodes = resolve_parent_nodes( - parent=sensor_cfg.extrinsics.parent, - assets={**self._articulations, **self._robots}, - num_envs=self.num_envs, + self.prepare() + sensor = sensor_factory( + sensor_cfg, + self.device, + owner=self, ) + else: + # Custom native sensors require a prepared physics scene; cameras + # only depend on the pre-created Arenas. + self.prepare() + # Preserve custom test/plugin factories whose two-argument + # constructor predates the manager-owned render context. + sensor = sensor_factory(sensor_cfg, self.device) + + self._sensors[uid] = sensor + self.notify_visualization_topology_changed() + return sensor - sensor = self.SUPPORTED_SENSOR_TYPES[sensor_type](sensor_cfg, self.device) - if isinstance(sensor, Camera) and parent_nodes is not None: - sensor.attach_to_parent_nodes(parent_nodes) + def _attach_camera_parent(self, sensor: Camera) -> None: + """Resolve and attach one camera to its configured parent nodes.""" + parent = sensor.cfg.extrinsics.parent + if parent is None: + return + parent_nodes = self._resolve_spawn_sensor_parent_nodes(parent) + sensor.attach_to_parent_nodes(parent_nodes) + + def _attach_parented_cameras(self) -> None: + """Restore parented cameras after a Spawn topology change.""" + for sensor in self._sensors.values(): + if isinstance(sensor, Camera) and sensor.cfg.extrinsics.parent is not None: + self._attach_camera_parent(sensor) + + def _resolve_spawn_sensor_parent_nodes(self, parent: str) -> list[object]: + """Resolve one canonical articulation link to a render node per Arena. + + A plain link name remains compatible with existing CameraCfg values. + When more than one robot/articulation owns that link, callers can use + ``"/"`` to disambiguate without introducing + backend clone suffixes. + """ + assets: dict[str, Articulation] = { + **self._articulations, + **self._robots, + } + asset_uid: str | None = None + link_name = parent + if "/" in parent: + candidate_uid, candidate_link = parent.split("/", maxsplit=1) + if candidate_uid in assets: + asset_uid = candidate_uid + link_name = candidate_link + + matches: list[tuple[str, list[object]]] = [] + for uid, asset in assets.items(): + if asset_uid is not None and uid != asset_uid: + continue + handles = list(getattr(asset, "_entities", ())) + if len(handles) != self.num_envs: + continue + if link_name not in handles[0].get_link_names(): + continue - self._sensors[sensor_uid] = sensor - if isinstance(sensor, Camera): - self.notify_visualization_topology_changed() + nodes: list[object] = [] + for handle in handles: + if link_name not in handle.get_link_names(): + raise RuntimeError( + f"Articulation {uid!r} has heterogeneous link topology; " + f"link {link_name!r} is missing in one Arena." + ) + render_body = handle.get_render_body(link_name) + if render_body is None: + raise RuntimeError( + f"Articulation {uid!r} link {link_name!r} has no public " + "render node for camera attachment." + ) + nodes.append(render_body.render_node()) + matches.append((uid, nodes)) - return sensor + if len(matches) == 1: + return matches[0][1] + if len(matches) > 1: + owners = ", ".join(uid for uid, _ in matches) + raise ValueError( + f"Camera parent link {link_name!r} is ambiguous across assets " + f"[{owners}]; use '/{link_name}'." + ) + scope = f" on asset {asset_uid!r}" if asset_uid is not None else "" + raise ValueError( + f"Camera parent link {link_name!r} was not found{scope} in any " + "Spawn-bound Robot or Articulation. Attachment to arbitrary render " + "nodes is not yet supported by the Spawn-only bridge." + ) def get_sensor(self, uid: str) -> BaseSensor | None: """Get a sensor by its UID. @@ -2494,48 +3578,35 @@ def get_sensor_uid_list(self) -> List[str]: def remove_asset(self, uid: str) -> bool: """Remove an asset by its UID. - The asset can be a light, sensor, robot, rigid object or articulation. - - Note: - Currently, lights and sensors are not supported to be removed. + Native render lights are not removed by this method. Sensors and + Spawn-owned physical assets are supported. Args: uid (str): The UID of the asset. Returns: bool: True if the asset is removed successfully, otherwise False. """ - if uid in self._rigid_objects: - obj = self._rigid_objects.pop(uid) - obj.destroy() - self.notify_visualization_topology_changed() - return True - - if uid in self._soft_objects: - obj = self._soft_objects.pop(uid) - obj.destroy() - self.notify_visualization_topology_changed() - return True - - if uid in self._cloth_objects: - obj = self._cloth_objects.pop(uid) - obj.destroy() + if uid in self._sensors: + sensor = self._sensors.pop(uid) + destroy = getattr(sensor, "destroy", None) + if callable(destroy): + destroy() self.notify_visualization_topology_changed() return True - if uid in self._rigid_object_groups: - group = self._rigid_object_groups.pop(uid) - group.destroy() - self.notify_visualization_topology_changed() - return True + scene = self._spawn_scene + if uid not in scene: + return False + if uid == "default_plane": + raise ValueError("The Spawn-owned default plane cannot be removed.") - if uid in self._articulations: - art = self._articulations.pop(uid) - art.destroy() - self.notify_visualization_topology_changed() - return True + was_materialized = scene.builder.is_finalized + scene.remove(uid) + if was_materialized: + self.prepare() - if uid in self._robots: - robot = self._robots.pop(uid) + robot = self._robots.get(uid) + if robot is not None: for key, gizmo in self.get_gizmo_items(): if gizmo.target is robot: self.disable_gizmo(key) @@ -2544,11 +3615,14 @@ def remove_asset(self, uid: str) -> bool: for key in self._disabled_robot_gizmos if key != uid and not key.startswith(f"{uid}:") } - robot.destroy() - self.notify_visualization_topology_changed() - return True - return False + self._rigid_objects.pop(uid, None) + self._rigid_object_groups.pop(uid, None) + self._deformable_objects.pop(uid, None) + self._articulations.pop(uid, None) + self._robots.pop(uid, None) + self.notify_visualization_topology_changed() + return True def draw_marker( self, @@ -3294,12 +4368,9 @@ def reset_objects_state( for uid, rigid_obj_group in self._rigid_object_groups.items(): if uid not in excluded_uids: rigid_obj_group.reset(env_ids) - for uid, soft_obj in self._soft_objects.items(): - if uid not in excluded_uids: - soft_obj.reset(env_ids) - for uid, cloth_obj in self._cloth_objects.items(): + for uid, deformable_obj in self._deformable_objects.items(): if uid not in excluded_uids: - cloth_obj.reset(env_ids) + deformable_obj.reset(env_ids) for uid, light in self._lights.items(): if uid not in excluded_uids: light.reset(env_ids) @@ -3402,6 +4473,48 @@ def _deferred_destroy(self) -> None: import sys, gc + # Release backend-owned views before Scene closes the native + # resources that back them. Newton also synchronizes its device here. + self.physics.prepare_for_teardown() + # Run wrapper destructors while their World is still alive. The later + # collections continue to break cycles left by the native teardown. + gc.collect() + + # Render-only cameras may be attached to Spawn articulation link + # nodes. Remove their Arena views before closing Scene, which + # releases those parent nodes, and before World.quit releases their + # CameraGroups. + for sensor in list(getattr(self, "_sensors", {}).values()): + try: + sensor.destroy() + except Exception as error: + logger.log_warning( + f"Failed to destroy sensor {getattr(sensor, 'uid', None)!r}: " + f"{error!r}" + ) + + if self._spawn_scene is not None: + # Release result-scoped batches/facades before closing the + # Scene and, finally, the World that owns native resources. + for registry_name in ( + "_rigid_objects", + "_rigid_object_groups", + "_deformable_objects", + "_articulations", + "_robots", + ): + for asset in getattr(self, registry_name, {}).values(): + if hasattr(asset, "_data"): + asset._data = None + if hasattr(asset, "_spawn_result"): + asset._spawn_result = None + if hasattr(asset, "_entities"): + asset._entities = [] + try: + self._spawn_scene.close() + finally: + self._spawn_scene = None + self.clean_materials() if self._env: @@ -3436,15 +4549,13 @@ def _sever_wrapper_refs(obj_registry): _sever_wrapper_refs("_rigid_objects") _sever_wrapper_refs("_constraints") _sever_wrapper_refs("_rigid_object_groups") - _sever_wrapper_refs("_soft_objects") - _sever_wrapper_refs("_cloth_objects") + _sever_wrapper_refs("_deformable_objects") _sever_wrapper_refs("_articulations") _sever_wrapper_refs("_robots") _sever_wrapper_refs("_sensors") _sever_wrapper_refs("_lights") # Explicitly clear Python references to trigger C++ object destructors - self._ps = None self._env = None self._world = None self._default_plane = None @@ -3501,3 +4612,12 @@ def flush_cleanup_queue() -> None: # At this point, wait for the C++ Scene to return to zero, since the stack is at the top level, there will definitely be no deadlock SimulationManager.wait_scene_destruction() + + +def get_physics_scene(instance_id: int = 0): + """Return the active physics scene from a SimulationManager instance. + + This is the unified EmbodiChain access point for code that previously + reached through ``dexsim.default_world().get_physics_scene()``. + """ + return SimulationManager.get_instance(instance_id).get_physics_scene() diff --git a/embodichain/lab/sim/spawn/__init__.py b/embodichain/lab/sim/spawn/__init__.py new file mode 100644 index 000000000..93e11beaa --- /dev/null +++ b/embodichain/lab/sim/spawn/__init__.py @@ -0,0 +1,36 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Translate EmbodiChain asset configs into DexSim Spawn descriptors.""" + +from __future__ import annotations + +from .descriptors import ( + articulation_desc_from_cfg, + rigid_desc_from_cfg, + surface_deformable_desc_from_cfg, + volume_deformable_desc_from_cfg, +) +from .usd import articulation_desc_from_usd, rigid_desc_from_usd + +__all__ = [ + "articulation_desc_from_cfg", + "articulation_desc_from_usd", + "rigid_desc_from_cfg", + "rigid_desc_from_usd", + "surface_deformable_desc_from_cfg", + "volume_deformable_desc_from_cfg", +] diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py new file mode 100644 index 000000000..d9b19c39b --- /dev/null +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -0,0 +1,1663 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Translate EmbodiChain asset configurations into DexSim Spawn descriptors. + +This module translates one EmbodiChain configuration into a canonical +descriptor carrying both the common physics values and the optional backend +extension blocks. The selected :mod:`dexsim.spawn` adapter remains the only +component that chooses between the Default and Newton backends. When supplied, the active +Newton solver type only prevents common contact values from being authored to +a solver that cannot consume them. + +Articulation source names come from the handles produced by normal backend +materialization. EmbodiChain owns regex/group selection, applies exact-name +typed properties, and explicitly rebuilds Newton once when those post-load +properties must be committed to its immutable model. +""" + +from __future__ import annotations + +from collections.abc import Sequence +from dataclasses import MISSING, dataclass, field, fields +import math +import numbers +import os +import warnings +from typing import TYPE_CHECKING + +import numpy as np +from dexsim.spawn import ( + ArticulationDesc, + ClothDesc, + ClothPhysicsDesc, + CollisionApproximation, + CollisionDesc, + DexsimCollisionDesc, + DexsimJointDesc, + DexsimPhysicsDesc, + GeometryDesc, + MaterialDesc, + NewtonCollisionDesc, + NewtonJointDesc, + ObjectDesc, + RenderDesc, + RigidBodyPhysicsDesc, + SoftBodyDesc, + SoftBodyMeshingDesc, + SoftBodyPhysicsDesc, +) +from dexsim.spawn.descs import NEWTON_CONTACT_SOLVER_FIELDS +from dexsim.types import ActorType, DriveType, LoadOption as DexsimLoadOption + +from embodichain.lab.sim.cfg import ( + _normalize_joint_target_mode, + ArticulationCfg, + CollisionPropertiesCfg, + DefaultCollisionPropertiesCfg, + DefaultRigidBodyPropertiesCfg, + MassPropertiesCfg, + NewtonCollisionPropertiesCfg, + NewtonRigidBodyMaterialCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, + RigidObjectCfg, + SurfaceDeformableObjectCfg, + VolumeDeformableObjectCfg, +) +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, MeshCollisionCfg, SphereCfg +from embodichain.utils import logger +from embodichain.utils.math import convert_quat +from embodichain.utils.string import ( + resolve_matching_names, + resolve_matching_names_values, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.material import VisualMaterialCfg + +__all__ = [ + "articulation_desc_from_cfg", + "configure_articulation_desc", + "rigid_desc_from_cfg", + "surface_deformable_desc_from_cfg", + "volume_deformable_desc_from_cfg", +] + + +@dataclass +class _RigidPhysicsSpec: + """Canonical, backend-partitioned rigid-physics values.""" + + mass_props: dict[str, object] = field(default_factory=dict) + recompute_inertia: bool | None = None + default_rigid_props: dict[str, object] = field(default_factory=dict) + collision_enabled: bool | None = None + contact_offset: float | None = None + rest_offset: float | None = None + default_collision_props: dict[str, object] = field(default_factory=dict) + newton_collision_props: dict[str, object] = field(default_factory=dict) + material_props: dict[str, object] = field(default_factory=dict) + newton_material_props: dict[str, object] = field(default_factory=dict) + + def merged(self, override: _RigidPhysicsSpec) -> _RigidPhysicsSpec: + """Return ``override`` layered onto this spec using non-None values.""" + result = _RigidPhysicsSpec( + mass_props=dict(self.mass_props), + recompute_inertia=self.recompute_inertia, + default_rigid_props=dict(self.default_rigid_props), + collision_enabled=self.collision_enabled, + contact_offset=self.contact_offset, + rest_offset=self.rest_offset, + default_collision_props=dict(self.default_collision_props), + newton_collision_props=dict(self.newton_collision_props), + material_props=dict(self.material_props), + newton_material_props=dict(self.newton_material_props), + ) + for name in ( + "mass_props", + "default_rigid_props", + "default_collision_props", + "newton_collision_props", + "material_props", + "newton_material_props", + ): + getattr(result, name).update(getattr(override, name)) + if "mass" in override.mass_props: + mass = float(override.mass_props["mass"]) + if mass > 0.0: + result.mass_props.pop("density", None) + elif mass == 0.0 and "density" in result.mass_props: + result.mass_props.pop("mass", None) + elif "density" in override.mass_props: + result.mass_props.pop("mass", None) + if override.recompute_inertia is not None: + result.recompute_inertia = override.recompute_inertia + if override.collision_enabled is not None: + result.collision_enabled = override.collision_enabled + if override.contact_offset is not None: + result.contact_offset = override.contact_offset + if override.rest_offset is not None: + result.rest_offset = override.rest_offset + return result + + +def _configured_values(cfg: object | None) -> dict[str, object]: + """Return non-None configclass fields without backend metadata.""" + if cfg is None: + return {} + return { + item.name: value + for item in fields(cfg) + if (value := getattr(cfg, item.name)) is not None + } + + +def _resolve_rigid_physics( + cfg: RigidBodyPhysicsCfg, + *, + newton_solver_type: str | None = None, +) -> _RigidPhysicsSpec: + """Normalize grouped rigid-body configuration into one internal spec.""" + if isinstance(cfg, RigidBodyPhysicsCfg): + mass_props = _configured_values(cfg.mass_props) + recompute_inertia = mass_props.pop("recompute_inertia", None) + if recompute_inertia is not None and not isinstance( + recompute_inertia, (bool, np.bool_) + ): + raise TypeError("recompute_inertia must be a boolean or None.") + spec = _RigidPhysicsSpec( + mass_props=mass_props, + recompute_inertia=( + None if recompute_inertia is None else bool(recompute_inertia) + ), + collision_enabled=( + None + if cfg.collision_props is None + else cfg.collision_props.collision_enabled + ), + contact_offset=( + None + if cfg.collision_props is None + else cfg.collision_props.contact_offset + ), + rest_offset=( + None if cfg.collision_props is None else cfg.collision_props.rest_offset + ), + material_props={ + name: getattr(cfg.material_props, name) + for name in ("static_friction", "dynamic_friction", "restitution") + if cfg.material_props is not None + and getattr(cfg.material_props, name) is not None + }, + ) + + rigid_props = cfg.rigid_props + if isinstance(rigid_props, DefaultRigidBodyPropertiesCfg): + spec.default_rigid_props = _configured_values(rigid_props) + elif rigid_props is not None: + raise TypeError( + f"Unsupported rigid_props type {type(rigid_props).__name__!r}." + ) + + collision_props = cfg.collision_props + if isinstance(collision_props, DefaultCollisionPropertiesCfg): + spec.default_collision_props = _configured_values(collision_props) + for name in ("collision_enabled", "contact_offset", "rest_offset"): + spec.default_collision_props.pop(name, None) + elif isinstance(collision_props, NewtonCollisionPropertiesCfg): + values = _configured_values(collision_props) + for name in ("collision_enabled", "contact_offset", "rest_offset"): + values.pop(name, None) + spec.newton_collision_props = values + elif ( + collision_props is not None + and type(collision_props) is not CollisionPropertiesCfg + ): + raise TypeError( + f"Unsupported collision_props type {type(collision_props).__name__!r}." + ) + + material_props = cfg.material_props + if isinstance(material_props, NewtonRigidBodyMaterialCfg): + values = _configured_values(material_props) + for name in ("static_friction", "dynamic_friction", "restitution"): + values.pop(name, None) + if "torsional_friction" in values: + values["mu_torsional"] = values.pop("torsional_friction") + if "rolling_friction" in values: + values["mu_rolling"] = values.pop("rolling_friction") + spec.newton_material_props = values + elif ( + material_props is not None + and type(material_props) is not RigidBodyMaterialCfg + ): + raise TypeError( + f"Unsupported material_props type {type(material_props).__name__!r}." + ) + + return spec + + raise AssertionError("Unhandled grouped rigid-body physics configuration.") + + +def _with_procedural_collision_defaults( + physics: _RigidPhysicsSpec, +) -> _RigidPhysicsSpec: + """Apply the shared envelope to a newly authored shape when needed. + + Source USD/URDF descriptors retain sparse-overlay semantics. A procedural + shape with no portable or native envelope has no authored value to inherit, + so it receives the common portable defaults instead. Any explicit portable + offset or Newton ``margin``/``gap`` deliberately owns that envelope and is + left unchanged. + """ + if ( + physics.contact_offset is not None + or physics.rest_offset is not None + or physics.newton_collision_props.get("margin") is not None + or physics.newton_collision_props.get("gap") is not None + ): + return physics + defaults = CollisionPropertiesCfg() + return _RigidPhysicsSpec( + contact_offset=defaults.contact_offset, + rest_offset=defaults.rest_offset, + ).merged(physics) + + +def rigid_desc_from_cfg( + cfg: RigidObjectCfg, + *, + per_env: bool = True, + newton_solver_type: str | None = None, +) -> tuple[ObjectDesc, dict[str, MaterialDesc]]: + """Translate a rigid-object config into a DexSim Spawn descriptor.""" + uid = _required_uid(cfg.uid, "Rigid object") + if ( + isinstance(cfg.shape, MeshCfg) + and not _is_missing(cfg.shape.fpath) + and _is_usd_path(cfg.shape.fpath) + ): + raise NotImplementedError( + "USD files describe typed scenes; use rigid_desc_from_usd() to " + "select the sole rigid object." + ) + + physics = _with_procedural_collision_defaults( + _resolve_rigid_physics( + cfg.attrs, + newton_solver_type=newton_solver_type, + ) + ) + geometry, approximation, max_hulls = _compile_geometry(cfg) + material_ref, material_entry = _compile_visual_material( + uid, cfg.shape.visual_material + ) + collision = CollisionDesc.from_geometry( + geometry, + approximation=approximation, + ) + collision.enable_collision = physics.collision_enabled + collision.decomp_max_hulls = max_hulls + collision.dexsim = _compile_default_collision(physics) + collision.newton = _compile_newton_collision( + physics, + newton_solver_type=newton_solver_type, + author_shape_defaults=True, + mesh_collision=( + cfg.shape.collision if isinstance(cfg.shape, MeshCfg) else None + ), + ) + collision.render_source_index = 0 + + descriptor = ObjectDesc( + name=uid, + pose=_pose_from_cfg(cfg), + renders=[ + RenderDesc.from_geometry( + geometry, + load_option=_compile_load_option(cfg.shape), + material_ref=material_ref, + ) + ], + collisions=[collision], + physics=_compile_rigid_physics(physics, cfg.body_type), + per_env=per_env, + body_scale=_vector3(cfg.body_scale, field_name="body_scale"), + ) + materials = {} if material_entry is None else {material_entry[0]: material_entry[1]} + return descriptor, materials + + +def _particle_flags_from_cfg( + flags: int | Sequence[int] | np.ndarray | None, +) -> int | np.ndarray | None: + """Validate and copy optional Newton particle flags.""" + max_flag = int(np.iinfo(np.int32).max) + if flags is None: + return None + if np.isscalar(flags): + if isinstance(flags, (bool, np.bool_)) or not isinstance( + flags, numbers.Integral + ): + raise TypeError("particle_flags scalar must be an integer bitmask.") + value = int(flags) + if value < 0 or value > max_flag: + raise ValueError(f"particle_flags values must lie in [0, {max_flag}].") + return value + + values = np.asarray(flags) + if values.ndim != 1 or values.size == 0: + raise ValueError("particle_flags must be a non-empty one-dimensional array.") + if values.dtype.kind not in "iu": + raise TypeError("particle_flags array must contain integer bitmasks.") + if np.any(values < 0) or np.any(values > max_flag): + raise ValueError(f"particle_flags values must lie in [0, {max_flag}].") + return values.astype(np.int32, copy=True) + + +def _mesh_geometry_from_cfg(shape: MeshCfg, *, segment_name: str) -> GeometryDesc: + """Compile one file-backed or array-backed mesh configuration.""" + has_file = ( + not _is_missing(shape.fpath) + and shape.fpath is not None + and bool(str(shape.fpath).strip()) + ) + has_vertices = shape.vertices is not None + has_triangles = shape.triangles is not None + + if has_vertices != has_triangles: + raise ValueError( + "MeshCfg.vertices and MeshCfg.triangles must be provided together." + ) + if has_file and has_vertices: + raise ValueError( + "MeshCfg must provide either fpath or vertices/triangles, not both." + ) + if not has_file and not has_vertices: + raise ValueError( + "MeshCfg must provide a non-empty fpath or vertices/triangles." + ) + if has_file: + if shape.normals is not None or shape.uv_coords is not None: + raise ValueError( + "MeshCfg.normals and uv_coords require an array-backed mesh." + ) + return GeometryDesc.mesh(file_path=str(shape.fpath), segment_name=segment_name) + + vertices = np.asarray(shape.vertices, dtype=np.float32) + if vertices.ndim != 2 or vertices.shape[1:] != (3,) or len(vertices) == 0: + raise ValueError("MeshCfg.vertices must have non-empty shape (N, 3).") + if not np.isfinite(vertices).all(): + raise ValueError("MeshCfg.vertices must contain only finite values.") + + raw_triangles = np.asarray(shape.triangles) + if raw_triangles.dtype.kind not in "iu": + raise TypeError("MeshCfg.triangles must contain integer indices.") + if ( + raw_triangles.ndim != 2 + or raw_triangles.shape[1:] != (3,) + or len(raw_triangles) == 0 + ): + raise ValueError("MeshCfg.triangles must have non-empty shape (M, 3).") + if np.any(raw_triangles < 0) or np.any(raw_triangles >= len(vertices)): + raise ValueError("MeshCfg.triangles contain an out-of-range vertex index.") + triangles = raw_triangles.astype(np.int32, copy=True) + + normals = None + if shape.normals is not None: + normals = np.asarray(shape.normals, dtype=np.float32) + if normals.shape != vertices.shape or not np.isfinite(normals).all(): + raise ValueError( + f"MeshCfg.normals must have finite shape {vertices.shape}." + ) + normals = normals.copy() + + uv_coords = None + if shape.uv_coords is not None: + uv_coords = np.asarray(shape.uv_coords, dtype=np.float32) + if uv_coords.shape != (len(vertices), 2) or not np.isfinite(uv_coords).all(): + raise ValueError( + f"MeshCfg.uv_coords must have finite shape ({len(vertices)}, 2)." + ) + uv_coords = uv_coords.copy() + + return GeometryDesc.mesh( + vertices=vertices.copy(), + triangles=triangles, + normals=normals, + uv_coords=uv_coords, + segment_name=segment_name, + ) + + +def volume_deformable_desc_from_cfg( + cfg: VolumeDeformableObjectCfg, + *, + per_env: bool = True, +) -> tuple[SoftBodyDesc, dict[str, MaterialDesc]]: + """Translate a volume deformable into a Newton particle-set descriptor.""" + uid = _required_uid(cfg.uid, "Volume deformable") + geometry = _mesh_geometry_from_cfg(cfg.shape, segment_name=uid) + material_ref, material_entry = _compile_visual_material( + uid, cfg.shape.visual_material + ) + attrs = cfg.attrs + surface_props = attrs.surface_props + youngs = float(attrs.youngs) + poissons = float(attrs.poissons) + density = float(attrs.density) + particle_radius = ( + None if cfg.particle_radius is None else float(cfg.particle_radius) + ) + if not math.isfinite(youngs) or youngs < 0.0: + raise ValueError("Soft-body youngs must be a finite non-negative value.") + if not math.isfinite(poissons) or not -1.0 < poissons < 0.5: + raise ValueError("Soft-body poissons must be finite and lie in (-1, 0.5).") + if not math.isfinite(density) or density <= 0.0: + raise ValueError("Soft-body density must be a finite positive value.") + if particle_radius is not None and ( + not math.isfinite(particle_radius) or particle_radius <= 0.0 + ): + raise ValueError( + "Soft-body particle_radius must be finite and positive when set." + ) + + descriptor = SoftBodyDesc( + name=uid, + pose=_pose_from_cfg(cfg), + mesh=RenderDesc.from_geometry( + geometry, + load_option=_compile_load_option(cfg.shape), + material_ref=material_ref, + ), + physics=SoftBodyPhysicsDesc( + volume_density=density, + k_mu=youngs / (2.0 * (1.0 + poissons)), + k_lambda=(youngs * poissons / ((1.0 + poissons) * (1.0 - 2.0 * poissons))), + k_damp=float(attrs.elasticity_damping), + add_surface_edges=bool(attrs.add_surface_edges), + **{ + f"surface_{item.name}": ( + 0.0 + if (value := getattr(surface_props, item.name)) is None + else float(value) + ) + for item in fields(surface_props) + }, + ), + meshing=SoftBodyMeshingDesc( + proxy_simplify_target=cfg.meshing.triangle_simplify_target, + proxy_remesh_resolution=cfg.meshing.triangle_remesh_resolution, + voxel_resolution=cfg.meshing.simulation_mesh_resolution, + voxel_num_relaxation_iters=cfg.meshing.voxel_num_relaxation_iters, + voxel_rel_min_tet_volume=cfg.meshing.voxel_rel_min_tet_volume, + voxel_surface_dist_ratio=cfg.meshing.voxel_surface_dist_ratio, + embedding_impl=cfg.meshing.embedding_impl, + ), + particle_flags=_particle_flags_from_cfg(cfg.particle_flags), + particle_radius=particle_radius, + validate_mesh=cfg.validate_mesh, + per_env=per_env, + ) + materials = {} if material_entry is None else {material_entry[0]: material_entry[1]} + return descriptor, materials + + +def surface_deformable_desc_from_cfg( + cfg: SurfaceDeformableObjectCfg, + *, + per_env: bool = True, +) -> tuple[ClothDesc, dict[str, MaterialDesc]]: + """Translate a surface deformable into a Newton particle-set descriptor.""" + uid = _required_uid(cfg.uid, "Surface deformable") + geometry = _mesh_geometry_from_cfg(cfg.shape, segment_name=uid) + if cfg.visual_binding_mode not in {"auto", "nearest_vertex"}: + raise ValueError( + "Surface deformable visual_binding_mode must be 'auto' or " + f"'nearest_vertex'; got {cfg.visual_binding_mode!r}." + ) + render_shape = cfg.shape if cfg.visual_shape is None else cfg.visual_shape + visual_geometry = ( + None + if cfg.visual_shape is None + else _mesh_geometry_from_cfg( + cfg.visual_shape, + segment_name=f"{uid}_visual", + ) + ) + material_ref, material_entry = _compile_visual_material( + uid, render_shape.visual_material + ) + attrs = cfg.attrs + surface_props = attrs.surface_props + density = float(attrs.density) + particle_radius = ( + None if cfg.particle_radius is None else float(cfg.particle_radius) + ) + if not math.isfinite(density) or density <= 0.0: + raise ValueError("Cloth density must be a finite positive value.") + if particle_radius is not None and ( + not math.isfinite(particle_radius) or particle_radius <= 0.0 + ): + raise ValueError("Cloth particle_radius must be finite and positive when set.") + descriptor = ClothDesc( + name=uid, + pose=_pose_from_cfg(cfg), + mesh=( + RenderDesc.from_geometry( + geometry, + load_option=_compile_load_option(cfg.shape), + material_ref=material_ref, + ) + if visual_geometry is None + else geometry + ), + visual_mesh=( + None + if visual_geometry is None + else RenderDesc.from_geometry( + visual_geometry, + load_option=_compile_load_option(render_shape), + material_ref=material_ref, + ) + ), + visual_binding_mode=cfg.visual_binding_mode, + physics=ClothPhysicsDesc( + surface_density=density, + add_springs=bool(attrs.add_springs), + spring_ke=attrs.spring_ke, + spring_kd=attrs.spring_kd, + **_configured_values(surface_props), + ), + particle_flags=_particle_flags_from_cfg(cfg.particle_flags), + particle_radius=particle_radius, + validate_mesh=cfg.validate_mesh, + per_env=per_env, + ) + materials = {} if material_entry is None else {material_entry[0]: material_entry[1]} + return descriptor, materials + + +def articulation_desc_from_cfg( + cfg: ArticulationCfg, + *, + per_env: bool = True, + source_path: str | None = None, + newton_solver_type: str | None = None, +) -> ArticulationDesc: + """Translate an articulation config into a DexSim Spawn descriptor.""" + path = source_path if source_path is not None else cfg.fpath + if path is None or not str(path).strip(): + raise ValueError( + "No articulation source path is available. Assemble the robot URDF " + "before converting its configuration." + ) + if _is_usd_path(path): + raise NotImplementedError( + "USD files describe typed scenes; use articulation_desc_from_usd() " + "to select the sole articulation." + ) + if cfg.resolve_asset_physics_mode() == "overlay": + _validate_articulation_rigid_physics( + cfg, + newton_solver_type=newton_solver_type, + ) + fixed_base, self_collision_enabled = _articulation_root_values(cfg) + return ArticulationDesc( + name=_articulation_uid(cfg.uid, str(path)), + pose=_pose_from_cfg(cfg), + path=str(path), + urdf_path=str(path), + fixed_base=fixed_base, + enable_self_collision=self_collision_enabled, + urdf_fix_root_link=fixed_base, + # EmbodiChain's preserve/overlay policy starts from source-authored + # inertia. MassPropertiesCfg can request geometry-based recomputation + # after exact source names are available. + urdf_read_inertia=True, + per_env=per_env, + body_scale=_vector3(cfg.body_scale, field_name="body_scale"), + ) + + +def _validate_articulation_rigid_physics( + cfg: ArticulationCfg, + *, + newton_solver_type: str | None, +) -> None: + """Validate global and per-link physics before source materialization.""" + _resolve_rigid_physics( + cfg.attrs, + newton_solver_type=newton_solver_type, + ) + for group in (cfg.link_attrs or {}).values(): + _resolve_rigid_physics( + group.attrs, + newton_solver_type=newton_solver_type, + ) + + +def _articulation_root_values( + cfg: ArticulationCfg, + *, + fixed_base_default: bool = True, + self_collision_default: bool = False, +) -> tuple[bool, bool]: + """Resolve articulation-root values over source/import defaults.""" + props = cfg.root_props + fixed_base = ( + fixed_base_default if props.fixed_base is None else bool(props.fixed_base) + ) + self_collision_enabled = ( + self_collision_default + if props.self_collision_enabled is None + else bool(props.self_collision_enabled) + ) + return fixed_base, self_collision_enabled + + +def _configured_articulation_overlay_fields(cfg: ArticulationCfg) -> list[str]: + """Return physics overlay fields that preserve mode would ignore.""" + configured: list[str] = [] + if any( + _configured_values(group) + for group in ( + cfg.attrs.mass_props, + cfg.attrs.rigid_props, + cfg.attrs.collision_props, + cfg.attrs.material_props, + ) + ): + configured.append("attrs") + if cfg.link_attrs: + configured.append("link_attrs") + if _configured_values(cfg.joint_drive_props): + configured.append("joint_drive_props") + if cfg.qpos_limits is not None: + configured.append("qpos_limits") + return configured + + +def _compile_link_properties( + physics: _RigidPhysicsSpec, + *, + newton_solver_type: str | None, + author_newton_shape_defaults: bool, +) -> tuple[RigidBodyPhysicsDesc, CollisionDesc, bool]: + collision = CollisionDesc( + enable_collision=physics.collision_enabled, + dexsim=_compile_default_collision(physics), + newton=_compile_newton_collision( + physics, + newton_solver_type=newton_solver_type, + author_shape_defaults=author_newton_shape_defaults, + ), + ) + return ( + _compile_rigid_physics(physics, "dynamic"), + collision, + bool(physics.recompute_inertia), + ) + + +def configure_articulation_desc( + desc: ArticulationDesc, + cfg: ArticulationCfg, + *, + newton_solver_type: str | None = None, +) -> ArticulationDesc: + """Apply one EmbodiChain config to exact source-resolved names. + + Regex/default/group semantics remain private to EmbodiChain. The DexSim + descriptor receives only concrete link and joint properties. + """ + if not desc.links: + raise RuntimeError( + f"Articulation source {desc.name!r} must be resolved before " + "configuration." + ) + preserve_source_physics = cfg.resolve_asset_physics_mode() == "preserve" + setattr(desc, "_embodichain_preserve_source_physics", preserve_source_physics) + for link in desc.links: + # The Default adapter receives a source snapshot for descriptor/query + # parity, but it must only call its native physical-attribute setter + # for an actual user overlay. Keep that decision attached to the exact + # resolved link so both backends share the same ownership boundary. + setattr(link, "_embodichain_apply_physics", False) + setattr(link, "_embodichain_mass_override", False) + if preserve_source_physics: + configured_fields = _configured_articulation_overlay_fields(cfg) + if configured_fields: + warnings.warn( + "asset_physics_mode='preserve' ignores configured articulation " + f"physics overlays: {', '.join(configured_fields)}. Set " + "asset_physics_mode='overlay' to apply them.", + UserWarning, + stacklevel=2, + ) + return desc + default_physics = _resolve_rigid_physics( + cfg.attrs, + newton_solver_type=newton_solver_type, + ) + author_newton_shape_defaults = not _is_usd_path(cfg.fpath) + default_link_properties = _compile_link_properties( + default_physics, + newton_solver_type=newton_solver_type, + author_newton_shape_defaults=author_newton_shape_defaults, + ) + link_properties = {link.name: default_link_properties for link in desc.links} + + claimed_links: dict[str, str] = {} + link_names = [link.name for link in desc.links] + for group_name, group in (cfg.link_attrs or {}).items(): + _, matched_names = resolve_matching_names( + group.link_names_expr, + link_names, + ) + group_properties = _compile_link_properties( + default_physics.merged( + _resolve_rigid_physics( + group.attrs, + newton_solver_type=newton_solver_type, + ) + ), + newton_solver_type=newton_solver_type, + author_newton_shape_defaults=author_newton_shape_defaults, + ) + for link_name in matched_names: + previous = claimed_links.get(link_name) + if previous is not None: + raise ValueError( + f"Link {link_name!r} matches both {previous!r} and " + f"{group_name!r}." + ) + claimed_links[link_name] = group_name + link_properties[link_name] = group_properties + + ( + joint_properties, + joint_common, + joint_limits, + joint_target_modes, + ) = _compile_joint_properties( + desc, + cfg, + newton_solver_type=newton_solver_type, + ) + + # Commit only after every regex, value, and limit has been validated. Each + # source-resolved item receives one exact-name update. + for link_name, ( + rigid_body, + collision, + recompute_inertia, + ) in link_properties.items(): + link = desc.get_link_desc(link_name) + source_inertia_invalid = ( + getattr(link, "_embodichain_source_inertia_valid", None) is False + ) + source_has_collision_geometry = ( + getattr(link, "_embodichain_has_collision_geometry", None) is True + ) + # An all-zero/invalid source tensor is not an asset value that either + # backend can safely preserve. Newton already derives its fallback + # from shapes; make that fallback explicit in the shared descriptor so + # Default does the same instead of retaining its native epsilon tensor. + source_geometry_fallback = ( + source_inertia_invalid and source_has_collision_geometry + ) + # A source link without collision geometry has no quantity from which + # to recompute inertia. Keep its source/fallback tensor instead of + # asking Newton to apply a mass override to an empty shape set. The + # marker is attached by the URDF source resolver; explicit descriptors + # without the marker retain the historical, caller-controlled behavior. + effective_recompute = recompute_inertia or source_geometry_fallback + if ( + effective_recompute + and getattr(link, "_embodichain_has_collision_geometry", None) is False + ): + effective_recompute = False + apply_physics = _has_articulation_link_physics_overlay( + rigid_body, + collision, + recompute_inertia=effective_recompute, + ) + setattr(link, "_embodichain_apply_physics", apply_physics) + setattr(link, "_embodichain_mass_override", rigid_body.mass is not None) + if not apply_physics: + continue + if ( + rigid_body.density is not None + and not effective_recompute + and getattr(link, "_embodichain_source_inertia_valid", None) is True + ): + raise ValueError( + f"Density override for source-backed link {link_name!r} requires " + "mass_props.recompute_inertia=True. Density derives mass " + "properties and cannot preserve the source inertia." + ) + desc.set_link_properties( + link_name, + rigid_body=rigid_body, + # The URDF resolver intentionally keeps source-owned collision + # geometry outside LinkDesc. An attribute-only CollisionDesc is + # still required so the adapters can overlay properties onto the + # native source shapes; it does not synthesize geometry. Explicit + # descriptors, including collisionless links, remain unchanged. + collision=( + collision if link.collisions or desc.urdf_path is not None else None + ), + replace_inertial=effective_recompute, + ) + for joint_name, (default_desc, newton_desc) in joint_properties.items(): + lower_limit, upper_limit = joint_limits.get(joint_name, (None, None)) + common = joint_common[joint_name] + desc.set_joint_properties( + joint_name, + lower_limit=lower_limit, + upper_limit=upper_limit, + effort_limit=common.get("effort_limit"), + velocity_limit=common.get("velocity_limit"), + armature=common.get("armature"), + dexsim=default_desc, + newton=newton_desc, + newton_target_mode=joint_target_modes.get(joint_name), + ) + return desc + + +def _has_articulation_link_physics_overlay( + rigid_body: RigidBodyPhysicsDesc, + collision: CollisionDesc, + *, + recompute_inertia: bool, +) -> bool: + """Return whether an exact link needs a native physics write. + + ``RigidBodyPhysicsDesc.dynamic()`` is the compilation container for a + sparse configuration; its actor type alone is not an authored override. + Avoiding a no-op native setter matters for source URDFs because the + Default backend otherwise derives a new tensor from collision geometry. + """ + if recompute_inertia: + return True + if any( + getattr(rigid_body, name) is not None + for name in ( + "mass", + "density", + "inertia", + "com_position", + "com_quaternion", + "collision_filter_data", + "dexsim", + "newton", + ) + ): + return True + return any( + getattr(collision, name) is not None + for name in ("enable_collision", "dexsim", "newton") + ) + + +def _compile_joint_properties( + desc: ArticulationDesc, + cfg: ArticulationCfg, + *, + newton_solver_type: str | None, +) -> tuple[ + dict[str, tuple[DexsimJointDesc, NewtonJointDesc]], + dict[str, dict[str, float]], + dict[str, tuple[object, object]], + dict[str, int], +]: + joint_names = [joint.name for joint in desc.joints] + control_parts = getattr(cfg, "control_parts", None) + target_mode_cfg: object = None + drive_type: str | None = None + if cfg.joint_drive_props is not None: + target_mode_cfg, drive_type = cfg.joint_drive_props._resolve_modes() + + joint_target_modes: dict[str, int] = {} + if target_mode_cfg is not None: + matches = _joint_property_matches( + target_mode_cfg, + joint_names, + property_name="target_mode", + numeric_only=False, + control_parts=control_parts, + ) + for joint_name, value in matches: + joint_target_modes[joint_name] = _normalize_joint_target_mode(value) + + # A scalar drive type remains the fallback for joints not selected by an + # explicit target-mode rule. The established force drive activates both + # position and velocity targets. + if drive_type is not None: + fallback_target_mode = 0 if drive_type == "none" else 3 + for joint_name in joint_names: + joint_target_modes.setdefault(joint_name, fallback_target_mode) + + active_joints = [ + name for name, mode in joint_target_modes.items() if mode in {1, 2, 3} + ] + if drive_type == "none" and active_joints: + raise ValueError( + "drive_type='none' conflicts with an active joint target_mode; " + "use target_mode='none' or 'effort'." + ) + if newton_solver_type is not None and drive_type == "acceleration": + if active_joints: + raise NotImplementedError( + "Newton Spawn does not have an exact acceleration-drive " + "equivalent; use drive_type='force' or disable the drive." + ) + + default_drive_mode = { + None: None, + "force": DriveType.FORCE, + "acceleration": DriveType.ACCELERATION, + "none": DriveType.NONE, + }[drive_type] + joint_properties = { + joint_name: ( + DexsimJointDesc( + drive_mode=( + DriveType.NONE + if joint_target_modes.get(joint_name) in {0, 4} + else ( + ( + default_drive_mode + if default_drive_mode is not None + else DriveType.FORCE + ) + if joint_target_modes.get(joint_name) in {1, 2, 3} + else None + ) + ) + ), + NewtonJointDesc(), + ) + for joint_name in joint_names + } + joint_common: dict[str, dict[str, float]] = { + joint_name: {} for joint_name in joint_names + } + property_fields = { + "stiffness": ("stiffness", "target_ke"), + "damping": ("damping", "target_kd"), + "friction": ("joint_friction", "friction"), + } + for property_name in ("stiffness", "damping"): + if cfg.joint_drive_props is None: + continue + configured = getattr(cfg.joint_drive_props, property_name) + if configured is None: + continue + matches = _joint_property_matches( + configured, + joint_names, + property_name=property_name, + control_parts=control_parts, + ) + for joint_name, value in matches: + if not isinstance(value, numbers.Number): + raise TypeError( + f"Articulation drive rule for {joint_name!r} and " + f"{property_name!r} must contain a numeric value." + ) + scalar = float(value) + default_desc, newton_desc = joint_properties[joint_name] + default_field, newton_field = property_fields[property_name] + setattr(default_desc, default_field, scalar) + setattr(newton_desc, newton_field, scalar) + + if cfg.joint_drive_props is not None: + source = cfg.joint_drive_props + for property_name in ( + "max_effort", + "max_velocity", + "friction", + "armature", + ): + configured = getattr(source, property_name) + if configured is None: + continue + matches = _joint_property_matches( + configured, + joint_names, + property_name=property_name, + control_parts=control_parts, + ) + for joint_name, value in matches: + if not isinstance(value, numbers.Number): + raise TypeError( + f"Articulation joint rule for {joint_name!r} and " + f"{property_name!r} must contain a numeric value." + ) + scalar = float(value) + default_desc, newton_desc = joint_properties[joint_name] + if property_name == "armature": + joint_common[joint_name]["armature"] = scalar + elif property_name == "max_effort": + default_desc.max_force = scalar + joint_common[joint_name]["effort_limit"] = scalar + elif property_name == "max_velocity": + default_desc.max_velocity = scalar + joint_common[joint_name]["velocity_limit"] = scalar + else: + default_field, newton_field = property_fields[property_name] + setattr(default_desc, default_field, scalar) + setattr(newton_desc, newton_field, scalar) + + # Solvers that ignore Newton's target-mode enum still consume drive gains. + # Masking inactive components makes NONE, EFFORT, and VELOCITY deterministic + # across the currently supported solver set. + for joint_name, target_mode in joint_target_modes.items(): + default_desc, newton_desc = joint_properties[joint_name] + if target_mode in {0, 4}: + default_desc.stiffness = 0.0 + default_desc.damping = 0.0 + newton_desc.target_ke = 0.0 + newton_desc.target_kd = 0.0 + elif target_mode == 2: + default_desc.stiffness = 0.0 + newton_desc.target_ke = 0.0 + + normalized_solver = ( + None + if newton_solver_type is None + else newton_solver_type.replace("-", "_").lower() + ) + if normalized_solver not in {None, "auto", "mujoco_warp", "mjwarp"} and any( + mode == 1 for mode in joint_target_modes.values() + ): + warnings.warn( + f"Newton solver {newton_solver_type!r} does not consume " + "joint_target_mode. POSITION is emulated with its configured " + "gains and assumes the velocity target remains zero.", + UserWarning, + stacklevel=3, + ) + + joint_limits = _compile_joint_limits(desc, cfg) + + return joint_properties, joint_common, joint_limits, joint_target_modes + + +def _joint_limit_array(value: object) -> np.ndarray: + """Convert a tensor/array/sequence limit value to a CPU NumPy array.""" + if hasattr(value, "detach"): + value = value.detach().cpu().numpy() + return np.asarray(value, dtype=np.float32) + + +def _compile_joint_limits( + desc: ArticulationDesc, + cfg: ArticulationCfg, +) -> dict[str, tuple[object, object]]: + """Compile regex or flattened-DOF joint limits before backend build.""" + joint_limits: dict[str, tuple[object, object]] = {} + if cfg.qpos_limits is None: + return joint_limits + + joint_names = [joint.name for joint in desc.joints] + if isinstance(cfg.qpos_limits, dict): + indices, _, values = resolve_matching_names_values( + cfg.qpos_limits, + joint_names, + ) + for index, limits in zip(indices, values): + limit_values = _joint_limit_array(limits).reshape(-1) + if limit_values.size != 2: + raise ValueError( + f"qpos_limits for {joint_names[index]!r} must contain " + "[lower, upper]." + ) + lower_limit, upper_limit = map(float, limit_values) + if not math.isfinite(lower_limit) or not math.isfinite(upper_limit): + raise ValueError( + f"qpos_limits for {joint_names[index]!r} must be finite." + ) + if lower_limit > upper_limit: + raise ValueError( + f"qpos_limits for {joint_names[index]!r} has lower limit " + f"{lower_limit} greater than upper limit {upper_limit}." + ) + joint_limits[joint_names[index]] = (lower_limit, upper_limit) + return joint_limits + + dof_joints = [joint for joint in desc.joints if joint.dof_count > 0] + dof_count = sum(joint.dof_count for joint in dof_joints) + limit_values = _joint_limit_array(cfg.qpos_limits) + expected_shape = (dof_count, 2) + if tuple(limit_values.shape) != expected_shape: + raise ValueError( + "Array qpos_limits must have flattened source-resolved DOF shape " + f"{expected_shape}, got {tuple(limit_values.shape)}." + ) + if not np.isfinite(limit_values).all(): + raise ValueError("Array qpos_limits must contain only finite values.") + if np.any(limit_values[:, 0] > limit_values[:, 1]): + raise ValueError( + "Array qpos_limits contains a lower limit greater than its upper limit." + ) + + dof_start = 0 + for joint in dof_joints: + dof_stop = dof_start + joint.dof_count + joint_values = limit_values[dof_start:dof_stop] + if joint.dof_count == 1: + lower_limit: object = float(joint_values[0, 0]) + upper_limit: object = float(joint_values[0, 1]) + else: + lower_limit = joint_values[:, 0].copy() + upper_limit = joint_values[:, 1].copy() + joint_limits[joint.name] = (lower_limit, upper_limit) + dof_start = dof_stop + return joint_limits + + +def _joint_property_matches( + configured: object, + joint_names: list[str], + *, + property_name: str, + numeric_only: bool = True, + control_parts: dict[str, Sequence[str]] | None = None, +) -> list[tuple[str, object]]: + """Resolve scalar, regex, and robot control-part drive rules.""" + scalar_types = (numbers.Number,) if numeric_only else (numbers.Number, str) + if isinstance(configured, scalar_types): + return [(name, configured) for name in joint_names] + if isinstance(configured, dict): + control_parts = control_parts or {} + part_rules = { + name: value for name, value in configured.items() if name in control_parts + } + direct_rules = { + name: value + for name, value in configured.items() + if name not in control_parts + } + + resolved: dict[str, object] = {} + owners: dict[str, str] = {} + for part_name, value in part_rules.items(): + expressions = list(control_parts[part_name]) + if not expressions: + raise ValueError(f"Robot control part {part_name!r} has no joints.") + indices, _, _ = resolve_matching_names_values( + {expression: value for expression in expressions}, + joint_names, + ) + for index in indices: + joint_name = joint_names[index] + previous = owners.get(joint_name) + if previous is not None: + raise ValueError( + f"Joint {joint_name!r} is selected by both control " + f"parts {previous!r} and {part_name!r} for drive " + f"property {property_name!r}." + ) + resolved[joint_name] = value + owners[joint_name] = part_name + + if direct_rules: + indices, _, values = resolve_matching_names_values( + direct_rules, + joint_names, + ) + # Exact/regex joint rules intentionally override a broader control + # part rule, matching RobotCfg's public configuration contract. + for index, value in zip(indices, values): + resolved[joint_names[index]] = value + return [(name, resolved[name]) for name in joint_names if name in resolved] + expected = "number" if numeric_only else "string/integer" + raise TypeError( + f"Articulation drive property {property_name!r} must be a {expected} " + f"or regex-to-{expected} mapping." + ) + + +def _compile_rigid_physics( + physics: _RigidPhysicsSpec, + body_type: str, +) -> RigidBodyPhysicsDesc: + actor_types = { + "dynamic": ActorType.DYNAMIC, + "kinematic": ActorType.KINEMATIC, + "static": ActorType.STATIC, + } + try: + actor_type = actor_types[body_type] + except KeyError as exc: + raise ValueError( + f"Unsupported rigid body_type {body_type!r}; expected one of " + f"{tuple(actor_types)}." + ) from exc + + mass_value = physics.mass_props.get("mass") + density_value = physics.mass_props.get("density") + if mass_value is not None and float(mass_value) < 0: + raise ValueError("Rigid-body mass cannot be negative.") + if density_value is not None and float(density_value) <= 0: + raise ValueError("Rigid-body density must be positive.") + if mass_value == 0 and density_value is None: + raise ValueError("Rigid-body density is required when mass is zero.") + + inertia = _rigid_array( + physics.mass_props.get("inertia"), + field_name="inertia", + allowed_sizes=(3, 9), + ) + if inertia is not None and physics.recompute_inertia: + raise ValueError( + "Rigid-body inertia cannot be explicit when recompute_inertia is true." + ) + com_position = _rigid_array( + physics.mass_props.get("com_position"), + field_name="com_position", + allowed_sizes=(3,), + ) + com_quaternion = _rigid_array( + physics.mass_props.get("com_quaternion"), + field_name="com_quaternion", + allowed_sizes=(4,), + ) + if inertia is not None: + if mass_value is None or float(mass_value) <= 0: + raise ValueError("Explicit rigid-body inertia requires a positive mass.") + if inertia.size == 3 and (np.any(inertia <= 0.0) or np.allclose(inertia, 0.0)): + raise ValueError( + "Rigid-body inertia must contain positive principal moments." + ) + if inertia.size == 9: + inertia_matrix = inertia.reshape(3, 3) + if not np.allclose(inertia_matrix, inertia_matrix.T, atol=1.0e-6): + raise ValueError("Rigid-body inertia matrix must be symmetric.") + if np.any(np.linalg.eigvalsh(inertia_matrix) <= 0.0): + raise ValueError("Rigid-body inertia matrix must be positive definite.") + if com_quaternion is not None: + quaternion_norm = float(np.linalg.norm(com_quaternion)) + if quaternion_norm <= 1.0e-8: + raise ValueError("Rigid-body com_quaternion cannot be zero.") + com_quaternion = com_quaternion / quaternion_norm + # DexSim descriptors use wxyz; EmbodiChain configuration uses xyzw. + com_quaternion = convert_quat(com_quaternion, to="wxyz") + + if body_type != "static": + mass = ( + float(mass_value) + if mass_value is not None and float(mass_value) > 0 + else None + ) + density = ( + float(density_value) + if mass is None and density_value is not None and float(density_value) > 0 + else None + ) + else: + # Both backends ignore mass properties on static actors. Omitting them + # also avoids a Newton build warning for the common default cfg. + mass = None + density = None + inertia = None + com_position = None + com_quaternion = None + + if physics.default_rigid_props: + default_values = {item.name: None for item in fields(DexsimPhysicsDesc)} + default_values.update(physics.default_rigid_props) + default_desc = DexsimPhysicsDesc(**default_values) + else: + default_desc = None + return RigidBodyPhysicsDesc( + actor_type=actor_type, + mass=mass, + density=density, + inertia=inertia, + com_position=com_position, + com_quaternion=com_quaternion, + dexsim=default_desc, + newton=None, + ) + + +def _rigid_array( + value: object | None, + *, + field_name: str, + allowed_sizes: tuple[int, ...], +) -> np.ndarray | None: + """Validate and copy a rigid-body mass-property array.""" + if value is None: + return None + result = np.asarray(value, dtype=np.float32).reshape(-1) + if result.size not in allowed_sizes or not np.all(np.isfinite(result)): + expected = " or ".join(str(size) for size in allowed_sizes) + raise ValueError( + f"Rigid-body {field_name} must contain {expected} finite values." + ) + return result.copy() + + +def _common_collision_envelope( + physics: _RigidPhysicsSpec, +) -> tuple[float | None, float | None]: + """Validate and return the portable contact/rest envelope.""" + + def optional_float(value: object | None, field_name: str) -> float | None: + if value is None: + return None + try: + result = float(value) + except (TypeError, ValueError) as exc: + raise TypeError(f"{field_name} must be a finite number.") from exc + if not math.isfinite(result): + raise ValueError(f"{field_name} must be finite.") + return result + + contact_offset = optional_float(physics.contact_offset, "contact_offset") + rest_offset = optional_float(physics.rest_offset, "rest_offset") + if contact_offset is not None and contact_offset < 0.0: + raise ValueError("contact_offset must be non-negative.") + if ( + contact_offset is not None + and rest_offset is not None + and contact_offset < rest_offset + ): + raise ValueError("contact_offset must be no smaller than rest_offset.") + return contact_offset, rest_offset + + +def _compile_default_collision( + physics: _RigidPhysicsSpec, +) -> DexsimCollisionDesc | None: + values = dict(physics.material_props) + contact_offset, rest_offset = _common_collision_envelope(physics) + if contact_offset is not None: + values["contact_offset"] = contact_offset + if rest_offset is not None: + values["rest_offset"] = rest_offset + values.update(physics.default_collision_props) + if not values: + return None + configured = {item.name: None for item in fields(DexsimCollisionDesc)} + configured.update(values) + return DexsimCollisionDesc(**configured) + + +def _compile_newton_collision( + physics: _RigidPhysicsSpec, + *, + mesh_collision: MeshCollisionCfg | None = None, + newton_solver_type: str | None = None, + author_shape_defaults: bool = False, +) -> NewtonCollisionDesc | None: + # Keep partial descriptors sparse for source overlays. Once a newly authored + # shape has a Newton override, fill the Spawn margin/gap defaults because a + # non-None descriptor suppresses DexSim's descriptor factory defaults. + values = {field.name: None for field in fields(NewtonCollisionDesc)} + contact_offset, rest_offset = _common_collision_envelope(physics) + native_margin = physics.newton_collision_props.get("margin") + native_gap = physics.newton_collision_props.get("gap") + if rest_offset is not None: + values["margin"] = rest_offset + if contact_offset is not None and native_gap is None: + effective_margin = native_margin if native_margin is not None else rest_offset + if effective_margin is None: + if newton_solver_type is not None: + raise ValueError( + "Newton requires rest_offset (or a native margin) when a " + "portable contact_offset is configured." + ) + else: + try: + gap = contact_offset - float(effective_margin) + except (TypeError, ValueError) as exc: + raise TypeError( + "Newton collision margin must be a finite number." + ) from exc + if not math.isfinite(gap): + raise ValueError("Newton collision margin must be finite.") + if gap < 0.0: + raise ValueError( + "Newton collision margin must be no larger than contact_offset." + ) + values["gap"] = gap + values.update(physics.newton_collision_props) + if mesh_collision is not None and mesh_collision.approximation == "sdf": + values["force_sdf"] = True + for field_name in ( + "is_hydroelastic", + "sdf_narrow_band_range", + "sdf_target_voxel_size", + "sdf_texture_format", + "sdf_padding", + ): + value = getattr(mesh_collision, field_name) + if value is not None: + values[field_name] = value + if mesh_collision.sdf_resolution is not None: + values["sdf_max_resolution"] = int(mesh_collision.sdf_resolution) + values.update(physics.newton_material_props) + dynamic_friction = physics.material_props.get("dynamic_friction") + if dynamic_friction is not None: + values["mu"] = float(dynamic_friction) + solver_contact_fields = NEWTON_CONTACT_SOLVER_FIELDS.get(newton_solver_type) + restitution = physics.material_props.get("restitution") + if restitution is not None and ( + solver_contact_fields is None or "restitution" in solver_contact_fields + ): + values["restitution"] = float(restitution) + if all(value is None for value in values.values()): + return None + if author_shape_defaults: + defaults = NewtonCollisionDesc() + if values["margin"] is None: + values["margin"] = defaults.margin + if values["gap"] is None: + values["gap"] = defaults.gap + return NewtonCollisionDesc(**values) + + +def _compile_geometry( + cfg: RigidObjectCfg, +) -> tuple[GeometryDesc, CollisionApproximation, int]: + shape = cfg.shape + if isinstance(shape, MeshCfg): + geometry = _mesh_geometry_from_cfg(shape, segment_name=cfg.uid or "mesh") + collision_cfg = shape.collision or MeshCollisionCfg() + approximation = { + "convex_hull": CollisionApproximation.CONVEX_HULL, + "convex_decomposition": CollisionApproximation.CONVEX_DECOMPOSITION, + "triangle_mesh": CollisionApproximation.NONE, + "sdf": CollisionApproximation.SDF, + }[collision_cfg.approximation] + max_hulls = collision_cfg.max_hulls or 1 + acd_method = collision_cfg.acd_method or "coacd" + + if collision_cfg.approximation == "triangle_mesh" and cfg.body_type != "static": + raise ValueError( + "triangle_mesh collision is supported only for static rigid objects." + ) + + if shape.compute_uv: + logger.log_warning( + "Mesh UV projection is not represented by GeometryDesc and was " + "not applied." + ) + if ( + collision_cfg.approximation == "convex_decomposition" + and acd_method != "coacd" + ): + raise ValueError( + "Spawn supports only acd_method='coacd' for convex_decomposition." + ) + if collision_cfg.sdf_resolution is not None: + logger.log_warning( + "CollisionApproximation.SDF is preserved and Newton receives " + "sdf_max_resolution, but the DexSim descriptor does not expose " + "its cooking resolution." + ) + return ( + geometry, + approximation, + max(1, max_hulls), + ) + + if isinstance(shape, CubeCfg): + size = tuple(float(value) for value in shape.size) + if len(size) != 3 or any(value <= 0 for value in size): + raise ValueError("CubeCfg.size must contain three positive values.") + return GeometryDesc.cube(size), CollisionApproximation.NONE, 1 + + if isinstance(shape, SphereCfg): + if shape.radius <= 0: + raise ValueError("SphereCfg.radius must be positive.") + return ( + GeometryDesc.sphere(float(shape.radius)), + CollisionApproximation.NONE, + 1, + ) + + raise NotImplementedError( + f"RigidObjectCfg shape {type(shape).__name__!r} is not supported by " + "the Spawn converter; supported shapes are MeshCfg, CubeCfg, and SphereCfg." + ) + + +def _compile_load_option(shape: object) -> DexsimLoadOption | None: + """Translate mesh import options without leaking EmbodiChain config types.""" + if not isinstance(shape, MeshCfg): + return None + source = shape.load_option + option = DexsimLoadOption() + option.rebuild_normals = bool(source.rebuild_normals) + option.rebuild_tangent = bool(source.rebuild_tangent) + option.rebuild_3rdnormal = bool(source.rebuild_3rdnormal) + option.rebuild_3rdtangent = bool(source.rebuild_3rdtangent) + option.smooth = float(source.smooth) + return option + + +def _compile_visual_material( + object_uid: str, + cfg: VisualMaterialCfg | None, +) -> tuple[str | None, tuple[str, MaterialDesc] | None]: + if cfg is None: + return None, None + key = str(cfg.uid or f"{object_uid}_material") + base_color = tuple(float(value) for value in cfg.base_color) + if len(base_color) != 4: + raise ValueError("VisualMaterialCfg.base_color must be RGBA.") + emissive_rgb = tuple( + float(value) * float(cfg.emissive_intensity) for value in cfg.emissive + ) + if len(emissive_rgb) != 3: + raise ValueError("VisualMaterialCfg.emissive must be RGB.") + desc = MaterialDesc( + name=key, + base_color=base_color, + base_color_map=cfg.base_color_texture, + normal_map=cfg.normal_texture, + emissive=(*emissive_rgb, 1.0), + roughness=float(cfg.roughness), + roughness_map=cfg.roughness_texture, + metallic=float(cfg.metallic), + metallic_map=cfg.metallic_texture, + ao_map=cfg.ao_texture, + ior=float(cfg.ior), + ) + return key, (key, desc) + + +def _pose_from_cfg(cfg: object) -> np.ndarray: + local_pose = getattr(cfg, "init_local_pose", None) + if local_pose is not None: + pose = np.asarray(local_pose, dtype=np.float32).reshape(4, 4).copy() + else: + position = _vector3(getattr(cfg, "init_pos"), field_name="init_pos") + rotation_deg = _vector3(getattr(cfg, "init_rot"), field_name="init_rot") + rx, ry, rz = np.deg2rad(rotation_deg) + cx, sx = math.cos(rx), math.sin(rx) + cy, sy = math.cos(ry), math.sin(ry) + cz, sz = math.cos(rz), math.sin(rz) + rot_x = np.array( + ((1.0, 0.0, 0.0), (0.0, cx, -sx), (0.0, sx, cx)), + dtype=np.float32, + ) + rot_y = np.array( + ((cy, 0.0, sy), (0.0, 1.0, 0.0), (-sy, 0.0, cy)), + dtype=np.float32, + ) + rot_z = np.array( + ((cz, -sz, 0.0), (sz, cz, 0.0), (0.0, 0.0, 1.0)), + dtype=np.float32, + ) + pose = np.eye(4, dtype=np.float32) + # Match EmbodiChain's shared matrix_from_euler(..., "XYZ") contract + # used by the legacy RigidObject reset path. + pose[:3, :3] = rot_x @ rot_y @ rot_z + pose[:3, 3] = position + + if not np.isfinite(pose).all(): + raise ValueError("init_local_pose must contain finite values.") + if not np.allclose(pose[3], (0.0, 0.0, 0.0, 1.0), atol=1e-6): + raise ValueError("init_local_pose must be a homogeneous 4x4 transform.") + return pose + + +def _vector3(value: object, *, field_name: str) -> np.ndarray: + result = np.asarray(value, dtype=np.float32).reshape(-1) + if result.size != 3 or not np.isfinite(result).all(): + raise ValueError(f"{field_name} must contain three finite values.") + if field_name == "body_scale" and np.any(result <= 0): + raise ValueError("body_scale values must be positive.") + return result.copy() + + +def _required_uid(value: str | None, label: str) -> str: + if value is None or not str(value).strip(): + raise ValueError(f"{label} uid must be specified before Spawn conversion.") + uid = str(value) + if "/" in uid: + raise ValueError(f"{label} uid cannot contain '/': {uid!r}.") + return uid + + +def _articulation_uid(value: str | None, path: str | None) -> str: + if value is not None and str(value).strip(): + return _required_uid(str(value), "Articulation") + if path is None or not str(path).strip(): + raise ValueError( + "Articulation uid is required when its source path is unresolved." + ) + inferred = os.path.splitext(os.path.basename(str(path)))[0] + return _required_uid(inferred, "Articulation") + + +def _is_usd_path(path: object) -> bool: + return str(path).lower().endswith((".usd", ".usda", ".usdc")) + + +def _is_missing(value: object) -> bool: + # ``@configclass`` deepcopy can create a distinct _MISSING_TYPE instance. + return value is MISSING or isinstance(value, type(MISSING)) diff --git a/embodichain/lab/sim/spawn/scene.py b/embodichain/lab/sim/spawn/scene.py new file mode 100644 index 000000000..3d27441a1 --- /dev/null +++ b/embodichain/lab/sim/spawn/scene.py @@ -0,0 +1,359 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Thin EmbodiChain coordination around DexSim Spawn.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Callable, Literal + +__all__ = ["SpawnScene"] + +_AssetKind = Literal[ + "rigid_object", + "rigid_object_group", + "articulation", + "soft_object", + "cloth_object", +] + + +@dataclass(slots=True) +class _AssetDeclaration: + kind: _AssetKind + descriptor: Any + facade: Any | None + source_configurator: Callable[[Any], None] | None = None + + +class SpawnScene: + """Map EmbodiChain asset declarations onto one DexSim Spawn scene. + + DexSim owns declaration materialization, stable handles, and topology + revisions. EmbodiChain resolves and configures source metadata before the + first backend build so Newton does not materialize an articulation twice. + """ + + def __init__( + self, + world: Any, + *, + num_envs: int, + spacing: tuple[float, float, float] = (0.0, 0.0, 0.0), + ) -> None: + from dexsim.spawn import SceneBuilder + + self.builder = SceneBuilder(world) + self._num_envs = num_envs + self.builder.replicate( + count=num_envs, + spacing=spacing, + name_format="arena_{i}", + collision_policy="isolated", + ) + self._assets: dict[str, _AssetDeclaration] = {} + + @property + def arena_names(self) -> tuple[str, ...]: + """Names of the replicated per-environment Arenas.""" + return tuple(self.builder.replicate_plan.env_names()) + + def __contains__(self, uid: str) -> bool: + return uid in self._assets + + def declare( + self, + kind: _AssetKind, + uid: str, + descriptor: Any, + *, + facade: Any | None = None, + configure_source: Callable[[Any], None] | None = None, + ) -> None: + """Add a descriptor and associate it with an EmbodiChain facade.""" + if uid in self._assets: + raise ValueError(f"Spawn asset uid is already declared: {uid!r}.") + self._initialize_facade_declaration(facade) + declaration = _AssetDeclaration( + kind=kind, + descriptor=descriptor, + facade=facade, + source_configurator=configure_source, + ) + + if kind == "rigid_object_group": + declaration.descriptor = tuple( + self.builder.add_object(member) for member in descriptor + ) + else: + if ( + kind == "articulation" + and configure_source is not None + and (self.builder.is_finalized or self.builder.result is not None) + and self._can_resolve_before_materialization() + ): + self._resolve_articulation_source(descriptor) + configure_source(descriptor) + declaration.source_configurator = None + add_name = { + "rigid_object": "add_object", + "articulation": "add_articulation", + "soft_object": "add_soft_object", + "cloth_object": "add_cloth_object", + }[kind] + declaration.descriptor = getattr(self.builder, add_name)(descriptor) + self._assets[uid] = declaration + self._configure_materialized_source(uid) + handles = self.handles(uid) + if facade is not None and handles: + facade.attach_spawn_handles(handles) + + def resolve_sources(self) -> None: + """Resolve and configure declarations before backend materialization.""" + if self.builder.is_finalized: + return + + builder_resolver = getattr(self.builder, "resolve_sources", None) + if builder_resolver is not None: + builder_resolver() + elif getattr(self.builder, "backend", None) == "newton": + for declaration in self._assets.values(): + if ( + declaration.kind == "articulation" + and declaration.source_configurator is not None + ): + self._resolve_articulation_source(declaration.descriptor) + else: + return + + for declaration in self._assets.values(): + configure = declaration.source_configurator + if configure is None: + continue + configure(declaration.descriptor) + declaration.source_configurator = None + + def track( + self, + kind: _AssetKind, + uid: str, + descriptor: Any, + *, + facade: Any | None = None, + ) -> None: + """Track a descriptor that was already added to ``SceneBuilder``.""" + if uid in self._assets: + raise ValueError(f"Spawn asset uid is already declared: {uid!r}.") + self._initialize_facade_declaration(facade) + declaration = _AssetDeclaration(kind, descriptor, facade) + self._assets[uid] = declaration + handles = self.handles(uid) + if facade is not None and handles: + facade.attach_spawn_handles(handles) + + def remove(self, uid: str) -> None: + """Remove a declared asset from its DexSim owner.""" + declaration = self._assets[uid] + if declaration.kind in {"soft_object", "cloth_object"}: + raise NotImplementedError( + "DexSim Spawn does not yet expose pending removal for " + f"{declaration.kind.replace('_', ' ')}." + ) + if declaration.kind == "rigid_object_group": + for member in declaration.descriptor: + self.builder.remove_object(member.name) + else: + remove_name = { + "rigid_object": "remove_object", + "articulation": "remove_articulation", + }[declaration.kind] + removed = getattr(self.builder, remove_name)(declaration.descriptor.name) + if removed is None: + raise KeyError(f"Spawn asset is absent from SceneBuilder: {uid!r}.") + del self._assets[uid] + + def commit(self) -> Any: + """Finalize once or let the current ``Scene`` consume pending changes.""" + if not self.builder.is_finalized: + self.resolve_sources() + result = self.builder.finalize() + else: + result = self.builder.result + assert result is not None + if self.builder.has_pending_changes or result.needs_rebuild: + result = result.rebuild(self.builder) + + for uid in self._assets: + self._configure_materialized_source(uid) + self.builder.result = result + return result + + def bind(self) -> None: + """Complete post-finalize runtime binding for declared facades. + + Native entity creation belongs to ``SceneBuilder`` and its backend + adapter. This method only attaches handles that were unavailable during + declaration, then lets each facade create its result-dependent + Batch/Data state through ``bind_spawn()``. Eager Default handles may + already be attached; deferred Newton handles are resolved here. + """ + result = self.builder.result + if result is None or not self.builder.is_finalized: + raise RuntimeError("Spawn scene must be materialized before binding.") + + for uid, declaration in self._assets.items(): + facade = declaration.facade + if facade is None or not facade.is_declared: + continue + if not facade._entities: + facade.attach_spawn_handles(self.handles(uid)) + facade.bind_spawn(result) + + def prepare_runtime_config(self, result: Any) -> None: + """Apply facade configuration required before backend initialization. + + Default Direct GPU simulation snapshots some native articulation + properties during initialization. Articulation facades therefore get + a narrow pre-bind hook after materialization but before the manager + initializes backend runtime buffers. + """ + if result is not self.builder.result or not self.builder.is_finalized: + raise RuntimeError("Spawn scene must be materialized before runtime setup.") + + for uid, declaration in self._assets.items(): + facade = declaration.facade + if facade is None or declaration.kind != "articulation": + continue + if not facade._entities: + facade.attach_spawn_handles(self.handles(uid)) + facade._prepare_spawn_runtime_config(result) + + def close(self) -> None: + """Release Spawn resources and facade references.""" + result = self.builder.result + if result is not None: + result.close() + self.builder.result = None + self._assets.clear() + + def handles(self, uid: str) -> tuple[Any, ...]: + """Return currently materialized handles for one logical asset.""" + result = self.builder.result + if result is None: + return () + declaration = self._assets[uid] + if declaration.kind == "rigid_object_group": + paths = tuple( + f"{arena}/{member.name}" + for arena in self.arena_names + for member in declaration.descriptor + ) + elif declaration.descriptor.per_env: + paths = tuple( + f"{arena}/{declaration.descriptor.name}" for arena in self.arena_names + ) + else: + paths = (declaration.descriptor.name,) + if any(path not in result.handles for path in paths): + return () + return tuple(result.handles[path] for path in paths) + + def _initialize_facade_declaration(self, facade: Any | None) -> None: + """Give a Spawn facade the instance count owned by this scene. + + The duck-typed fallback keeps ``SpawnScene`` usable by lightweight + callers that only need descriptor tracking and do not expose an + EmbodiChain object facade. + """ + if facade is None: + return + initialize = getattr(facade, "_initialize_spawn_declaration", None) + if initialize is not None: + initialize(self._num_envs) + + def _resolve_articulation_source(self, descriptor: Any) -> None: + """Resolve one descriptor through the available DexSim boundary.""" + builder_resolver = getattr( + self.builder, + "resolve_articulation_source", + None, + ) + if builder_resolver is not None: + builder_resolver(descriptor) + # Keep the invalid-source COM policy identical when a newer + # SceneBuilder supplies its own resolver implementation. + from embodichain.lab.sim.spawn.source import _clear_invalid_source_com + + _clear_invalid_source_com(descriptor) + return + + from embodichain.lab.sim.spawn.source import resolve_articulation_source + + resolve_articulation_source(self.builder, descriptor) + + def _can_resolve_before_materialization(self) -> bool: + """Return whether exact source metadata is available before add.""" + return ( + getattr(self.builder, "resolve_articulation_source", None) is not None + or getattr(self.builder, "backend", None) == "newton" + ) + + def _configure_materialized_source(self, uid: str) -> None: + """Apply a pending source config to an eager Default articulation.""" + declaration = self._assets[uid] + configure = declaration.source_configurator + if configure is None or declaration.kind != "articulation": + return + + handles = self.handles(uid) + if not handles: + return + + result = self.builder.result + assert result is not None + if result.backend != "dexsim": + raise RuntimeError( + "Newton articulation source configuration must run before " + "SceneBuilder.finalize()." + ) + + prototype = declaration.descriptor + source = ( + prototype + if getattr(prototype, "links", None) or getattr(prototype, "joints", None) + else handles[0].articulation_desc + ) + # The Default URDF loader owns the native source mass properties but + # does not copy them into ``LinkDesc``. Capture them before compiling + # the sparse overlay, then apply only explicitly configured link + # physics. This keeps the source tensor intact by default while still + # presenting both backends with one resolved descriptor contract. + from embodichain.lab.sim.spawn.source import ( + _apply_dexsim_source_overlay, + _capture_dexsim_source_physics, + _retain_dexsim_source_descriptor, + ) + + _capture_dexsim_source_physics(handles[0], source) + configure(source) + if getattr(source, "_embodichain_preserve_source_physics", False): + for handle in handles: + _retain_dexsim_source_descriptor(handle, source) + else: + for handle in handles: + _apply_dexsim_source_overlay(handle, source) + declaration.source_configurator = None diff --git a/embodichain/lab/sim/spawn/source.py b/embodichain/lab/sim/spawn/source.py new file mode 100644 index 000000000..6bad9e484 --- /dev/null +++ b/embodichain/lab/sim/spawn/source.py @@ -0,0 +1,610 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Normalize source-backed articulation physics for both Spawn backends.""" + +from __future__ import annotations + +import os +from dataclasses import dataclass, fields +from typing import TYPE_CHECKING, Any +from xml.etree import ElementTree + +import numpy as np +from dexsim.spawn import ( + ArticulationDesc, + CollisionDesc, + DexsimCollisionDesc, + DexsimPhysicsDesc, + RigidBodyPhysicsDesc, +) + +if TYPE_CHECKING: + from dexsim.spawn import SceneBuilder + +__all__ = ["resolve_articulation_source"] + + +@dataclass(frozen=True) +class _UrdfInertialState: + """Validity of one source ```` block. + + The native Default loader replaces an all-zero URDF inertia with a small + epsilon tensor. That replacement is useful as a native fallback, but it + must not be mistaken for an authored source inertia when EmbodiChain + applies a sparse overlay later. + """ + + has_inertial: bool + mass_valid: bool + inertia_valid: bool + has_collision_geometry: bool + + +def resolve_articulation_source( + builder: SceneBuilder, + desc: ArticulationDesc, +) -> ArticulationDesc: + """Populate exact URDF metadata without building a Newton model. + + DexSim 0.4.3 removed its public source-resolution phase while retaining + the same URDF-to-descriptor translator inside the Newton adapter. This + compatibility boundary invokes that translator with a disposable + render-only skeleton, allowing name-dependent EmbodiChain overlays to be + authored before :meth:`SceneBuilder.finalize`. + + Args: + builder: Scene builder that owns the target arena layout. + desc: Articulation descriptor to resolve in place. + + Returns: + The resolved descriptor. + """ + signature = _source_signature(desc) + previous = getattr(desc, "_embodichain_source_signature", None) + if previous == signature: + return desc + + if desc.urdf_path is None: + setattr(desc, "_embodichain_source_signature", signature) + return desc + + had_retained_links = bool(desc.links) + if previous is not None: + desc.links = [] + desc.joints = [] + desc.root_link_name = None + + arena = _source_arena(builder, desc) + temp_name = f"__embodichain_resolve__{desc.name.replace('/', '__')}__{id(desc)}" + skeleton = arena.create_skeleton("skeleton") + if skeleton is None: + raise RuntimeError(f"Failed to create a source resolver for {desc.name!r}.") + skeleton.set_name(temp_name) + skeleton.detach_parent() + try: + scale = np.asarray(desc.body_scale, dtype=np.float32).reshape(3) + load_result = skeleton.load_urdf(os.path.abspath(desc.urdf_path), scale) + if load_result != 0: + raise RuntimeError( + f"Skeleton.load_urdf({desc.urdf_path!r}) failed: {load_result}" + ) + + # DexSim currently exposes no public metadata-only resolver. Reuse the + # adapter's source translator so its retained descriptor semantics stay + # identical to the subsequent Newton build. + from dexsim.spawn.adapters.newton_articulation_adapter import ( + _translate_urdf_articulation, + ) + + collision_link_names = set() + for link_name in skeleton.get_link_names(True): + try: + if skeleton.get_collision_shapes(link_name): + collision_link_names.add(link_name) + except (KeyError, RuntimeError, TypeError, AttributeError): + continue + _translate_urdf_articulation(skeleton, desc) + source_states = _read_urdf_inertial_states(desc.urdf_path) + _annotate_urdf_source_physics( + desc, + source_states, + collision_link_names, + ) + # A zero/invalid source tensor falls back to geometry in both backends. + # The Newton translator currently carries the URDF origin through even + # after rejecting that tensor; clear it before configuration can retain + # the value as an explicit COM override. Do not touch descriptors that + # already contain caller-authored links (those are not source defaults). + if previous is not None or not had_retained_links: + _clear_invalid_source_com(desc) + finally: + # Drop the wrapper before deleting its Arena-owned native object. + skeleton = None + arena.remove_skeleton(temp_name) + + setattr(desc, "_embodichain_source_signature", signature) + return desc + + +def _capture_dexsim_source_physics( + handle: Any, + desc: ArticulationDesc, +) -> ArticulationDesc: + """Copy source physics into a materialized Default descriptor. + + DexSim's URDF adapter populates topology but intentionally leaves + ``LinkDesc.rigid_body`` empty. A later sparse overlay therefore calls the + native setter with an empty mass-property block, which derives geometry + inertia and silently replaces valid URDF inertia. Capture the complete + native body/contact snapshot before the config is merged, then let the + Default application boundary write only links with an explicit overlay. + + Invalid/all-zero source inertia is represented as ``inertia=None`` and + ``com_* = None``. Both backends then use collision geometry, while a + valid source mass remains an explicit mass override. + """ + if getattr(desc, "_embodichain_source_physics_captured", False): + return desc + + getter = getattr(handle, "get_physical_attr", None) + if getter is None or desc.urdf_path is None: + return desc + + source_states = _read_urdf_inertial_states(desc.urdf_path) + for link in desc.links: + try: + attrib = getter(link.name) + except (KeyError, RuntimeError, TypeError, AttributeError): + # A backend may expose a topology link without a physical body. + continue + + state = source_states.get(link.name) if source_states is not None else None + if state is None: + state = _infer_native_inertial_state(attrib) + mass = _finite_positive_scalar(getattr(attrib, "mass", None)) + inertia = _finite_vector(getattr(attrib, "inertia", None), 3) + com_position = _finite_vector(getattr(attrib, "com_position", None), 3) + com_quaternion = _finite_vector( + getattr(attrib, "com_quaternion", None), + 4, + ) + source_inertia_valid = bool( + state.has_inertial + and state.mass_valid + and state.inertia_valid + and mass is not None + and inertia is not None + and np.any(inertia > 0.0) + and com_position is not None + and com_quaternion is not None + and float(np.linalg.norm(com_quaternion)) > 1.0e-8 + ) + _set_source_link_markers( + link, + has_collision_geometry=state.has_collision_geometry, + inertia_valid=source_inertia_valid, + ) + + # Keep a descriptor snapshot even when the source has no usable + # inertial block. It preserves native damping/material values for a + # later partial overlay without claiming native fallback inertia as an + # authored asset value. + body = _native_source_rigid_body( + attrib, + mass=( + mass + if source_inertia_valid + or (state.has_inertial and state.mass_valid and mass is not None) + else None + ), + density=( + None + if state.has_inertial and state.mass_valid and mass is not None + else _finite_positive_scalar(getattr(attrib, "density", None)) + ), + inertia=inertia if source_inertia_valid else None, + com_position=com_position if source_inertia_valid else None, + com_quaternion=com_quaternion if source_inertia_valid else None, + ) + link.rigid_body = body + link.replace_inertial = False + link._inertia_from_source = source_inertia_valid + if state.has_collision_geometry: + link.collisions = [_native_source_collision(attrib)] + + setattr(desc, "_embodichain_source_physics_captured", True) + return desc + + +def _clear_invalid_source_com(desc: ArticulationDesc) -> None: + """Drop invalid source inertia/COM before geometric fallback is built.""" + for link in desc.links: + body = getattr(link, "rigid_body", None) + if ( + body is None + or getattr(link, "_embodichain_source_inertia_valid", None) is not False + or not getattr(link, "_embodichain_has_collision_geometry", False) + ): + continue + body.inertia = None + body.com_position = None + body.com_quaternion = None + link._inertia_from_source = False + + +def _annotate_urdf_source_physics( + desc: ArticulationDesc, + source_states: dict[str, _UrdfInertialState] | None, + collision_link_names: set[str], +) -> None: + """Attach source ownership markers to a Newton-resolved descriptor.""" + if source_states is None: + for link in desc.links: + setattr( + link, + "_embodichain_has_collision_geometry", + link.name in collision_link_names, + ) + return + + for link in desc.links: + state = source_states.get(link.name) + if state is None: + continue + inertia_valid = bool( + state.has_inertial and state.mass_valid and state.inertia_valid + ) + _set_source_link_markers( + link, + has_collision_geometry=link.name in collision_link_names, + inertia_valid=inertia_valid, + ) + if not inertia_valid: + link._inertia_from_source = False + + +def _set_source_link_markers( + link: Any, + *, + has_collision_geometry: bool, + inertia_valid: bool, +) -> None: + """Record source provenance without changing public Spawn descriptors.""" + setattr( + link, + "_embodichain_has_collision_geometry", + bool(has_collision_geometry), + ) + setattr(link, "_embodichain_source_inertia_valid", bool(inertia_valid)) + + +def _native_source_rigid_body( + attrib: Any, + *, + mass: float | None, + density: float | None, + inertia: np.ndarray | None, + com_position: np.ndarray | None, + com_quaternion: np.ndarray | None, +) -> RigidBodyPhysicsDesc: + """Convert one native ``PhysicalAttr`` into a sparse Spawn body snapshot.""" + dexsim_values = { + item.name: getattr(attrib, item.name, None) + for item in fields(DexsimPhysicsDesc) + } + return RigidBodyPhysicsDesc.dynamic( + mass=mass, + density=density, + inertia=inertia, + com_position=com_position, + com_quaternion=com_quaternion, + dexsim=DexsimPhysicsDesc(**dexsim_values), + ) + + +def _native_source_collision(attrib: Any) -> CollisionDesc: + """Convert native contact attributes into an attribute-only collision desc.""" + dexsim_values = { + item.name: getattr(attrib, item.name, None) + for item in fields(DexsimCollisionDesc) + } + return CollisionDesc( + enable_collision=bool(getattr(attrib, "enable_collision", True)), + dexsim=DexsimCollisionDesc(**dexsim_values), + ) + + +def _retain_dexsim_source_descriptor( + handle: Any, + desc: ArticulationDesc, +) -> ArticulationDesc | None: + """Retain the normalized source descriptor without issuing native writes.""" + binding = getattr(handle, "_physics_binding", None) + if binding is None or not hasattr(handle, "articulation_desc"): + return None + + from dexsim.spawn._copy import copy_articulation_desc + + previous = handle.articulation_desc + collision_filters = { + link.name: link.rigid_body.collision_filter_data.copy() + for link in getattr(previous, "links", ()) + if link.rigid_body is not None + and link.rigid_body.collision_filter_data is not None + } + effective = copy_articulation_desc(desc) + for link in effective.links: + collision_filter = collision_filters.get(link.name) + if collision_filter is not None and link.rigid_body is not None: + link.rigid_body.collision_filter_data = collision_filter + handle.articulation_desc = effective + if hasattr(handle, "_desc_shared"): + handle._desc_shared = False + return effective + + +def _apply_dexsim_source_overlay( + handle: Any, + desc: ArticulationDesc, +) -> None: + """Apply only explicit Default physics overlays to a loaded articulation. + + The public DexSim helper applies every non-empty ``LinkDesc``. EmbodiChain + intentionally retains a full source snapshot in those descriptors so that + source properties are visible and mergeable just like Newton's pre-build + descriptor. Applying that snapshot wholesale would nevertheless invoke + the Default native setter for every link and trigger unwanted geometric + inertia derivation. This narrow compatibility boundary keeps its native + calls limited to links marked by :func:`configure_articulation_desc`. + """ + binding = getattr(handle, "_physics_binding", None) + if binding is None: + # Keep light-weight Scene tests and third-party Spawn handles on the + # public DexSim path. Real Default SpawnedArticulations always expose + # the binding used below. + apply = getattr(handle, "apply_dexsim_properties", None) + if apply is not None: + apply(desc) + return + + effective = _retain_dexsim_source_descriptor(handle, desc) + if effective is None: + apply = getattr(handle, "apply_dexsim_properties", None) + if apply is not None: + apply(desc) + return + + from dexsim.spawn.adapters.common import physical_attr_for_dexsim + from dexsim.spawn.adapters.dexsim_adapter import ( + _apply_dexsim_joint_properties, + _apply_rigid_body_mass_properties, + ) + + get_body = getattr(binding, "get_physical_body", None) + set_attr = getattr(binding, "set_physical_attr", None) + if get_body is None or set_attr is None: + raise RuntimeError( + "Default Spawn articulation binding does not expose source " + "physical-property APIs." + ) + + for link in effective.links: + if not getattr(link, "_embodichain_apply_physics", True): + continue + physics = link.rigid_body + if physics is None: + continue + rigid_body = get_body(link.name) + if rigid_body is None: + continue + attr = physical_attr_for_dexsim(physics, link.collisions) + set_attr(attr, link.name, link.replace_inertial) + _apply_rigid_body_mass_properties( + rigid_body, + physics, + apply_inertia=( + effective.urdf_read_inertia or not link._inertia_from_source + ), + ) + _restore_dexsim_mass_override(rigid_body, link) + + if effective.joints: + _apply_dexsim_joint_properties(binding, effective.joints) + + +def _restore_dexsim_mass_override(rigid_body: Any, link: Any) -> None: + """Correct Default's non-replacing source-mass behavior. + + ``DFArticulationX::DX_SetPhysicAttrib(..., replace_inertial=False)`` keeps + the currently loaded mass even when the descriptor authors another one. + Newton honors that mass before its model is built. Apply it afterwards at + the raw body boundary and restore retained/explicit inertia and COM, which + makes the two backends follow the same descriptor contract. + """ + physics = getattr(link, "rigid_body", None) + if ( + physics is None + or link.replace_inertial + or not getattr(link, "_embodichain_mass_override", False) + or physics.mass is None + ): + return + + set_mass = getattr(rigid_body, "set_mass", None) + if set_mass is None: + return + set_mass(float(physics.mass)) + + if physics.inertia is not None: + set_inertia = getattr(rigid_body, "set_mass_space_inertia_tensor", None) + if set_inertia is not None: + set_inertia(np.asarray(physics.inertia, dtype=np.float32).reshape(-1)[:3]) + if physics.com_position is not None or physics.com_quaternion is not None: + get_com = getattr(rigid_body, "get_cmass_local_pose", None) + set_com = getattr(rigid_body, "set_cmass_local_pose", None) + if get_com is not None and set_com is not None: + position, quaternion = get_com() + if physics.com_position is not None: + position = np.asarray( + physics.com_position, + dtype=np.float32, + ).reshape( + -1 + )[:3] + if physics.com_quaternion is not None: + quaternion = np.asarray( + physics.com_quaternion, + dtype=np.float32, + ).reshape(-1)[:4] + set_com(position, quaternion) + + +def _read_urdf_inertial_states( + path: str, +) -> dict[str, _UrdfInertialState] | None: + """Read source inertial validity without changing the native asset.""" + try: + root = ElementTree.parse(path).getroot() + except (OSError, ElementTree.ParseError): + return None + + states: dict[str, _UrdfInertialState] = {} + for link_node in root.iter(): + if _xml_local_name(link_node.tag) != "link": + continue + link_name = link_node.attrib.get("name") + if not link_name: + continue + inertial_node = next( + (child for child in link_node if _xml_local_name(child.tag) == "inertial"), + None, + ) + if inertial_node is None: + states[link_name] = _UrdfInertialState( + False, + False, + False, + any(_xml_local_name(child.tag) == "collision" for child in link_node), + ) + continue + + mass_node = next( + (child for child in inertial_node if _xml_local_name(child.tag) == "mass"), + None, + ) + inertia_node = next( + ( + child + for child in inertial_node + if _xml_local_name(child.tag) == "inertia" + ), + None, + ) + mass = _parse_float( + None if mass_node is None else mass_node.attrib.get("value") + ) + values = ( + None + if inertia_node is None + else [ + _parse_float(inertia_node.attrib.get(name)) + for name in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz") + ] + ) + inertia_valid = False + if values is not None and all(value is not None for value in values): + matrix = np.asarray( + [ + [values[0], values[1], values[2]], + [values[1], values[3], values[4]], + [values[2], values[4], values[5]], + ], + dtype=np.float64, + ) + try: + inertia_valid = bool( + np.all(np.isfinite(matrix)) + and not np.allclose(matrix, 0.0) + and np.all(np.linalg.eigvalsh(matrix) >= 0.0) + ) + except np.linalg.LinAlgError: + inertia_valid = False + states[link_name] = _UrdfInertialState( + True, + mass is not None and bool(np.isfinite(mass) and mass > 0.0), + inertia_valid, + any(_xml_local_name(child.tag) == "collision" for child in link_node), + ) + return states + + +def _infer_native_inertial_state(attrib: Any) -> _UrdfInertialState: + """Conservative validity fallback when a source file cannot be parsed.""" + mass = _finite_positive_scalar(getattr(attrib, "mass", None)) + inertia = _finite_vector(getattr(attrib, "inertia", None), 3) + valid = inertia is not None and np.any(inertia > 0.0) + return _UrdfInertialState(True, mass is not None, bool(valid), True) + + +def _finite_positive_scalar(value: Any) -> float | None: + try: + scalar = float(value) + except (TypeError, ValueError): + return None + return scalar if np.isfinite(scalar) and scalar > 0.0 else None + + +def _finite_vector(value: Any, size: int) -> np.ndarray | None: + try: + array = np.asarray(value, dtype=np.float32).reshape(-1) + except (TypeError, ValueError): + return None + if array.size != size or not np.all(np.isfinite(array)): + return None + return array.copy() + + +def _parse_float(value: str | None) -> float | None: + try: + return float(value) if value is not None else None + except (TypeError, ValueError): + return None + + +def _xml_local_name(tag: str) -> str: + return str(tag).rsplit("}", 1)[-1] + + +def _source_signature(desc: ArticulationDesc) -> tuple[object, ...]: + if desc.urdf_path is None: + return "explicit", id(desc) + return ( + "urdf", + os.path.abspath(desc.urdf_path), + tuple(float(value) for value in np.asarray(desc.body_scale).reshape(3)), + ) + + +def _source_arena(builder: SceneBuilder, desc: ArticulationDesc) -> Any: + if desc.per_env and builder.replicate_plan is not None: + arenas = builder.prepare_arenas() + if not arenas: + raise RuntimeError( + f"No replicated Arena is available to resolve {desc.name!r}." + ) + return arenas[0] + return builder.world.get_env() diff --git a/embodichain/lab/sim/spawn/usd.py b/embodichain/lab/sim/spawn/usd.py new file mode 100644 index 000000000..84d68bf49 --- /dev/null +++ b/embodichain/lab/sim/spawn/usd.py @@ -0,0 +1,245 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Compatibility translation for EmbodiChain's singleton USD APIs.""" + +from __future__ import annotations + +import os +from dataclasses import fields, replace +from typing import TypeVar + +from dexsim.spawn import ( + ArticulationDesc, + CollisionDesc, + MaterialDesc, + ObjectDesc, + RenderDesc, + RigidBodyPhysicsDesc, +) +from dexsim.types import ActorType + +from embodichain.lab.sim.cfg import ArticulationCfg, RigidObjectCfg +from embodichain.lab.sim.spawn.descriptors import ( + _compile_default_collision, + _compile_newton_collision, + _compile_rigid_physics, + _compile_visual_material, + _articulation_root_values, + _pose_from_cfg, + _required_uid, + _resolve_rigid_physics, + _validate_articulation_rigid_physics, + _vector3, +) + +__all__ = ["articulation_desc_from_usd", "rigid_desc_from_usd"] + +_PropertyCfgT = TypeVar("_PropertyCfgT") + + +def _overlay_optional_properties( + source: _PropertyCfgT | None, + configured: _PropertyCfgT | None, +) -> _PropertyCfgT | None: + """Overlay non-None dataclass fields without erasing source values.""" + if configured is None: + return source + if source is None: + return configured + for item in fields(configured): + value = getattr(configured, item.name) + if value is not None: + setattr(source, item.name, value) + return source + + +def _overlay_rigid_body_properties( + source: RigidBodyPhysicsDesc | None, + configured: RigidBodyPhysicsDesc, + *, + recompute_inertia: bool = False, +) -> RigidBodyPhysicsDesc: + """Merge a partial body config into properties parsed from USD.""" + if source is None: + return configured + source.actor_type = configured.actor_type + source.dexsim = _overlay_optional_properties(source.dexsim, configured.dexsim) + source.newton = _overlay_optional_properties(source.newton, configured.newton) + if configured.mass is not None: + source.mass = configured.mass + source.density = None + elif configured.density is not None: + source.mass = None + source.density = configured.density + if recompute_inertia: + source.inertia = None + for name in ("inertia", "com_position", "com_quaternion"): + value = getattr(configured, name) + if value is not None: + setattr(source, name, value) + return source + + +def _overlay_collision_properties( + source: CollisionDesc, + configured: CollisionDesc, +) -> None: + """Merge partial contact properties while retaining parsed geometry.""" + if configured.enable_collision is not None: + source.enable_collision = configured.enable_collision + source.dexsim = _overlay_optional_properties(source.dexsim, configured.dexsim) + source.newton = _overlay_optional_properties(source.newton, configured.newton) + + +def rigid_desc_from_usd( + cfg: RigidObjectCfg, + *, + per_env: bool = True, + newton_solver_type: str | None = None, +) -> tuple[ObjectDesc, dict[str, MaterialDesc]]: + """Select the sole rigid object in a USD stage.""" + uid = _required_uid(cfg.uid, "Rigid object") + path = getattr(cfg.shape, "fpath", None) + scene, desc = _parse_singleton(path, "mesh_objects", "rigid object") + + desc.name = uid + desc.pose = _pose_from_cfg(cfg) + desc.per_env = per_env + materials = _namespace_materials(desc.renders, scene.materials, uid) + + if cfg.resolve_asset_physics_mode() == "preserve": + if desc.physics is None: + raise ValueError(f"USD rigid object {path!r} has no physics.") + cfg.body_type = { + ActorType.DYNAMIC: "dynamic", + ActorType.KINEMATIC: "kinematic", + ActorType.STATIC: "static", + }[desc.physics.actor_type] + cfg.body_scale = tuple(float(value) for value in desc.body_scale) + return desc, materials + + physics = _resolve_rigid_physics( + cfg.attrs, + newton_solver_type=newton_solver_type, + ) + configured_body = _compile_rigid_physics(physics, cfg.body_type) + desc.physics = _overlay_rigid_body_properties( + desc.physics, + configured_body, + recompute_inertia=bool(physics.recompute_inertia), + ) + desc.body_scale = _vector3(cfg.body_scale, field_name="body_scale") + for collision in desc.collisions: + _overlay_collision_properties( + collision, + CollisionDesc( + enable_collision=physics.collision_enabled, + dexsim=_compile_default_collision(physics), + newton=_compile_newton_collision( + physics, + newton_solver_type=newton_solver_type, + ), + ), + ) + + material_ref, material_entry = _compile_visual_material( + uid, + cfg.shape.visual_material, + ) + if material_entry is not None: + materials = {material_entry[0]: material_entry[1]} + for render in desc.renders: + render.material = None + render.material_ref = material_ref + return desc, materials + + +def articulation_desc_from_usd( + cfg: ArticulationCfg, + *, + per_env: bool = True, + source_path: str | None = None, + newton_solver_type: str | None = None, +) -> tuple[ArticulationDesc, dict[str, MaterialDesc]]: + """Select the sole articulation in a USD stage.""" + preserve_asset_physics = cfg.resolve_asset_physics_mode() == "preserve" + if not preserve_asset_physics: + _validate_articulation_rigid_physics( + cfg, + newton_solver_type=newton_solver_type, + ) + path = source_path or cfg.fpath + scene, desc = _parse_singleton(path, "articulations", "articulation") + uid = _required_uid( + cfg.uid or os.path.splitext(os.path.basename(str(path)))[0], + "Articulation", + ) + cfg.uid = uid + desc.name = uid + desc.pose = _pose_from_cfg(cfg) + desc.per_env = per_env + renders = [visual for link in desc.links for visual in link.visuals] + materials = _namespace_materials(renders, scene.materials, uid) + + if preserve_asset_physics: + cfg.body_scale = tuple(float(value) for value in desc.body_scale) + else: + desc.body_scale = _vector3(cfg.body_scale, field_name="body_scale") + desc.fixed_base, desc.enable_self_collision = _articulation_root_values( + cfg, + fixed_base_default=bool(desc.fixed_base), + self_collision_default=desc.enable_self_collision, + ) + return desc, materials + + +def _parse_singleton(path: object, collection: str, label: str): + if path is None: + raise ValueError(f"A USD path is required for the {label}.") + + from dexsim.kit.usd import parse_usd + + scene = parse_usd(str(path)) + candidates = getattr(scene, collection) + if len(candidates) != 1: + found = [ + (item.name, None if item.usd is None else item.usd.prim_path) + for item in candidates + ] + raise ValueError( + f"Expected exactly one {label} in USD file {path!r}, found " + f"{len(candidates)}: {found}." + ) + return scene, candidates[0] + + +def _namespace_materials( + renders: list[RenderDesc], + materials: dict[str, MaterialDesc], + uid: str, +) -> dict[str, MaterialDesc]: + selected = {} + for render in renders: + if render.material_ref is None: + continue + source_ref = render.material_ref + material = materials[source_ref] + render.material_ref = f"{uid}::{source_ref}" + selected[render.material_ref] = replace( + material, + name=f"{uid}::{material.name}", + ) + return selected diff --git a/embodichain/lab/sim/utility/cfg_utils.py b/embodichain/lab/sim/utility/cfg_utils.py index 722042513..19b0328a0 100644 --- a/embodichain/lab/sim/utility/cfg_utils.py +++ b/embodichain/lab/sim/utility/cfg_utils.py @@ -14,9 +14,38 @@ # limitations under the License. # ---------------------------------------------------------------------------- -from embodichain.lab.sim.cfg import RobotCfg +from typing import TypeVar + +from embodichain.lab.sim.cfg import ( + _raise_removed_articulation_cfg_fields, + JointDrivePropertiesCfg, + RigidBodyPhysicsCfg, + RobotCfg, +) +from embodichain.lab.sim.cfg.rigid import _rigid_body_physics_from_dict from embodichain.lab.sim.motion.solvers import SolverCfg -from embodichain.utils import logger +from embodichain.utils import is_configclass, logger + +_ConfigT = TypeVar("_ConfigT") + + +def _merge_non_none_config(base: _ConfigT | None, override: _ConfigT) -> _ConfigT: + """Merge non-None configclass fields without discarding base defaults.""" + if base is None: + return override + for field_name in override.__dataclass_fields__: + value = getattr(override, field_name) + if value is not None: + base_value = getattr(base, field_name) + if ( + base_value is not None + and type(base_value) is type(value) + and is_configclass(base_value) + ): + _merge_non_none_config(base_value, value) + else: + setattr(base, field_name, value) + return base def merge_solver_cfg( @@ -83,6 +112,8 @@ def merge_robot_cfg(base_cfg: RobotCfg, override_cfg_dict: dict[str, any]) -> Ro RobotCfg: The merged robot configuration. """ + _raise_removed_articulation_cfg_fields(override_cfg_dict) + # Only parse keys the base RobotCfg recognizes, so subclass-only variant # fields (version, ...) set by _build_defaults don't trigger # spurious "Key not found in RobotCfg" warnings from the base from_dict. @@ -142,30 +173,47 @@ def merge_robot_cfg(base_cfg: RobotCfg, override_cfg_dict: dict[str, any]) -> Ro f"new solver entry, or ensure the part name " f"matches an existing solver." ) - elif key == "drive_pros": + elif key == "joint_drive_props": # merge joint drive properties - user_drive_pros_dict = override_cfg_dict.get("drive_pros") - if isinstance(user_drive_pros_dict, dict): - for prop, val in user_drive_pros_dict.items(): + user_joint_drive_props_dict = override_cfg_dict.get("joint_drive_props") + if isinstance(user_joint_drive_props_dict, dict): + if user_joint_drive_props_dict.get("backend") == "newton": + base_cfg.joint_drive_props = JointDrivePropertiesCfg.from_dict( + user_joint_drive_props_dict, + defaults=base_cfg.joint_drive_props, + ) + continue + for prop, val in user_joint_drive_props_dict.items(): + if prop == "backend": + continue # Get the current value in cfg (which has defaults) - default_val = getattr(base_cfg.drive_pros, prop, None) + default_val = getattr(base_cfg.joint_drive_props, prop, None) if isinstance(val, dict) and isinstance(default_val, dict): # Merge dictionaries default_val.update(val) else: # Overwrite if not both dicts - setattr(base_cfg.drive_pros, prop, val) + setattr(base_cfg.joint_drive_props, prop, val) else: logger.log_warning( - "drive_pros should be a dictionary. Skipping drive_pros merge." + "joint_drive_props should be a dictionary. Skipping joint_drive_props merge." ) elif key == "attrs": # merge physics attributes user_attrs_dict = override_cfg_dict.get("attrs") if isinstance(user_attrs_dict, dict): - for attr_key, attr_val in user_attrs_dict.items(): - setattr(base_cfg.attrs, attr_key, attr_val) + grouped_fields = set(RigidBodyPhysicsCfg.__dataclass_fields__) + parsed = _rigid_body_physics_from_dict(user_attrs_dict) + for field_name in grouped_fields: + override = getattr(parsed, field_name) + if override is None: + continue + base = getattr(base_cfg.attrs, field_name) + if base is not None and type(base) is type(override): + _merge_non_none_config(base, override) + else: + setattr(base_cfg.attrs, field_name, override) else: logger.log_warning( "attrs should be a dictionary. Skipping attrs merge." diff --git a/embodichain/lab/sim/utility/keyboard_utils.py b/embodichain/lab/sim/utility/keyboard_utils.py index d64eca180..a7623e4ab 100644 --- a/embodichain/lab/sim/utility/keyboard_utils.py +++ b/embodichain/lab/sim/utility/keyboard_utils.py @@ -220,8 +220,7 @@ def run_keyboard_control_for_camera( quaternion = rot.as_quat() log_info("Current Camera pose:") log_info(f"Translation: {translation}") - quat_wxyz = [quaternion[3], quaternion[0], quaternion[1], quaternion[2]] - log_info(f"Quaternion (w, x, y, z): {quat_wxyz}") + log_info(f"Quaternion (x, y, z, w): {quaternion.tolist()}") rotation_euler = rot.as_euler("xyz", degrees=True) log_info(f"Rotation (XYZ Euler, degrees): {rotation_euler}") diff --git a/embodichain/lab/sim/utility/render_utils.py b/embodichain/lab/sim/utility/render_utils.py index d82bb2644..50d063c25 100644 --- a/embodichain/lab/sim/utility/render_utils.py +++ b/embodichain/lab/sim/utility/render_utils.py @@ -56,7 +56,7 @@ def select_default_renderer(gpu_id: int = 0) -> str: return cfg.DEFAULT_RENDERER if not torch.cuda.is_available(): - logger.log_info("No CUDA device available; defaulting renderer to 'hybrid'.") + logger.log_debug("No CUDA device available; defaulting renderer to 'hybrid'.") return "hybrid" try: @@ -70,18 +70,18 @@ def select_default_renderer(gpu_id: int = 0) -> str: upper_name = device_name.upper() if any(keyword in upper_name for keyword in _FAST_RT_GPU_KEYWORDS): - logger.log_info( + logger.log_debug( f"Detected datacenter GPU '{device_name}'; selecting 'fast-rt' renderer." ) return "fast-rt" if "RTX" in upper_name: - logger.log_info( + logger.log_debug( f"Detected RTX GPU '{device_name}'; selecting 'hybrid' renderer." ) return "hybrid" - logger.log_info( + logger.log_debug( f"Unrecognized GPU '{device_name}'; defaulting renderer to 'hybrid'." ) return "hybrid" diff --git a/embodichain/lab/sim/utility/sim_utils.py b/embodichain/lab/sim/utility/sim_utils.py index bb1ffe8bc..97bf2f73f 100644 --- a/embodichain/lab/sim/utility/sim_utils.py +++ b/embodichain/lab/sim/utility/sim_utils.py @@ -14,20 +14,24 @@ # limitations under the License. # ---------------------------------------------------------------------------- +from __future__ import annotations + import os +import warnings as _warnings + import dexsim import open3d as o3d -from dataclasses import MISSING -from typing import List, Union +from typing import TYPE_CHECKING, List, Union from dexsim.types import ( + CloneStrategy, DriveType, ArticulationFlag, LoadOption, + ObjectCloneOptions, RigidBodyShape, SDFConfig, - PhysicalAttr, ) from dexsim.engine import Articulation from dexsim.environment import Env, Arena @@ -35,17 +39,30 @@ from embodichain.lab.sim.cfg import ( ArticulationCfg, + ArticulationRootPropertiesCfg, LinkPhysicsOverrideCfg, + RigidBodyPhysicsCfg, RigidObjectCfg, - SoftObjectCfg, - ClothObjectCfg, + VolumeDeformableObjectCfg, + SurfaceDeformableObjectCfg, ) from embodichain.utils.string import resolve_matching_names -from embodichain.lab.sim.shapes import MeshCfg, CubeCfg, SphereCfg +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, MeshCollisionCfg, SphereCfg from embodichain.utils import logger from dexsim.kit.meshproc import get_mesh_auto_uv import numpy as np +if TYPE_CHECKING: + from dexsim.scene import SpawnedArticulation + + +def _is_newton_backend_active() -> bool: + """Return whether the current default world uses the Newton physics scene.""" + from embodichain.lab.sim.sim_manager import get_physics_scene + from embodichain.lab.sim.objects.backends import is_newton_scene + + return is_newton_scene(get_physics_scene()) + def get_dexsim_arenas() -> List[dexsim.environment.Arena]: """Get all arenas in the default dexsim world. @@ -77,33 +94,9 @@ def get_dexsim_arena_num() -> int: def _resolve_mesh_collision_params( cfg: RigidObjectCfg, -) -> tuple[int, str, int]: - """Resolve legacy and shape-level mesh collision parameters.""" - - def is_missing(value) -> bool: - # deepcopy() can produce a distinct instance of dataclasses.MISSING. - return value is MISSING or isinstance(value, type(MISSING)) - - max_convex_hull_num = next( - value - for value in ( - cfg.max_convex_hull_num, - cfg.shape.max_convex_hull_num, - 1, - ) - if not is_missing(value) - ) - acd_method = next( - value - for value in (cfg.acd_method, cfg.shape.acd_method, "visacd") - if not is_missing(value) - ) - sdf_resolution = next( - value - for value in (cfg.sdf_resolution, cfg.shape.sdf_resolution, 0) - if not is_missing(value) - ) - return max_convex_hull_num, acd_method, sdf_resolution +) -> MeshCollisionCfg: + """Resolve mesh collision parameters from the shape configuration.""" + return cfg.shape.collision or MeshCollisionCfg() def get_dexsim_drive_type(drive_type: str) -> DriveType: @@ -160,67 +153,276 @@ def _apply_link_physics_overrides( group_cfg = link_to_group.get(name) if group_cfg is None: continue - physical_attr = group_cfg.attrs.merge_with(cfg.attrs) - replace_inertial = group_cfg.replace_inertial or ( - group_cfg.attrs.mass is not None + base_attr = cfg.attrs.to_dexsim_physical_attr() + physical_attr = group_cfg.attrs.to_dexsim_physical_attr(base=base_attr) + mass_props = group_cfg.attrs.mass_props + recompute_inertia = bool( + mass_props is not None and mass_props.recompute_inertia + ) + art.set_physical_attr( + physical_attr, + name, + is_replace_inertial=recompute_inertia, ) - art.set_physical_attr(physical_attr, name, is_replace_inertial=replace_inertial) -def set_dexsim_articulation_cfg(arts: List[Articulation], cfg: ArticulationCfg) -> None: - """Set articulation configuration for a list of dexsim articulations. +def _warn_legacy_articulation_api(name: str) -> None: + _warnings.warn( + f"{name}() bypasses the Spawn ownership/configuration path and is " + "deprecated; declare the articulation through SimulationManager instead.", + DeprecationWarning, + stacklevel=3, + ) - Args: - arts (List[Articulation]): List of dexsim articulations to configure. - cfg (ArticulationCfg): Configuration object containing articulation settings. + +def _default_articulation_clone_options() -> ObjectCloneOptions: + options = ObjectCloneOptions() + options.render.material = CloneStrategy.DEEP_COPY + return options + + +def default_articulation_clone_options() -> ObjectCloneOptions: + """Return legacy articulation clone options. + + Deprecated: new scene code must use the Spawn declaration path. """ + _warn_legacy_articulation_api("default_articulation_clone_options") + return _default_articulation_clone_options() + + +def default_rigid_object_clone_options() -> ObjectCloneOptions: + """Return clone options used when duplicating rigid actors across arenas.""" + options = ObjectCloneOptions() + options.render.material = CloneStrategy.DEEP_COPY + return options + + +def _clone_actor_between_arenas( + source_arena: Arena | Env, + source_name: str, + target_arena: Arena | Env, + target_name: str, + clone_options: ObjectCloneOptions, +) -> MeshObject: + """Clone a mesh actor from one arena/env to another.""" + return source_arena.clone_actor_to( + source_name, target_arena, target_name, clone_options + ) + - def get_drive_type(drive_pros): - if isinstance(drive_pros, dict): - return drive_pros.get("drive_type", None) - return getattr(drive_pros, "drive_type", None) +def _clone_articulation_between_arenas( + source_arena: Arena | Env, + source_name: str, + target_arena: Arena | Env, + target_name: str, + clone_options: ObjectCloneOptions, +) -> Articulation: + """Clone an articulation from one arena/env to another.""" + if _is_newton_backend_active(): + return source_arena.clone_skeleton_to( + source_name, target_arena, target_name, clone_options + ) + return source_arena.clone_articulation_to( + source_name, target_arena, target_name, clone_options + ) - drive_pros = getattr(cfg, "drive_pros", None) - drive_type = get_drive_type(drive_pros) if drive_pros is not None else None - if drive_type == "force": - drive_type = DriveType.FORCE - elif drive_type == "acceleration": - drive_type = DriveType.ACCELERATION - elif drive_type == "none": - drive_type = DriveType.NONE - else: - logger.log_error(f"Unknow drive type {drive_type}") +def spawn_articulation_entities( + cfg: ArticulationCfg, + env_list: list[Arena | Env], + *, + clone_options: ObjectCloneOptions | None = None, +) -> list[Articulation]: + """Load one articulation prototype and clone it into additional arenas. - for i, art in enumerate(arts): - art.set_body_scale(cfg.body_scale) - art.set_physical_attr(cfg.attrs.attr()) - link_names = art.get_link_names() - _apply_link_physics_overrides(art, cfg, link_names) - art.set_articulation_flag(ArticulationFlag.FIX_BASE, cfg.fix_base) - art.set_articulation_flag( - ArticulationFlag.DISABLE_SELF_COLLISION, cfg.disable_self_collision + DexSim configuration is applied once on the prototype before cloning. + + Deprecated: use ``SimulationManager.add_articulation()`` or + ``SimulationManager.add_robot()``. + """ + _warn_legacy_articulation_api("spawn_articulation_entities") + if cfg.uid is None: + logger.log_error("Articulation uid must be set before spawning entities.") + + if clone_options is None: + clone_options = _default_articulation_clone_options() + + source_env = env_list[0] + prototype_name = f"{cfg.uid}_0" + prototype = source_env.load_urdf(cfg.fpath) + prototype.set_name(prototype_name) + + if cfg.resolve_asset_physics_mode() == "overlay": + _set_dexsim_articulation_cfg(prototype, cfg) + + entities = [prototype] + for env_idx in range(1, len(env_list)): + target_name = f"{cfg.uid}_{env_idx}" + clone = _clone_articulation_between_arenas( + source_env, + prototype_name, + env_list[env_idx], + target_name, + clone_options, ) - art.set_solver_iteration_counts( - min_position_iters=cfg.min_position_iters, - min_velocity_iters=cfg.min_velocity_iters, + if clone is None: + logger.log_error( + f"Failed to clone articulation '{prototype_name}' into env {env_idx}." + ) + entities.append(clone) + return entities + + +def _find_single_articulation_in_usd_import(results: dict, fpath: str) -> Articulation: + """Return the sole articulation imported from a USD file.""" + articulations_found = [ + value for value in results.values() if isinstance(value, Articulation) + ] + if len(articulations_found) == 0: + logger.log_error(f"No articulation found in USD file {fpath}.") + if len(articulations_found) > 1: + logger.log_error(f"Multiple articulations found in USD file {fpath}.") + return articulations_found[0] + + +def spawn_usd_articulation_entities( + cfg: ArticulationCfg, + env_list: list[Arena | Env], + *, + cache_dir: str | None = None, + clone_options: ObjectCloneOptions | None = None, +) -> list[Articulation]: + """Import one USD articulation prototype and clone it into additional arenas. + + Deprecated: use ``SimulationManager.add_articulation()`` or + ``SimulationManager.add_robot()``. + """ + _warn_legacy_articulation_api("spawn_usd_articulation_entities") + if cfg.uid is None: + logger.log_error("Articulation uid must be set before spawning entities.") + if len(env_list) == 0: + return [] + + if clone_options is None: + clone_options = _default_articulation_clone_options() + + source_env = env_list[0] + prototype_name = f"{cfg.uid}_0" + results = source_env.import_from_usd_file( + cfg.fpath, return_object=True, cache_dir=cache_dir + ) + prototype = _find_single_articulation_in_usd_import(results, cfg.fpath) + prototype.set_name(prototype_name) + + if cfg.resolve_asset_physics_mode() == "overlay": + _set_dexsim_articulation_cfg(prototype, cfg) + + entities = [prototype] + for env_idx in range(1, len(env_list)): + target_name = f"{cfg.uid}_{env_idx}" + clone = _clone_articulation_between_arenas( + source_env, + prototype_name, + env_list[env_idx], + target_name, + clone_options, ) + if clone is None: + logger.log_error( + f"Failed to clone articulation '{prototype_name}' into env {env_idx}." + ) + entities.append(clone) + return entities - # TODO: We should change this part after improving spawning of articulation. - for name in link_names: - physical_body = art.get_physical_body(name) - inertia = physical_body.get_mass_space_inertia_tensor() - inertia = np.maximum(inertia, 1e-4) - physical_body.set_mass_space_inertia_tensor(inertia) - if i == 0 and cfg.compute_uv: - render_body = art.get_render_body(name) - if render_body: - render_body.set_projective_uv() +def set_dexsim_articulation_cfg( + art: Articulation | SpawnedArticulation, + cfg: ArticulationCfg, +) -> None: + """Apply cfg through the deprecated raw DexSim articulation path. - # TODO: will crash when exit if not explicitly delete. - # This may due to the destruction of render body order when exiting. - del render_body + Args: + art: DexSim articulation (or Newton skeleton carrier) to configure. + cfg: EmbodiChain articulation configuration. + """ + _warn_legacy_articulation_api("set_dexsim_articulation_cfg") + _set_dexsim_articulation_cfg(art, cfg) + + +def _set_dexsim_articulation_cfg( + art: Articulation | SpawnedArticulation, + cfg: ArticulationCfg, +) -> None: + """Implement the retained legacy path for compatibility wrappers.""" + + is_newton_art = hasattr(art, "dexsim_meta_links") + if is_newton_art: + raise TypeError( + "The deprecated raw articulation configuration path is " + "Default-backend-only. Declare the asset through SimulationManager " + "and use grouped RigidBodyPhysicsCfg properties for Newton." + ) + lifecycle_state = getattr(getattr(art, "_mgr", None), "_lifecycle_state", None) + lifecycle_name = getattr(lifecycle_state, "name", "") + if lifecycle_name == "BUILDER" or not is_newton_art: + art.set_body_scale(cfg.body_scale) + + link_names = art.get_link_names() + physical_attr = cfg.attrs.to_dexsim_physical_attr() + art.set_physical_attr(physical_attr) + _apply_link_physics_overrides(art, cfg, link_names) + root_props = cfg.root_props + fixed_base = True if root_props.fixed_base is None else bool(root_props.fixed_base) + self_collision_enabled = ( + False + if root_props.self_collision_enabled is None + else bool(root_props.self_collision_enabled) + ) + art.set_articulation_flag(ArticulationFlag.FIX_BASE, fixed_base) + art.set_articulation_flag( + ArticulationFlag.DISABLE_SELF_COLLISION, not self_collision_enabled + ) + _apply_default_articulation_root_properties(art, root_props) + + for name in link_names: + if not hasattr(art, "get_physical_body"): + continue + physical_body = art.get_physical_body(name) + inertia = physical_body.get_mass_space_inertia_tensor() + inertia = np.maximum(inertia, 1e-4) + physical_body.set_mass_space_inertia_tensor(inertia) + + if cfg.compute_uv: + render_body = art.get_render_body(name) + if render_body: + render_body.set_projective_uv() + + # TODO: will crash when exit if not explicitly delete. + # This may due to the destruction of render body order when exiting. + del render_body + + +def _apply_default_articulation_root_properties( + art: Articulation, + props: ArticulationRootPropertiesCfg, +) -> None: + """Apply explicitly configured Default-native articulation-root values.""" + if props.sleep_threshold is not None: + art.set_sleep_threshold(float(props.sleep_threshold)) + + position_iters = props.min_position_iters + velocity_iters = props.min_velocity_iters + if (position_iters is None) != (velocity_iters is None): + raise ValueError( + "Articulation-root min_position_iters and min_velocity_iters " + "must be configured together." + ) + if position_iters is not None: + assert velocity_iters is not None + art.set_solver_iteration_counts( + min_position_iters=int(position_iters), + min_velocity_iters=int(velocity_iters), + ) def is_rt_enabled() -> bool: @@ -284,181 +486,271 @@ def create_sphere( return spheres -def load_mesh_objects_from_cfg( - cfg: RigidObjectCfg, env_list: List[Arena], cache_dir: str | None = None -) -> List[MeshObject]: - """Load mesh objects from configuration. - - Args: - cfg (RigidObjectCfg): Configuration for the rigid object. - env_list (List[Arena]): List of arenas to load the objects into. - - cache_dir (str | None, optional): Directory for caching convex decomposition files. Defaults to None - Returns: - List[MeshObject]: List of loaded mesh objects. - """ - obj_list = [] - body_type = cfg.to_dexsim_body_type() - if isinstance(cfg.shape, MeshCfg): +def _mesh_load_option_from_cfg(cfg: RigidObjectCfg) -> LoadOption: + """Build DexSim mesh load options from a rigid-object configuration.""" + option = LoadOption() + option.rebuild_normals = cfg.shape.load_option.rebuild_normals + option.rebuild_tangent = cfg.shape.load_option.rebuild_tangent + option.rebuild_3rdnormal = cfg.shape.load_option.rebuild_3rdnormal + option.rebuild_3rdtangent = cfg.shape.load_option.rebuild_3rdtangent + option.smooth = cfg.shape.load_option.smooth + return option - option = LoadOption() - option.rebuild_normals = cfg.shape.load_option.rebuild_normals - option.rebuild_tangent = cfg.shape.load_option.rebuild_tangent - option.rebuild_3rdnormal = cfg.shape.load_option.rebuild_3rdnormal - option.rebuild_3rdtangent = cfg.shape.load_option.rebuild_3rdtangent - option.smooth = cfg.shape.load_option.smooth - cfg: RigidObjectCfg - max_convex_hull_num, acd_method, sdf_resolution = ( - _resolve_mesh_collision_params(cfg) - ) - fpath = cfg.shape.fpath +def _apply_mesh_uv_mapping(obj: MeshObject, cfg: RigidObjectCfg) -> None: + """Compute and apply UV mapping for a mesh rigid-object prototype.""" + if not cfg.shape.compute_uv: + return - compute_uv = cfg.shape.compute_uv + vertices = obj.get_vertices() + triangles = obj.get_triangles() + o3d_mesh = o3d.t.geometry.TriangleMesh(vertices, triangles) + _, uvs = get_mesh_auto_uv(o3d_mesh, np.array(cfg.shape.project_direction)) + obj.set_uv_mapping(uvs) - is_usd = fpath.endswith((".usd", ".usda", ".usdc")) - if is_usd: - # TODO: Currently add checking for num_envs when file is USD. After we support spawn via cloning, we can remove this. - if len(env_list) > 1: - logger.log_error(f"Currently not supporting multiple arenas for USD.") - _env: dexsim.environment.Env = dexsim.default_world().get_env() - results = _env.import_from_usd_file(fpath, return_object=True) - # print(f"import usd result: {results}") - - rigidbodys_found = [] - for key, value in results.items(): - if isinstance(value, MeshObject): - rigidbodys_found.append(value) - if len(rigidbodys_found) == 0: - logger.log_error(f"No rigid body found in USD file: {fpath}") - elif len(rigidbodys_found) > 1: - logger.log_error(f"Multiple rigid bodies found in USD file: {fpath}.") - elif len(rigidbodys_found) == 1: - obj_list.append(rigidbodys_found[0]) - return obj_list - else: - # non-usd file does not support this option, will be forced set False to avoid potential issues. - cfg.use_usd_properties = False - - for i, env in enumerate(env_list): - if max_convex_hull_num > 1: - obj = env.load_actor_with_acd( - fpath, - duplicate=True, - attach_scene=True, - option=option, - cache_path=cache_dir, - actor_type=body_type, - max_convex_hull_num=max_convex_hull_num, - method=acd_method, - ) - elif sdf_resolution > 0: - obj = env.load_actor( - fpath, duplicate=True, attach_scene=True, option=option - ) - sdf_cfg = SDFConfig() - sdf_cfg.resolution = sdf_resolution - obj.add_physical_body( - body_type, - RigidBodyShape.SDF, - config=sdf_cfg, - attr=PhysicalAttr(), - ) - else: - obj = env.load_actor( - fpath, duplicate=True, attach_scene=True, option=option - ) - obj.add_rigidbody(body_type, RigidBodyShape.CONVEX) - obj.set_name(f"{cfg.uid}_{i}") - obj_list.append(obj) - if compute_uv: - vertices = obj.get_vertices() - triangles = obj.get_triangles() +def _configure_primitive_rigidbody( + obj: MeshObject, + cfg: RigidObjectCfg, + body_type, + *, + is_newton_backend: bool, + shape_type: RigidBodyShape, +) -> None: + """Attach primitive rigid-body physics to a cube or sphere prototype.""" + if is_newton_backend: + raise TypeError( + "The deprecated raw rigid-object initialization path is " + "Default-backend-only. Use SimulationManager with grouped " + "RigidBodyPhysicsCfg properties for Newton." + ) + obj.set_body_scale(*cfg.body_scale) + obj.add_rigidbody( + body_type, + shape_type, + cfg.attrs.to_dexsim_physical_attr(), + ) - o3d_mesh = o3d.t.geometry.TriangleMesh(vertices, triangles) - _, uvs = get_mesh_auto_uv( - o3d_mesh, np.array(cfg.shape.project_direction) - ) - obj.set_uv_mapping(uvs) - elif isinstance(cfg.shape, CubeCfg): - from embodichain.lab.sim.utility.sim_utils import create_cube +def _import_usd_rigid_prototype( + env: Arena | Env, + fpath: str, + prototype_name: str, +) -> MeshObject: + """Import a single rigid mesh actor from USD as the spawn prototype.""" + results = env.import_from_usd_file(fpath, return_object=True) + rigidbodys_found = [ + value for value in results.values() if isinstance(value, MeshObject) + ] + if len(rigidbodys_found) == 0: + logger.log_error(f"No rigid body found in USD file: {fpath}") + if len(rigidbodys_found) > 1: + logger.log_error(f"Multiple rigid bodies found in USD file: {fpath}.") + prototype = rigidbodys_found[0] + prototype.set_name(prototype_name) + return prototype + + +def _load_rigid_mesh_prototype( + env: Arena | Env, + cfg: RigidObjectCfg, + *, + cache_dir: str | None, + body_type, + is_newton_backend: bool, +) -> MeshObject: + """Load and configure one mesh rigid-object prototype in the source arena.""" + if is_newton_backend: + raise TypeError( + "The deprecated raw rigid-object initialization path is " + "Default-backend-only. Use SimulationManager with grouped " + "RigidBodyPhysicsCfg properties for Newton." + ) + option = _mesh_load_option_from_cfg(cfg) + fpath = cfg.shape.fpath + collision_cfg = _resolve_mesh_collision_params(cfg) + + if collision_cfg.approximation == "convex_decomposition": + obj = env.load_actor_with_acd( + fpath, + duplicate=True, + attach_scene=True, + option=option, + cache_path=cache_dir, + actor_type=body_type, + max_convex_hull_num=collision_cfg.max_hulls, + method=collision_cfg.acd_method or "coacd", + ) + elif collision_cfg.approximation == "sdf": + if collision_cfg.sdf_resolution is None: + raise ValueError( + "The deprecated raw Default path requires sdf_resolution for " + "MeshCollisionCfg(approximation='sdf')." + ) + if cfg.body_scale not in [ + (1.0, 1.0, 1.0), + [1.0, 1.0, 1.0], + ]: + logger.log_error( + f"Non-unit body scale {cfg.body_scale} is not supported for SDF " + "collision yet. Please set body_scale to (1.0, 1.0, 1.0) for SDF " + "collision." + ) + obj = env.load_actor(fpath, duplicate=True, attach_scene=True, option=option) + sdf_cfg = SDFConfig(resolution=collision_cfg.sdf_resolution) + obj.add_physical_body( + body_type, + RigidBodyShape.SDF, + config=sdf_cfg, + attr=cfg.attrs.to_dexsim_physical_attr(), + ) + else: + if collision_cfg.approximation == "triangle_mesh" and cfg.body_type != "static": + raise ValueError( + "triangle_mesh collision is supported only for static rigid objects." + ) + obj = env.load_actor(fpath, duplicate=True, attach_scene=True, option=option) + shape_type = ( + RigidBodyShape.MESH + if collision_cfg.approximation == "triangle_mesh" + else RigidBodyShape.CONVEX + ) + obj.add_rigidbody( + body_type, + shape_type, + cfg.attrs.to_dexsim_physical_attr(), + ) - obj_list = create_cube(env_list, cfg.shape.size, uid=cfg.uid) - for obj in obj_list: - obj.add_rigidbody(body_type, RigidBodyShape.BOX) + _apply_mesh_uv_mapping(obj, cfg) + return obj - elif isinstance(cfg.shape, SphereCfg): - from embodichain.lab.sim.utility.sim_utils import create_sphere - obj_list = create_sphere( - env_list, cfg.shape.radius, cfg.shape.resolution, uid=cfg.uid - ) - for obj in obj_list: - obj.add_rigidbody(body_type, RigidBodyShape.SPHERE) - else: +def _spawn_clones_from_prototype( + source_env: Arena | Env, + prototype_name: str, + env_list: list[Arena | Env], + uid: str, + clone_options: ObjectCloneOptions, +) -> list[MeshObject]: + """Return the prototype plus clones for all remaining arenas.""" + prototype = source_env.get_actor(prototype_name) + if prototype is None: logger.log_error( - f"Unsupported rigid object shape type: {type(cfg.shape)}. Supported types: MeshCfg, CubeCfg, SphereCfg." + f"Rigid object prototype '{prototype_name}' was not found in the source arena." ) - return obj_list + entities = [prototype] + for env_idx in range(1, len(env_list)): + target_name = f"{uid}_{env_idx}" + clone = _clone_actor_between_arenas( + source_env, + prototype_name, + env_list[env_idx], + target_name, + clone_options, + ) + if clone is None: + logger.log_error( + f"Failed to clone rigid object '{prototype_name}' into env {env_idx}." + ) + entities.append(clone) + return entities -def load_soft_object_from_cfg( - cfg: SoftObjectCfg, env_list: List[Arena] -) -> List[MeshObject]: - obj_list = [] - option = LoadOption() - option.rebuild_normals = cfg.shape.load_option.rebuild_normals - option.rebuild_tangent = cfg.shape.load_option.rebuild_tangent - option.rebuild_3rdnormal = cfg.shape.load_option.rebuild_3rdnormal - option.rebuild_3rdtangent = cfg.shape.load_option.rebuild_3rdtangent - option.smooth = cfg.shape.load_option.smooth - option.share_mesh = False +def spawn_rigid_object_entities( + cfg: RigidObjectCfg, + env_list: list[Arena | Env], + *, + cache_dir: str | None = None, + clone_options: ObjectCloneOptions | None = None, +) -> list[MeshObject]: + """Load one rigid-object prototype and clone it into additional arenas. + + Mesh loading, convex decomposition, and physics setup run once on the + prototype in ``env_list[0]`` before cloning. + """ + if cfg.uid is None: + logger.log_error("Rigid object uid must be set before spawning entities.") + if len(env_list) == 0: + return [] + + if clone_options is None: + clone_options = default_rigid_object_clone_options() - for i, env in enumerate(env_list): - obj = env.load_actor( - fpath=cfg.shape.fpath, duplicate=True, attach_scene=True, option=option + body_type = cfg.to_dexsim_body_type() + is_newton_backend = _is_newton_backend_active() + if is_newton_backend: + raise TypeError( + "spawn_rigid_object_entities() is a deprecated " + "Default-backend-only initialization path. Use " + "SimulationManager.add_rigid_object() with grouped " + "RigidBodyPhysicsCfg properties for Newton." ) - obj.add_softbody(cfg.voxel_attr.attr(), cfg.physical_attr.attr()) - if cfg.shape.compute_uv: - vertices = obj.get_vertices() - triangles = obj.get_triangles() + source_env = env_list[0] + prototype_name = f"{cfg.uid}_0" - o3d_mesh = o3d.t.geometry.TriangleMesh(vertices, triangles) - _, uvs = get_mesh_auto_uv(o3d_mesh, cfg.shape.project_direction) - obj.set_uv_mapping(uvs) - obj.set_name(f"{cfg.uid}_{i}") - obj_list.append(obj) - return obj_list + if isinstance(cfg.shape, MeshCfg): + fpath = cfg.shape.fpath + is_usd = fpath.endswith((".usd", ".usda", ".usdc")) + if is_usd: + prototype = _import_usd_rigid_prototype(source_env, fpath, prototype_name) + else: + cfg.asset_physics_mode = "overlay" + prototype = _load_rigid_mesh_prototype( + source_env, + cfg, + cache_dir=cache_dir, + body_type=body_type, + is_newton_backend=is_newton_backend, + ) + prototype.set_name(prototype_name) + elif isinstance(cfg.shape, CubeCfg): + prototype = source_env.create_cube( + cfg.shape.size[0], cfg.shape.size[1], cfg.shape.size[2] + ) + prototype.set_name(prototype_name) + _configure_primitive_rigidbody( + prototype, + cfg, + body_type, + is_newton_backend=is_newton_backend, + shape_type=RigidBodyShape.BOX, + ) + elif isinstance(cfg.shape, SphereCfg): + prototype = source_env.create_sphere(cfg.shape.radius, cfg.shape.resolution) + prototype.set_name(prototype_name) + _configure_primitive_rigidbody( + prototype, + cfg, + body_type, + is_newton_backend=is_newton_backend, + shape_type=RigidBodyShape.SPHERE, + ) + else: + logger.log_error( + f"Unsupported rigid object shape type: {type(cfg.shape)}. " + "Supported types: MeshCfg, CubeCfg, SphereCfg." + ) + return [] + if len(env_list) == 1: + return [prototype] + return _spawn_clones_from_prototype( + source_env, prototype_name, env_list, cfg.uid, clone_options + ) -def load_cloth_object_from_cfg( - cfg: ClothObjectCfg, env_list: List[Arena] + +def load_mesh_objects_from_cfg( + cfg: RigidObjectCfg, env_list: List[Arena], cache_dir: str | None = None ) -> List[MeshObject]: - obj_list = [] + """Load mesh objects from configuration. - option = LoadOption() - option.rebuild_normals = cfg.shape.load_option.rebuild_normals - option.rebuild_tangent = cfg.shape.load_option.rebuild_tangent - option.rebuild_3rdnormal = cfg.shape.load_option.rebuild_3rdnormal - option.rebuild_3rdtangent = cfg.shape.load_option.rebuild_3rdtangent - option.smooth = cfg.shape.load_option.smooth - option.share_mesh = False + Args: + cfg (RigidObjectCfg): Configuration for the rigid object. + env_list (List[Arena]): List of arenas to load the objects into. - for i, env in enumerate(env_list): - obj = env.load_actor( - fpath=cfg.shape.fpath, duplicate=True, attach_scene=True, option=option - ) - obj.add_clothbody(cfg.physical_attr.attr()) - if cfg.shape.compute_uv: - vertices = obj.get_vertices() - triangles = obj.get_triangles() - - o3d_mesh = o3d.t.geometry.TriangleMesh(vertices, triangles) - _, uvs = get_mesh_auto_uv(o3d_mesh, cfg.shape.project_direction) - obj.set_uv_mapping(uvs) - obj.set_name(f"{cfg.uid}_{i}") - obj_list.append(obj) - return obj_list + cache_dir (str | None, optional): Directory for caching convex decomposition files. Defaults to None + Returns: + List[MeshObject]: List of loaded mesh objects. + """ + return spawn_rigid_object_entities(cfg, env_list, cache_dir=cache_dir) diff --git a/embodichain/lab/task_program/compiler/program.py b/embodichain/lab/task_program/compiler/program.py index 57607bf02..9ef08ee80 100644 --- a/embodichain/lab/task_program/compiler/program.py +++ b/embodichain/lab/task_program/compiler/program.py @@ -1681,7 +1681,7 @@ def _compile_targets( (*path, "values", index), "Target values must be exact PoseCfg values.", ) - poses.append(SemanticPose(pose.position, pose.quaternion_wxyz)) + poses.append(SemanticPose(pose.position, pose.quaternion_xyzw)) compiled[target_id] = tuple(poses) return MappingProxyType(compiled) diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index 2062c2a35..e0859633d 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -1685,7 +1685,7 @@ def _decode_handover_pose_provider( { "kind", "final_position", - "final_quaternion_wxyz", + "final_quaternion_xyzw", } ), ) @@ -1701,9 +1701,9 @@ def _decode_handover_pose_provider( path=f"{path}.final_position", expected_length=3, ), - final_quaternion_wxyz=_finite_tuple( - config["final_quaternion_wxyz"], - path=f"{path}.final_quaternion_wxyz", + final_quaternion_xyzw=_finite_tuple( + config["final_quaternion_xyzw"], + path=f"{path}.final_quaternion_xyzw", expected_length=4, ), ) diff --git a/embodichain/lab/task_program/integrations/simulation/handover.py b/embodichain/lab/task_program/integrations/simulation/handover.py index 1e6d8d902..ba5f0c6fa 100644 --- a/embodichain/lab/task_program/integrations/simulation/handover.py +++ b/embodichain/lab/task_program/integrations/simulation/handover.py @@ -37,19 +37,19 @@ def _validated_pose( position: tuple[float, float, float], - quaternion_wxyz: tuple[float, float, float, float], + quaternion_xyzw: tuple[float, float, float, float], *, field_name: str, ) -> SemanticPose: """Build and validate one unbatched semantic pose declaration.""" if type(position) is not tuple or len(position) != 3: raise TypeError(f"{field_name}_position must be an exact 3-tuple.") - if type(quaternion_wxyz) is not tuple or len(quaternion_wxyz) != 4: - raise TypeError(f"{field_name}_quaternion_wxyz must be an exact 4-tuple.") + if type(quaternion_xyzw) is not tuple or len(quaternion_xyzw) != 4: + raise TypeError(f"{field_name}_quaternion_xyzw must be an exact 4-tuple.") try: return SemanticPose( position=position, - quaternion_wxyz=quaternion_wxyz, + quaternion_xyzw=quaternion_xyzw, ) except (TypeError, ValueError) as exc: raise type(exc)(f"Invalid {field_name} hand-over pose: {exc}") from exc @@ -67,18 +67,18 @@ class ConfiguredHandOverPoseProvider(HandOverPoseProvider): Args: final_position: World-frame object delivery position. - final_quaternion_wxyz: World-frame object delivery orientation. + final_quaternion_xyzw: World-frame object delivery orientation. """ provider_id: ClassVar[str] = "simulation.configured_handover_pose" final_position: tuple[float, float, float] - final_quaternion_wxyz: tuple[float, float, float, float] + final_quaternion_xyzw: tuple[float, float, float, float] def __post_init__(self) -> None: final = _validated_pose( self.final_position, - self.final_quaternion_wxyz, + self.final_quaternion_xyzw, field_name="final", ) object.__setattr__( @@ -88,8 +88,8 @@ def __post_init__(self) -> None: ) object.__setattr__( self, - "final_quaternion_wxyz", - tuple(float(value) for value in final.quaternion_wxyz.tolist()), + "final_quaternion_xyzw", + tuple(float(value) for value in final.quaternion_xyzw.tolist()), ) def resolve( @@ -114,7 +114,7 @@ def resolve( final=SemanticObjectTarget( pose=SemanticPose( position=self.final_position, - quaternion_wxyz=self.final_quaternion_wxyz, + quaternion_xyzw=self.final_quaternion_xyzw, ) ), ) diff --git a/embodichain/lab/task_program/language/decoder.py b/embodichain/lab/task_program/language/decoder.py index 8e156db02..877e4f197 100644 --- a/embodichain/lab/task_program/language/decoder.py +++ b/embodichain/lab/task_program/language/decoder.py @@ -392,14 +392,14 @@ def _decode_pose(value: object, *, path: ConfigPath) -> PoseCfg: mapping = _expect_mapping(value, path=path) _validate_fields( mapping, - allowed=frozenset({"position", "quaternion_wxyz"}), - required=frozenset({"position", "quaternion_wxyz"}), + allowed=frozenset({"position", "quaternion_xyzw"}), + required=frozenset({"position", "quaternion_xyzw"}), path=path, ) position_values = _expect_list(mapping["position"], path=(*path, "position")) quaternion_values = _expect_list( - mapping["quaternion_wxyz"], - path=(*path, "quaternion_wxyz"), + mapping["quaternion_xyzw"], + path=(*path, "quaternion_xyzw"), ) if len(position_values) != 3: raise _error( @@ -410,12 +410,12 @@ def _decode_pose(value: object, *, path: ConfigPath) -> PoseCfg: if len(quaternion_values) != 4: raise _error( "invalid_pose_shape", - (*path, "quaternion_wxyz"), - "quaternion_wxyz must contain exactly four numbers.", + (*path, "quaternion_xyzw"), + "quaternion_xyzw must contain exactly four numbers.", ) for name, values in ( ("position", position_values), - ("quaternion_wxyz", quaternion_values), + ("quaternion_xyzw", quaternion_values), ): for index, number in enumerate(values): if type(number) not in (int, float): @@ -428,7 +428,7 @@ def _decode_pose(value: object, *, path: ConfigPath) -> PoseCfg: PoseCfg, path=path, position=tuple(position_values), - quaternion_wxyz=tuple(quaternion_values), + quaternion_xyzw=tuple(quaternion_values), ) # type: ignore[return-value] diff --git a/embodichain/lab/task_program/language/schema.py b/embodichain/lab/task_program/language/schema.py index e8055012a..106d2b550 100644 --- a/embodichain/lab/task_program/language/schema.py +++ b/embodichain/lab/task_program/language/schema.py @@ -231,33 +231,33 @@ def __post_init__(self) -> None: @configclass class PoseCfg: - """One declarative Cartesian pose using a WXYZ quaternion.""" + """One declarative Cartesian pose using an XYZW quaternion.""" position: tuple[float, float, float] = MISSING - quaternion_wxyz: tuple[float, float, float, float] = MISSING + quaternion_xyzw: tuple[float, float, float, float] = MISSING def __post_init__(self) -> None: """Validate pose shape, finiteness, and quaternion magnitude.""" if type(self.position) not in (list, tuple) or len(self.position) != 3: raise ValueError("position must contain exactly three numbers.") if ( - type(self.quaternion_wxyz) not in (list, tuple) - or len(self.quaternion_wxyz) != 4 + type(self.quaternion_xyzw) not in (list, tuple) + or len(self.quaternion_xyzw) != 4 ): - raise ValueError("quaternion_wxyz must contain exactly four numbers.") + raise ValueError("quaternion_xyzw must contain exactly four numbers.") position = tuple( _validate_number(value, field_name=f"position[{index}]") for index, value in enumerate(self.position) ) quaternion = tuple( - _validate_number(value, field_name=f"quaternion_wxyz[{index}]") - for index, value in enumerate(self.quaternion_wxyz) + _validate_number(value, field_name=f"quaternion_xyzw[{index}]") + for index, value in enumerate(self.quaternion_xyzw) ) norm = math.sqrt(sum(value * value for value in quaternion)) if norm <= 1.0e-12: - raise ValueError("quaternion_wxyz must have non-zero magnitude.") + raise ValueError("quaternion_xyzw must have non-zero magnitude.") self.position = position # type: ignore[assignment] - self.quaternion_wxyz = quaternion # type: ignore[assignment] + self.quaternion_xyzw = quaternion # type: ignore[assignment] @configclass diff --git a/embodichain/lab/task_program/semantics/calls.py b/embodichain/lab/task_program/semantics/calls.py index 3935f74bc..2b8e803da 100644 --- a/embodichain/lab/task_program/semantics/calls.py +++ b/embodichain/lab/task_program/semantics/calls.py @@ -184,52 +184,52 @@ def _validate_static_skill_descriptor( @dataclass(frozen=True, slots=True, init=False, eq=False) class SemanticPose: - """Object-space pose expressed as position and a WXYZ quaternion. + """Object-space pose expressed as position and an XYZW quaternion. The value owns normalized tensor snapshots and never exposes its internal tensors directly. A single pose or an environment batch is accepted. Args: position: Shape ``(3,)`` or ``(B, 3)``. - quaternion_wxyz: Shape ``(4,)`` or ``(B, 4)``. Finite, non-zero + quaternion_xyzw: Shape ``(4,)`` or ``(B, 4)``. Finite, non-zero quaternions are normalized at construction. """ _position: torch.Tensor = field(repr=False) - _quaternion_wxyz: torch.Tensor = field(repr=False) + _quaternion_xyzw: torch.Tensor = field(repr=False) def __init__( self, position: torch.Tensor | tuple[float, float, float] | list[float], - quaternion_wxyz: torch.Tensor | tuple[float, float, float, float] | list[float], + quaternion_xyzw: torch.Tensor | tuple[float, float, float, float] | list[float], ) -> None: position_tensor = torch.as_tensor(position, dtype=torch.float32) - quaternion_tensor = torch.as_tensor(quaternion_wxyz, dtype=torch.float32) + quaternion_tensor = torch.as_tensor(quaternion_xyzw, dtype=torch.float32) if position_tensor.dim() not in (1, 2) or position_tensor.shape[-1] != 3: raise ValueError("position must have shape (3,) or (B, 3).") if quaternion_tensor.dim() not in (1, 2) or quaternion_tensor.shape[-1] != 4: - raise ValueError("quaternion_wxyz must have shape (4,) or (B, 4).") + raise ValueError("quaternion_xyzw must have shape (4,) or (B, 4).") if position_tensor.dim() != quaternion_tensor.dim(): raise ValueError( - "position and quaternion_wxyz must both be unbatched or batched." + "position and quaternion_xyzw must both be unbatched or batched." ) if position_tensor.dim() == 2 and ( position_tensor.shape[0] != quaternion_tensor.shape[0] ): - raise ValueError("position and quaternion_wxyz batch sizes must match.") + raise ValueError("position and quaternion_xyzw batch sizes must match.") if position_tensor.dim() == 2 and position_tensor.shape[0] == 0: raise ValueError("SemanticPose batches must contain at least one pose.") if not torch.isfinite(position_tensor).all(): raise ValueError("position must contain only finite values.") if not torch.isfinite(quaternion_tensor).all(): - raise ValueError("quaternion_wxyz must contain only finite values.") + raise ValueError("quaternion_xyzw must contain only finite values.") norms = torch.linalg.vector_norm(quaternion_tensor, dim=-1, keepdim=True) if torch.any(norms <= torch.finfo(torch.float32).eps): - raise ValueError("quaternion_wxyz must be non-zero.") + raise ValueError("quaternion_xyzw must be non-zero.") object.__setattr__(self, "_position", position_tensor.clone()) object.__setattr__( self, - "_quaternion_wxyz", + "_quaternion_xyzw", (quaternion_tensor / norms).clone(), ) @@ -239,9 +239,9 @@ def position(self) -> torch.Tensor: return self._position.clone() @property - def quaternion_wxyz(self) -> torch.Tensor: + def quaternion_xyzw(self) -> torch.Tensor: """Return an independent normalized quaternion tensor.""" - return self._quaternion_wxyz.clone() + return self._quaternion_xyzw.clone() @property def batch_size(self) -> int | None: @@ -250,7 +250,7 @@ def batch_size(self) -> int | None: def snapshot(self) -> SemanticPose: """Return an independently owned pose value.""" - return SemanticPose(self._position, self._quaternion_wxyz) + return SemanticPose(self._position, self._quaternion_xyzw) def to_matrix(self) -> torch.Tensor: """Convert the semantic pose to a homogeneous transform. @@ -259,14 +259,14 @@ def to_matrix(self) -> torch.Tensor: Shape ``(4, 4)`` for an unbatched pose or ``(B, 4, 4)`` for a batched pose. """ - quaternion = self._quaternion_wxyz + quaternion = self._quaternion_xyzw was_unbatched = quaternion.dim() == 1 if was_unbatched: quaternion = quaternion.unsqueeze(0) position = self._position.unsqueeze(0) else: position = self._position - w, x, y, z = quaternion.unbind(dim=-1) + x, y, z, w = quaternion.unbind(dim=-1) output = torch.zeros( quaternion.shape[0], 4, @@ -291,7 +291,7 @@ def to_metadata(self) -> dict[str, object]: """Return the pose as deterministic JSON-safe semantic data.""" return { "position": self._position.detach().cpu().tolist(), - "quaternion_wxyz": self._quaternion_wxyz.detach().cpu().tolist(), + "quaternion_xyzw": self._quaternion_xyzw.detach().cpu().tolist(), } diff --git a/embodichain/lab/visualization/protocol.py b/embodichain/lab/visualization/protocol.py index 5da1eed1b..7eaf6d7de 100644 --- a/embodichain/lab/visualization/protocol.py +++ b/embodichain/lab/visualization/protocol.py @@ -130,8 +130,9 @@ def pose_to_position_wxyz(pose: object) -> tuple[np.ndarray, np.ndarray]: """Split pose arrays into positions and normalized wxyz quaternions. The accepted layouts are ``(..., 7)`` in EmbodiChain's - ``(x, y, z, qw, qx, qy, qz)`` convention or homogeneous ``(..., 4, 4)`` - matrices. This is the single conversion boundary used by scene exporters. + ``(x, y, z, qx, qy, qz, qw)`` convention or homogeneous ``(..., 4, 4)`` + matrices. Viser uses ``wxyz``, so this is the single conversion boundary + used by scene exporters. Args: pose: Pose or batch of poses. @@ -145,11 +146,12 @@ def pose_to_position_wxyz(pose: object) -> tuple[np.ndarray, np.ndarray]: array = _array(pose, np.float32) if array.ndim >= 1 and array.shape[-1] == 7: position = array[..., :3].copy() - wxyz = array[..., 3:7].copy() - norms = np.linalg.norm(wxyz, axis=-1, keepdims=True) + xyzw = array[..., 3:7].copy() + norms = np.linalg.norm(xyzw, axis=-1, keepdims=True) if np.any(norms <= np.finfo(np.float32).eps): raise ValueError("Pose contains a degenerate quaternion.") - return position, wxyz / norms + xyzw = xyzw / norms + return position, np.roll(xyzw, 1, axis=-1) if array.ndim >= 2 and array.shape[-2:] == (4, 4): position = array[..., :3, 3].copy() @@ -164,6 +166,22 @@ def pose_to_position_wxyz(pose: object) -> tuple[np.ndarray, np.ndarray]: ) +def _normalize_position_wxyz( + position: object, quaternion: object +) -> tuple[np.ndarray, np.ndarray]: + """Validate one protocol-native position and Viser ``wxyz`` quaternion.""" + position_array = _array(position, np.float32).copy() + quaternion_array = _array(quaternion, np.float32).copy() + if position_array.shape != (3,): + raise ValueError(f"position must have shape (3,), got {position_array.shape}.") + if quaternion_array.shape != (4,): + raise ValueError(f"wxyz must have shape (4,), got {quaternion_array.shape}.") + norm = np.linalg.norm(quaternion_array) + if norm <= np.finfo(np.float32).eps: + raise ValueError("Pose contains a degenerate quaternion.") + return position_array, quaternion_array / norm + + @dataclass(frozen=True) class MeshGeometry: """Backend-neutral triangle mesh stored in local coordinates.""" @@ -311,11 +329,7 @@ class GizmoState: visible: bool = True def __post_init__(self) -> None: - position, wxyz = pose_to_position_wxyz( - np.concatenate( - (_array(self.position, np.float32), _array(self.wxyz, np.float32)) - ) - ) + position, wxyz = _normalize_position_wxyz(self.position, self.wxyz) object.__setattr__(self, "position", position) object.__setattr__(self, "wxyz", wxyz) @@ -347,11 +361,7 @@ def __post_init__(self) -> None: raise ValueError("Gizmo command phase must be 'start', 'update', or 'end'.") if not self.client_id: raise ValueError("Gizmo command client_id must not be empty.") - position, wxyz = pose_to_position_wxyz( - np.concatenate( - (_array(self.position, np.float32), _array(self.wxyz, np.float32)) - ) - ) + position, wxyz = _normalize_position_wxyz(self.position, self.wxyz) object.__setattr__(self, "position", position) object.__setattr__(self, "wxyz", wxyz) @@ -560,11 +570,7 @@ class FrameOverlay: visible: bool = True def __post_init__(self) -> None: - position, wxyz = pose_to_position_wxyz( - np.concatenate( - (_array(self.position, np.float32), _array(self.wxyz, np.float32)) - ) - ) + position, wxyz = _normalize_position_wxyz(self.position, self.wxyz) object.__setattr__(self, "position", position) object.__setattr__(self, "wxyz", wxyz) @@ -580,11 +586,7 @@ class TargetOverlay: visible: bool = True def __post_init__(self) -> None: - position, wxyz = pose_to_position_wxyz( - np.concatenate( - (_array(self.position, np.float32), _array(self.wxyz, np.float32)) - ) - ) + position, wxyz = _normalize_position_wxyz(self.position, self.wxyz) object.__setattr__(self, "position", position) object.__setattr__(self, "wxyz", wxyz) diff --git a/embodichain/lab/visualization/scene_exporter.py b/embodichain/lab/visualization/scene_exporter.py index f5654b9c8..b7d6061a8 100644 --- a/embodichain/lab/visualization/scene_exporter.py +++ b/embodichain/lab/visualization/scene_exporter.py @@ -367,18 +367,8 @@ def build_manifest(self) -> SceneManifest: self._append_deformable_objects( sources=sources, geometries=geometries, - uids=self._sim.get_soft_object_uid_list(), - getter=self._sim.get_soft_object, - kind="soft_object", - asset_prefix="soft", - ) - self._append_deformable_objects( - sources=sources, - geometries=geometries, - uids=self._sim.get_cloth_object_uid_list(), - getter=self._sim.get_cloth_object, - kind="cloth_object", - asset_prefix="cloth", + uids=self._sim.get_deformable_object_uid_list(), + getter=self._sim.get_deformable_object, ) self._append_cameras(camera_sources) self._append_gizmos(gizmo_sources) @@ -568,31 +558,29 @@ def _append_deformable_objects( geometries: dict[str, MeshGeometry], uids: list[str], getter: object, - kind: str, - asset_prefix: str, ) -> None: for uid in uids: asset = getter(uid) if asset is None: continue - if kind == "soft_object": - current_vertices = _to_numpy( - asset.get_current_collision_vertices(), - np.float32, - ) + if asset.deformable_type == "volume": + kind = "soft_object" + asset_prefix = "soft" + elif asset.deformable_type == "surface": + kind = "cloth_object" + asset_prefix = "cloth" else: - current_vertices = _to_numpy( - asset.get_current_vertex_position(), - np.float32, + raise ValueError( + f"Unsupported deformable_type {asset.deformable_type!r} " + f"for asset {uid!r}." ) + current_vertices = _to_numpy( + asset.get_surface_vertices(), + np.float32, + ) uid_component = safe_path_component(uid) selected_env_ids = list(self._env_ids) - if kind == "soft_object": - faces_by_env = asset.get_collision_surface_triangles( - env_ids=selected_env_ids, - ) - else: - faces_by_env = asset.get_triangles(env_ids=selected_env_ids) + faces_by_env = asset.get_surface_triangles(env_ids=selected_env_ids) for selected_index, env_id in enumerate(self._env_ids): vertices = current_vertices[env_id] - self._env_offsets[env_id] faces = faces_by_env[selected_index] @@ -869,10 +857,7 @@ def capture( if not source.node.dynamic_geometry: continue if source.asset_key not in dynamic_vertex_cache: - if source.asset_key[0] == "soft": - vertices = source.asset.get_current_collision_vertices() - else: - vertices = source.asset.get_current_vertex_position() + vertices = source.asset.get_surface_vertices() dynamic_vertex_cache[source.asset_key] = _to_numpy( vertices, np.float32, diff --git a/embodichain/learning/rl/__init__.py b/embodichain/learning/rl/__init__.py index 9c97766b1..4808e9caa 100644 --- a/embodichain/learning/rl/__init__.py +++ b/embodichain/learning/rl/__init__.py @@ -19,6 +19,8 @@ Algorithms (PPO/GRPO), rollout buffers, collectors, policy/model builders, and the training entry point; rollout data flows as ``TensorDict`` objects. """ +from __future__ import annotations + from . import algo from . import buffer from . import models @@ -29,26 +31,37 @@ ) from .env import ( DifferentiableObservation, + DifferentiableRolloutSpec, DifferentiableVecEnv, LearningVecEnv, build_learning_env, get_registered_learning_env_names, register_learning_env, + ScheduledDifferentiableVecEnv, + stratified_rollout_value, ) from .evaluation import evaluate_episodes +from .gradients import BatchedGradientNormStats, clip_batched_gradient_norm +from .normalization import RunningObservationNormalizer from .routing import get_trainer_class __all__ = [ "DifferentiableObservation", "DifferentiableTrainer", "DifferentiableTrainerCfg", + "DifferentiableRolloutSpec", "DifferentiableVecEnv", + "BatchedGradientNormStats", "LearningVecEnv", + "RunningObservationNormalizer", + "ScheduledDifferentiableVecEnv", "build_learning_env", + "clip_batched_gradient_norm", "evaluate_episodes", "get_registered_learning_env_names", "get_trainer_class", "register_learning_env", + "stratified_rollout_value", "algo", "buffer", "models", diff --git a/embodichain/learning/rl/algo/__init__.py b/embodichain/learning/rl/algo/__init__.py index 7551d2b0c..cededb485 100644 --- a/embodichain/learning/rl/algo/__init__.py +++ b/embodichain/learning/rl/algo/__init__.py @@ -28,7 +28,12 @@ coerce_optimizer_cfg, ) -from .apg import APG, APGCfg, segmented_discounted_return +from .apg import ( + APG, + APGCfg, + complete_discounted_return, + segmented_discounted_return, +) from .base import BaseAlgorithm, RolloutKind from .common import compute_gae from .grpo import GRPO, GRPOCfg @@ -95,6 +100,7 @@ def build_algo( "RolloutKind", "APGCfg", "APG", + "complete_discounted_return", "segmented_discounted_return", "PPOCfg", "PPO", diff --git a/embodichain/learning/rl/algo/apg.py b/embodichain/learning/rl/algo/apg.py index 279320593..9d40b09d0 100644 --- a/embodichain/learning/rl/algo/apg.py +++ b/embodichain/learning/rl/algo/apg.py @@ -23,12 +23,18 @@ import torch from embodichain.learning.rl.collector import DifferentiableRollout +from embodichain.learning.rl.gradients import BatchedGradientNormStats from embodichain.learning.rl.utils import AlgorithmCfg from embodichain.utils import configclass from .base import BaseAlgorithm, RolloutKind -__all__ = ["APG", "APGCfg", "segmented_discounted_return"] +__all__ = [ + "APG", + "APGCfg", + "complete_discounted_return", + "segmented_discounted_return", +] def segmented_discounted_return( @@ -51,6 +57,37 @@ def segmented_discounted_return( return returns +def complete_discounted_return( + rollout: DifferentiableRollout, + gamma: float, +) -> torch.Tensor: + """Compute per-environment returns up to the first terminal transition. + + Unlike segmented TBPTT returns, complete-rollout discounting never restarts + after ``done``. This excludes rewards emitted by any automatic reset during + the fixed full horizon. + + Args: + rollout: Complete graph-preserving trajectory. + gamma: Per-step discount factor. + + Returns: + One masked discounted return per environment. + + Raises: + ValueError: If the rollout is empty. + """ + if rollout.num_steps == 0: + raise ValueError("Cannot compute returns for an empty rollout.") + rewards = rollout.rewards + discounts = torch.as_tensor(gamma, device=rewards.device, dtype=rewards.dtype) ** ( + torch.arange(rollout.num_steps, device=rewards.device, dtype=rewards.dtype) + ) + return ( + rewards * rollout.alive_mask.to(rewards.dtype) * discounts.unsqueeze(-1) + ).sum(dim=0) + + @configclass class APGCfg(AlgorithmCfg): """Analytic policy-gradient config. @@ -60,6 +97,7 @@ class APGCfg(AlgorithmCfg): ent_coef: float = 0.0 skip_nonfinite_updates: bool = True + max_grad_norm_before_clip: float = 0.0 class APG(BaseAlgorithm[DifferentiableRollout]): @@ -68,6 +106,8 @@ class APG(BaseAlgorithm[DifferentiableRollout]): rollout_kind = RolloutKind.DIFFERENTIABLE def __init__(self, cfg: APGCfg, policy: torch.nn.Module) -> None: + if cfg.max_grad_norm_before_clip < 0.0: + raise ValueError("max_grad_norm_before_clip cannot be negative.") self.cfg = cfg self.policy = policy self.device = torch.device(cfg.device) @@ -79,6 +119,7 @@ def __init__(self, cfg: APGCfg, policy: torch.nn.Module) -> None: self._objective_total = 0.0 self._entropy_total = 0.0 self._num_accumulated_steps = 0 + self._action_gradient_stats: list[BatchedGradientNormStats] = [] def update(self, rollout: DifferentiableRollout) -> Dict[str, float]: """Apply one pathwise-gradient update from a rollout segment.""" @@ -101,6 +142,7 @@ def begin_update(self) -> None: self._objective_total = 0.0 self._entropy_total = 0.0 self._num_accumulated_steps = 0 + self._action_gradient_stats = [] def accumulate_segment(self, rollout: DifferentiableRollout) -> None: """Accumulate gradients from one TBPTT segment without stepping the optimizer.""" @@ -116,27 +158,105 @@ def accumulate_segment(self, rollout: DifferentiableRollout) -> None: objective = returns.mean() entropy = entropy_returns.mean() loss = -objective - self.cfg.ent_coef * entropy + self._accumulate_loss( + rollout, + loss=loss, + objective=objective, + entropy=entropy, + ) + + def accumulate_complete_rollout( + self, + rollout: DifferentiableRollout, + *, + objective_scale: float | torch.Tensor = 1.0, + accumulation_scale: float = 1.0, + ) -> None: + """Accumulate one independent full-horizon rollout. + + Args: + rollout: Complete graph-preserving trajectory. + objective_scale: Scalar or per-environment return multiplier. + accumulation_scale: Loss multiplier, normally reciprocal to the + number of independent rollout microbatches in one update. + + Raises: + RuntimeError: If no APG update is active. + ValueError: If the rollout, scaling, or accumulation factor is invalid. + """ + if not self._update_active: + raise RuntimeError("Call begin_update() before accumulating a rollout.") + if rollout.num_steps == 0: + raise ValueError("APG requires a non-empty differentiable rollout.") + if accumulation_scale <= 0.0: + raise ValueError("accumulation_scale must be positive.") + + returns = complete_discounted_return(rollout, self.cfg.gamma) + scale = torch.as_tensor( + objective_scale, + dtype=returns.dtype, + device=returns.device, + ).detach() + if scale.ndim > 1 or (scale.ndim == 1 and scale.shape != returns.shape): + raise ValueError( + "objective_scale must be scalar or have one value per environment." + ) + if not bool(torch.isfinite(scale).all()): + raise ValueError("objective_scale must contain only finite values.") + + entropy_returns = torch.zeros_like(returns) + discount = torch.ones_like(returns) + alive = torch.ones_like(returns, dtype=torch.bool) + for transition in rollout.transitions: + if "entropy" in transition.policy_output.keys(): + entropy_returns = entropy_returns + ( + discount + * transition.policy_output["entropy"] + * alive.to(discount.dtype) + ) + alive = alive & ~transition.done + discount = discount * self.cfg.gamma + + objective = (returns * scale).mean() + entropy = (entropy_returns * scale).mean() + loss = (-objective - self.cfg.ent_coef * entropy) * accumulation_scale + self._accumulate_loss( + rollout, + loss=loss, + objective=objective * accumulation_scale, + entropy=entropy * accumulation_scale, + ) + + def _accumulate_loss( + self, + rollout: DifferentiableRollout, + *, + loss: torch.Tensor, + objective: torch.Tensor, + entropy: torch.Tensor, + ) -> None: + """Validate and backpropagate one contribution to the active update.""" self._loss_total += float(loss.detach()) self._objective_total += float(objective.detach()) self._entropy_total += float(entropy.detach()) self._num_accumulated_steps += rollout.num_steps + if rollout.action_gradient_stats is not None: + self._action_gradient_stats.append(rollout.action_gradient_stats) - if not bool(torch.isfinite(loss)): - if not self.cfg.skip_nonfinite_updates: - raise FloatingPointError("APG produced a non-finite loss.") - self._update_valid = False - self.optimizer.zero_grad(set_to_none=True) - return - if not self._update_valid: - return + loss_is_finite = bool(torch.isfinite(loss)) + if not loss_is_finite and not self.cfg.skip_nonfinite_updates: + raise FloatingPointError("APG produced a non-finite loss.") + # Backward is also the lifecycle boundary for custom differentiable + # simulator steps. Invoke it even when this accumulation window is + # already invalid so every rollout can release its retained graph/tape. loss.backward() parameters = tuple(self.policy.parameters()) gradients_are_finite = all( parameter.grad is None or bool(torch.isfinite(parameter.grad).all()) for parameter in parameters ) - if not gradients_are_finite: + if not loss_is_finite or not gradients_are_finite or not self._update_valid: if not self.cfg.skip_nonfinite_updates: raise FloatingPointError("APG produced a non-finite policy gradient.") self._update_valid = False @@ -155,6 +275,7 @@ def finish_update(self) -> Dict[str, float]: metrics = self._accumulated_metrics( grad_norm=float("nan"), skipped_update=1.0, + skipped_excessive_gradient=0.0, ) self._update_active = False return metrics @@ -162,11 +283,25 @@ def finish_update(self) -> Dict[str, float]: parameters, self.cfg.max_grad_norm, ) + excessive_gradient = not bool(torch.isfinite(grad_norm)) or ( + self.cfg.max_grad_norm_before_clip > 0.0 + and float(grad_norm) > self.cfg.max_grad_norm_before_clip + ) + if excessive_gradient: + self.optimizer.zero_grad(set_to_none=True) + metrics = self._accumulated_metrics( + grad_norm=float(grad_norm.detach()), + skipped_update=1.0, + skipped_excessive_gradient=1.0, + ) + self._update_active = False + return metrics self.optimizer.step() self._step_scheduler() metrics = self._accumulated_metrics( grad_norm=float(grad_norm.detach()), skipped_update=0.0, + skipped_excessive_gradient=0.0, ) self._update_active = False return metrics @@ -174,6 +309,7 @@ def finish_update(self) -> Dict[str, float]: def cancel_update(self) -> None: self.optimizer.zero_grad(set_to_none=True) self._update_active = False + self._action_gradient_stats = [] def _discounted_terms( self, @@ -205,12 +341,54 @@ def _accumulated_metrics( *, grad_norm: float, skipped_update: float, + skipped_excessive_gradient: float, ) -> Dict[str, float]: - return { + metrics = { "loss": self._loss_total, "objective": self._objective_total, "entropy": self._entropy_total, "grad_norm": grad_norm, "skipped_update": skipped_update, + "skipped_excessive_gradient": skipped_excessive_gradient, "learning_rate": self.current_learning_rate(), } + if self._action_gradient_stats: + rows = sum(float(stats.rows) for stats in self._action_gradient_stats) + finite_rows = sum( + float(stats.finite_rows) for stats in self._action_gradient_stats + ) + metrics.update( + { + "action_adjoint_preclip_mean_norm": ( + sum( + float(stats.norm_sum) + for stats in self._action_gradient_stats + ) + / finite_rows + if finite_rows > 0.0 + else 0.0 + ), + "action_adjoint_preclip_max_norm": max( + float(stats.norm_max) for stats in self._action_gradient_stats + ), + "action_adjoint_clipped_fraction": ( + sum( + float(stats.clipped_rows) + for stats in self._action_gradient_stats + ) + / rows + if rows > 0.0 + else 0.0 + ), + "action_adjoint_nonfinite_fraction": ( + sum( + float(stats.nonfinite_rows) + for stats in self._action_gradient_stats + ) + / rows + if rows > 0.0 + else 0.0 + ), + } + ) + return metrics diff --git a/embodichain/learning/rl/collector/differentiable.py b/embodichain/learning/rl/collector/differentiable.py index 787c7c2d4..f8550633b 100644 --- a/embodichain/learning/rl/collector/differentiable.py +++ b/embodichain/learning/rl/collector/differentiable.py @@ -28,7 +28,12 @@ DifferentiableObservation, DifferentiableVecEnv, ) +from embodichain.learning.rl.gradients import ( + BatchedGradientNormStats, + clip_batched_gradient_norm, +) from embodichain.learning.rl.models import Policy +from embodichain.learning.rl.normalization import RunningObservationNormalizer from embodichain.learning.rl.utils import flatten_dict_observation __all__ = [ @@ -67,6 +72,7 @@ class DifferentiableRollout: initial_observation: torch.Tensor transitions: tuple[DifferentiableTransition, ...] + action_gradient_stats: BatchedGradientNormStats | None = None @property def num_steps(self) -> int: @@ -89,6 +95,39 @@ def rewards(self) -> torch.Tensor: ) return torch.stack([transition.reward for transition in self.transitions]) + @property + def observations(self) -> torch.Tensor: + """Stack the raw policy observations. + + Returns: + Tensor shaped ``[time, num_envs, features]``. + """ + if not self.transitions: + return self.initial_observation.new_empty( + (0,) + tuple(self.initial_observation.shape) + ) + return torch.stack([transition.observation for transition in self.transitions]) + + @property + def alive_mask(self) -> torch.Tensor: + """Return rows active before each step, stopping after the first done. + + Returns: + Boolean tensor shaped ``[time, num_envs]``. + """ + if not self.transitions: + return torch.empty( + (0, self.initial_observation.shape[0]), + dtype=torch.bool, + device=self.initial_observation.device, + ) + alive = torch.ones_like(self.transitions[0].done, dtype=torch.bool) + masks = [] + for transition in self.transitions: + masks.append(alive) + alive = alive & ~transition.done + return torch.stack(masks) + class DifferentiableCollector: """Collect graph-preserving rollouts without a preallocated buffer.""" @@ -98,11 +137,38 @@ def __init__( env: DifferentiableVecEnv, policy: Policy, device: torch.device, + *, + observation_normalizer: RunningObservationNormalizer | None = None, + clip_actions_to_space: bool = False, + action_adjoint_max_norm: float = 0.0, ) -> None: + if action_adjoint_max_norm < 0.0: + raise ValueError("action_adjoint_max_norm cannot be negative.") self.env = env self.policy = policy self.device = device + self.observation_normalizer = observation_normalizer + self.clip_actions_to_space = bool(clip_actions_to_space) + self.action_adjoint_max_norm = float(action_adjoint_max_norm) self._observation: DifferentiableObservation | None = None + self._action_lower: torch.Tensor | None = None + self._action_upper: torch.Tensor | None = None + if self.clip_actions_to_space: + action_space = self.env.single_action_space + if not hasattr(action_space, "low") or not hasattr(action_space, "high"): + raise TypeError( + "clip_actions_to_space requires an action space with low/high bounds." + ) + self._action_lower = torch.as_tensor( + action_space.low, + device=self.device, + dtype=torch.float32, + ) + self._action_upper = torch.as_tensor( + action_space.high, + device=self.device, + dtype=torch.float32, + ) def reset( self, *, seed: int | None = None @@ -138,11 +204,21 @@ def collect( initial_observation = self._flatten_observation(self._observation) transitions: list[DifferentiableTransition] = [] + gradient_stats = ( + BatchedGradientNormStats(self.device) + if self.action_adjoint_max_norm > 0.0 + else None + ) for _ in range(num_steps): observation = self._flatten_observation(self._observation) + policy_observation = ( + self.observation_normalizer.normalize(observation) + if self.observation_normalizer is not None + else observation + ) policy_input = TensorDict( - {"obs": observation}, + {"obs": policy_observation}, batch_size=[self.env.num_envs], device=self.device, ) @@ -150,8 +226,25 @@ def collect( policy_input, deterministic=deterministic, ) + action = policy_output["action"] + if self.clip_actions_to_space: + assert self._action_lower is not None + assert self._action_upper is not None + action = torch.maximum( + torch.minimum(action, self._action_upper), + self._action_lower, + ) + policy_output["action"] = action + if gradient_stats is not None and action.requires_grad: + action.register_hook( + lambda gradient, stats=gradient_stats: clip_batched_gradient_norm( + gradient, + self.action_adjoint_max_norm, + stats, + ) + ) next_observation, reward, terminated, truncated, info = self.env.step( - policy_output["action"] + action ) transition = DifferentiableTransition( observation=observation, @@ -171,6 +264,7 @@ def collect( return DifferentiableRollout( initial_observation=initial_observation, transitions=tuple(transitions), + action_gradient_stats=gradient_stats, ) def detach_state(self) -> torch.Tensor: diff --git a/embodichain/learning/rl/differentiable_trainer.py b/embodichain/learning/rl/differentiable_trainer.py index a69cee762..24e487f74 100644 --- a/embodichain/learning/rl/differentiable_trainer.py +++ b/embodichain/learning/rl/differentiable_trainer.py @@ -28,10 +28,18 @@ import wandb from embodichain.learning.rl.algo import APG -from embodichain.learning.rl.collector import DifferentiableCollector -from embodichain.learning.rl.env import DifferentiableVecEnv +from embodichain.learning.rl.collector import ( + DifferentiableCollector, + DifferentiableRollout, +) +from embodichain.learning.rl.env import ( + DifferentiableRolloutSpec, + DifferentiableVecEnv, + ScheduledDifferentiableVecEnv, +) from embodichain.learning.rl.evaluation import evaluate_episodes from embodichain.learning.rl.models import Policy +from embodichain.learning.rl.normalization import RunningObservationNormalizer from embodichain.learning.rl.utils import LRSchedulerCfg, build_lr_scheduler from embodichain.utils import configclass @@ -45,11 +53,17 @@ @configclass class DifferentiableTrainerCfg: - """Configuration for graph-preserving segmented training.""" + """Configuration for segmented or complete graph-preserving training.""" segment_length: int = 16 update_horizon: int | None = None + rollout_mode: str = "segmented" + gradient_accumulation_steps: int = 1 deterministic_actions: bool = False + clip_actions_to_space: bool = False + action_adjoint_max_norm: float = 0.0 + normalize_observations: bool = False + rollout_seed: int | None = None checkpoint_dir: str = "outputs/checkpoints" experiment_name: str = "apg" save_frequency_updates: int = 0 @@ -62,7 +76,7 @@ class DifferentiableTrainerCfg: class DifferentiableTrainer: - """Coordinate APG updates and truncated-backpropagation boundaries.""" + """Coordinate APG updates, full rollouts, and optional TBPTT boundaries.""" def __init__( self, @@ -72,16 +86,25 @@ def __init__( algorithm: APG, writer: SummaryWriter | None = None, eval_env: DifferentiableVecEnv | None = None, + observation_normalizer: RunningObservationNormalizer | None = None, ) -> None: if cfg.segment_length <= 0: raise ValueError("segment_length must be positive.") update_horizon = ( cfg.segment_length if cfg.update_horizon is None else cfg.update_horizon ) - if update_horizon < cfg.segment_length: + if cfg.rollout_mode not in {"segmented", "complete"}: + raise ValueError("rollout_mode must be 'segmented' or 'complete'.") + if cfg.rollout_mode == "segmented" and update_horizon < cfg.segment_length: raise ValueError("update_horizon must be at least segment_length.") - if update_horizon % cfg.segment_length != 0: + if cfg.rollout_mode == "segmented" and update_horizon % cfg.segment_length != 0: raise ValueError("update_horizon must be divisible by segment_length.") + if update_horizon <= 0: + raise ValueError("update_horizon must be positive.") + if cfg.gradient_accumulation_steps <= 0: + raise ValueError("gradient_accumulation_steps must be positive.") + if cfg.action_adjoint_max_norm < 0.0: + raise ValueError("action_adjoint_max_norm cannot be negative.") if cfg.save_frequency_updates < 0: raise ValueError("save_frequency_updates cannot be negative.") if cfg.eval_frequency_steps < 0: @@ -100,10 +123,22 @@ def __init__( self.algorithm = algorithm self.writer = writer self.eval_env = eval_env + if observation_normalizer is None and cfg.normalize_observations: + observation_dim = int(env.single_observation_space.shape[-1]) + normalize_mask = getattr(env, "observation_normalize_mask", None) + observation_normalizer = RunningObservationNormalizer( + observation_dim, + algorithm.device, + normalize_mask=normalize_mask, + ) + self.observation_normalizer = observation_normalizer self.collector = DifferentiableCollector( env=env, policy=policy, device=algorithm.device, + observation_normalizer=observation_normalizer, + clip_actions_to_space=cfg.clip_actions_to_space, + action_adjoint_max_norm=cfg.action_adjoint_max_norm, ) self.global_step = 0 self.num_updates = 0 @@ -125,43 +160,72 @@ def __init__( self._next_eval_step = ( cfg.eval_frequency_steps if cfg.eval_frequency_steps > 0 else None ) + self._rollout_index = 0 + + def train( + self, + total_timesteps: int | None = None, + *, + total_updates: int | None = None, + ) -> dict[str, Any]: + """Train to a vector-transition or optimizer-update budget. + + ``total_updates`` is the stable budget for scheduled variable-horizon + training. Exactly one budget may be supplied. - def train(self, total_timesteps: int) -> dict[str, Any]: - """Train until at least ``total_timesteps`` vector transitions exist.""" - if total_timesteps < 0: + Args: + total_timesteps: Optional absolute vector-transition budget. + total_updates: Optional absolute optimizer-update budget. + + Returns: + Current training counters, metrics, histories, and checkpoint paths. + + Raises: + ValueError: If neither/both budgets are supplied or one is negative. + """ + if total_timesteps is None and total_updates is None: + raise ValueError("Provide total_timesteps or total_updates.") + if total_timesteps is not None and total_updates is not None: + raise ValueError("Provide only one training budget.") + if total_timesteps is not None and total_timesteps < 0: raise ValueError("total_timesteps cannot be negative.") + if total_updates is not None and total_updates < 0: + raise ValueError("total_updates cannot be negative.") - steps_per_update = self.update_horizon * self.env.num_envs - if total_timesteps > 0 and steps_per_update > 0: - total_updates = math.ceil(total_timesteps / steps_per_update) - self.algorithm.bind_schedule(total_updates=total_updates) + if total_updates is not None: + if total_updates > 0: + self.algorithm.bind_schedule(total_updates=total_updates) + else: + assert total_timesteps is not None + steps_per_update = ( + self.update_horizon + * self.env.num_envs + * ( + self.cfg.gradient_accumulation_steps + if self.cfg.rollout_mode == "complete" + else 1 + ) + ) + if total_timesteps > 0 and steps_per_update > 0: + estimated_updates = math.ceil(total_timesteps / steps_per_update) + self.algorithm.bind_schedule(total_updates=estimated_updates) self.policy.train() - while self.global_step < total_timesteps: - remaining_vector_steps = math.ceil( - (total_timesteps - self.global_step) / self.env.num_envs - ) - update_steps = min(self.update_horizon, remaining_vector_steps) - collected_steps = 0 - self.algorithm.begin_update() - try: - while collected_steps < update_steps: - segment_steps = min( - self.cfg.segment_length, - update_steps - collected_steps, - ) - rollout = self.collector.collect( - segment_steps, - deterministic=self.cfg.deterministic_actions, - on_step_callback=self._on_step, + while ( + self.num_updates < total_updates + if total_updates is not None + else self.global_step < total_timesteps + ): + if self.cfg.rollout_mode == "complete": + collected_steps, metrics = self._complete_rollout_update() + else: + update_steps = self.update_horizon + if total_timesteps is not None: + remaining_vector_steps = math.ceil( + (total_timesteps - self.global_step) / self.env.num_envs ) - self.algorithm.accumulate_segment(rollout) - self.collector.detach_state() - collected_steps += rollout.num_steps - metrics = self.algorithm.finish_update() - except Exception: - self.algorithm.cancel_update() - raise + update_steps = min(self.update_horizon, remaining_vector_steps) + collected_steps, metrics = self._segmented_update(update_steps) self.global_step += collected_steps * self.env.num_envs self.num_updates += 1 @@ -202,6 +266,80 @@ def train(self, total_timesteps: int) -> dict[str, Any]: return self.get_summary() + def _segmented_update(self, update_steps: int) -> tuple[int, dict[str, float]]: + """Run one backward-compatible TBPTT optimizer update.""" + collected_steps = 0 + self.algorithm.begin_update() + try: + while collected_steps < update_steps: + segment_steps = min( + self.cfg.segment_length, + update_steps - collected_steps, + ) + rollout = self.collector.collect( + segment_steps, + deterministic=self.cfg.deterministic_actions, + on_step_callback=self._on_step, + ) + self.algorithm.accumulate_segment(rollout) + self._update_observation_normalizer(rollout) + self.collector.detach_state() + collected_steps += rollout.num_steps + return collected_steps, self.algorithm.finish_update() + except Exception: + self.algorithm.cancel_update() + raise + + def _complete_rollout_update(self) -> tuple[int, dict[str, float]]: + """Accumulate independent full trajectories and apply one APG step.""" + accumulation_steps = self.cfg.gradient_accumulation_steps + collected_steps = 0 + metadata: dict[str, list[float]] = {} + self.algorithm.begin_update() + try: + for _ in range(accumulation_steps): + spec = self._prepare_complete_rollout() + seed = self.cfg.rollout_seed if self._rollout_index == 0 else None + self.collector.reset(seed=seed) + rollout = self.collector.collect( + spec.num_steps, + deterministic=self.cfg.deterministic_actions, + on_step_callback=self._on_step, + ) + self.algorithm.accumulate_complete_rollout( + rollout, + objective_scale=spec.objective_scale, + accumulation_scale=1.0 / accumulation_steps, + ) + self._update_observation_normalizer(rollout) + self.collector.detach_state() + collected_steps += rollout.num_steps + for key, value in spec.metadata.items(): + metadata.setdefault(str(key), []).append(float(value)) + self._rollout_index += 1 + metrics = self.algorithm.finish_update() + except Exception: + self.algorithm.cancel_update() + raise + for key, values in metadata.items(): + metrics[f"rollout_{key}_mean"] = sum(values) / len(values) + return collected_steps, metrics + + def _prepare_complete_rollout(self) -> DifferentiableRolloutSpec: + if isinstance(self.env, ScheduledDifferentiableVecEnv): + return self.env.prepare_differentiable_rollout(self._rollout_index) + return DifferentiableRolloutSpec(num_steps=self.update_horizon) + + def _update_observation_normalizer( + self, + rollout: DifferentiableRollout, + ) -> None: + if self.observation_normalizer is None: + return + observations = rollout.observations + alive_observations = observations[rollout.alive_mask] + self.observation_normalizer.update(alive_observations.detach()) + def save_checkpoint(self, path: str | Path | None = None) -> str: """Save policy, optimizer, and trainer counters.""" if path is None: @@ -218,7 +356,10 @@ def save_checkpoint(self, path: str | Path | None = None) -> str: "policy": self.policy.state_dict(), "optimizer": self.algorithm.optimizer.state_dict(), "best_eval_value": self.best_eval_value, + "rollout_index": self._rollout_index, } + if self.observation_normalizer is not None: + payload["observation_normalizer"] = self.observation_normalizer.state_dict() if self.algorithm.lr_scheduler is not None: payload["lr_scheduler"] = self.algorithm.lr_scheduler.state_dict() payload["lr_scheduler_cfg"] = { @@ -259,6 +400,15 @@ def load_checkpoint(self, path: str | Path) -> None: self.global_step = int(checkpoint["global_step"]) self.num_updates = int(checkpoint["num_updates"]) self.best_eval_value = checkpoint.get("best_eval_value") + self._rollout_index = int(checkpoint.get("rollout_index", self.num_updates)) + normalizer_state = checkpoint.get("observation_normalizer") + if normalizer_state is not None: + if self.observation_normalizer is None: + raise ValueError( + "Checkpoint contains observation normalization state, but the " + "trainer has normalization disabled." + ) + self.observation_normalizer.load_state_dict(normalizer_state) self.latest_checkpoint_path = str(path) def get_summary(self) -> dict[str, Any]: @@ -305,6 +455,11 @@ def _evaluate(self) -> dict[str, float]: num_episodes=self.cfg.num_eval_episodes, device=self.algorithm.device, seed=self.cfg.eval_seed, + observation_transform=( + self.observation_normalizer.normalize + if self.observation_normalizer is not None + else None + ), ) entry = {"global_step": float(self.global_step), **metrics} self.eval_history.append(entry) diff --git a/embodichain/learning/rl/env.py b/embodichain/learning/rl/env.py index 718c3df7f..60bdeaff7 100644 --- a/embodichain/learning/rl/env.py +++ b/embodichain/learning/rl/env.py @@ -19,6 +19,7 @@ from __future__ import annotations from collections.abc import Callable +from dataclasses import dataclass, field from typing import Any, Mapping, Protocol, TypeAlias, runtime_checkable import torch @@ -27,11 +28,14 @@ __all__ = [ "DifferentiableObservation", + "DifferentiableRolloutSpec", "DifferentiableVecEnv", "LearningVecEnv", "build_learning_env", "get_registered_learning_env_names", "register_learning_env", + "ScheduledDifferentiableVecEnv", + "stratified_rollout_value", ] DifferentiableObservation: TypeAlias = torch.Tensor | TensorDict @@ -40,6 +44,48 @@ _LEARNING_ENV_REGISTRY: dict[str, LearningEnvFactory] = {} +@dataclass(frozen=True) +class DifferentiableRolloutSpec: + """Describe one independent complete rollout used for a gradient microbatch. + + Args: + num_steps: Full rollout horizon. The trainer must not detach or truncate it. + objective_scale: Per-environment or scalar multiplier applied to returns. + metadata: Scalar labels recorded with training metrics. + """ + + num_steps: int + objective_scale: float | torch.Tensor = 1.0 + metadata: Mapping[str, float] = field(default_factory=dict) + + def __post_init__(self) -> None: + if isinstance(self.num_steps, bool) or int(self.num_steps) != self.num_steps: + raise TypeError("num_steps must be a positive integer.") + if self.num_steps <= 0: + raise ValueError("num_steps must be a positive integer.") + + +def stratified_rollout_value(index: int, minimum: int, maximum: int) -> int: + """Cycle uniformly through an integer range and rotate each cycle's order. + + Args: + index: Zero-based rollout index. + minimum: Inclusive minimum scheduled value. + maximum: Inclusive maximum scheduled value. + + Returns: + Scheduled integer for ``index``. + + Raises: + ValueError: If the inclusive range is empty. + """ + count = int(maximum) - int(minimum) + 1 + if count < 1: + raise ValueError("minimum must be less than or equal to maximum.") + cycle, position = divmod(int(index), count) + return int(minimum) + ((position + cycle) % count) + + @runtime_checkable class LearningVecEnv(Protocol): """Structural interface shared by lightweight vector environments.""" @@ -88,6 +134,31 @@ def detach_state(self) -> DifferentiableObservation: ... +@runtime_checkable +class ScheduledDifferentiableVecEnv(DifferentiableVecEnv, Protocol): + """Differentiable env that schedules variable independent rollouts. + + The trainer calls :meth:`prepare_differentiable_rollout` immediately before + resetting the environment. Implementations may select task difficulty for + the next reset, such as an ordered-waypoint count, and must return the full + horizon and objective scaling for that selection. + """ + + def prepare_differentiable_rollout( + self, + rollout_index: int, + ) -> DifferentiableRolloutSpec: + """Configure the next reset and return its complete-rollout contract. + + Args: + rollout_index: Zero-based independent-rollout index. + + Returns: + Full horizon, objective scaling, and optional metric metadata. + """ + ... + + def _is_nested_package_shadow(existing: Any, candidate: Any) -> bool: """Return True when ``candidate`` is an editable-install nested duplicate. diff --git a/embodichain/learning/rl/evaluation.py b/embodichain/learning/rl/evaluation.py index 974a2ab7c..9e5277ea9 100644 --- a/embodichain/learning/rl/evaluation.py +++ b/embodichain/learning/rl/evaluation.py @@ -87,8 +87,20 @@ def evaluate_episodes( device: torch.device | str, seed: int | None = None, on_step: Callable[[dict[str, Any]], None] | None = None, + observation_transform: Callable[[torch.Tensor], torch.Tensor] | None = None, ) -> dict[str, float]: - """Evaluate exactly ``num_episodes`` completed asynchronous episodes.""" + """Evaluate exactly ``num_episodes`` completed asynchronous episodes. + + Args: + policy: Policy evaluated with deterministic actions. + env: Vector environment with automatic row reset. + num_episodes: Exact number of completed episodes to collect. + device: Policy device. + seed: Optional environment reset seed. + on_step: Optional callback for raw step info. + observation_transform: Optional frozen preprocessing transform, such as + a running observation normalizer. + """ if num_episodes <= 0: raise ValueError("num_episodes must be positive.") device = torch.device(device) @@ -107,6 +119,8 @@ def evaluate_episodes( observation, _ = env.reset(seed=seed) while len(returns) < num_episodes: flat_observation = _flat_observation(observation, device) + if observation_transform is not None: + flat_observation = observation_transform(flat_observation) policy_input = TensorDict( {"obs": flat_observation}, batch_size=[num_envs], diff --git a/embodichain/learning/rl/gradients.py b/embodichain/learning/rl/gradients.py new file mode 100644 index 000000000..99828fc5d --- /dev/null +++ b/embodichain/learning/rl/gradients.py @@ -0,0 +1,127 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Gradient stabilization primitives for differentiable rollouts.""" + +from __future__ import annotations + +import torch + +__all__ = ["BatchedGradientNormStats", "clip_batched_gradient_norm"] + + +class BatchedGradientNormStats: + """Accumulate row-wise adjoint norm and clipping statistics on-device. + + Args: + device: Device on which hook-side counters are accumulated. + """ + + def __init__(self, device: torch.device | str) -> None: + self.device = torch.device(device) + self.norm_sum = torch.zeros((), device=self.device) + self.norm_max = torch.zeros((), device=self.device) + self.finite_rows = torch.zeros((), device=self.device) + self.rows = torch.zeros((), device=self.device) + self.clipped_rows = torch.zeros((), device=self.device) + self.nonfinite_rows = torch.zeros((), device=self.device) + + def metrics(self) -> dict[str, float]: + """Return statistics after backward has invoked registered hooks. + + Returns: + Mean and maximum pre-clip norm plus clipped/non-finite fractions. + """ + rows = float(self.rows) + finite_rows = float(self.finite_rows) + return { + "action_adjoint_preclip_mean_norm": ( + float(self.norm_sum) / finite_rows if finite_rows > 0.0 else 0.0 + ), + "action_adjoint_preclip_max_norm": float(self.norm_max), + "action_adjoint_clipped_fraction": ( + float(self.clipped_rows) / rows if rows > 0.0 else 0.0 + ), + "action_adjoint_nonfinite_fraction": ( + float(self.nonfinite_rows) / rows if rows > 0.0 else 0.0 + ), + } + + +def clip_batched_gradient_norm( + gradient: torch.Tensor, + max_norm: float, + stats: BatchedGradientNormStats | None = None, +) -> torch.Tensor: + """Clip each batch row without shortening the differentiable time horizon. + + Norms are computed with max-absolute-value scaling to avoid overflow in + float32. A non-finite row is replaced with zeros while finite rows remain + independent from one another. + + Args: + gradient: Tensor whose first dimension identifies independent rows. + max_norm: Maximum L2 norm per row. Zero disables clipping. + stats: Optional on-device accumulator populated before clipping. + + Returns: + A finite tensor with the same shape, dtype, and device as ``gradient``. + + Raises: + ValueError: If ``max_norm`` is negative or ``gradient`` is not batched. + """ + if max_norm < 0.0: + raise ValueError("max_norm cannot be negative.") + if max_norm == 0.0: + return gradient + if gradient.ndim < 2: + raise ValueError("gradient must have a leading batch dimension.") + + flat = gradient.flatten(start_dim=1) + finite_rows = torch.isfinite(flat).all(dim=1, keepdim=True) + finite_values = torch.where(finite_rows, flat, torch.zeros_like(flat)) + max_abs = finite_values.abs().amax(dim=1, keepdim=True) + safe_max_abs = max_abs.clamp_min(1.0e-12) + scaled_norm = (finite_values / safe_max_abs).norm(dim=1, keepdim=True) + raw_norm = max_abs * scaled_norm + + if stats is not None: + with torch.no_grad(): + detached_norm = raw_norm.detach().flatten() + finite = finite_rows.detach().flatten() & torch.isfinite(detached_norm) + finite_norm = torch.where( + finite, + detached_norm, + torch.zeros_like(detached_norm), + ) + stats.norm_sum.add_(finite_norm.sum()) + stats.finite_rows.add_(finite.sum()) + stats.rows.add_(detached_norm.numel()) + stats.clipped_rows.add_((finite & (detached_norm > max_norm)).sum()) + stats.nonfinite_rows.add_((~finite).sum()) + stats.norm_max.copy_(torch.maximum(stats.norm_max, finite_norm.max())) + + scale = ((float(max_norm) / safe_max_abs) / scaled_norm.clamp_min(1.0)).clamp( + max=1.0 + ) + scale = torch.where(finite_rows, scale, torch.zeros_like(scale)) + broadcast_shape = (-1,) + (1,) * (gradient.ndim - 1) + safe_gradient = torch.where( + finite_rows.view(broadcast_shape), + gradient, + torch.zeros_like(gradient), + ) + return safe_gradient * scale.view(broadcast_shape) diff --git a/embodichain/learning/rl/normalization.py b/embodichain/learning/rl/normalization.py new file mode 100644 index 000000000..248e798b1 --- /dev/null +++ b/embodichain/learning/rl/normalization.py @@ -0,0 +1,180 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Running statistics for learning-environment observations.""" + +from __future__ import annotations + +from collections.abc import Mapping +from typing import Any + +import torch + +__all__ = ["RunningObservationNormalizer"] + + +class RunningObservationNormalizer: + """Normalize continuous observation fields with running Welford statistics. + + The optional mask leaves semantic fields such as one-hot encodings and + validity bits unchanged while still tracking a single flat observation. + Statistics are updated only when :meth:`update` is called; :meth:`normalize` + is side-effect free so a complete differentiable rollout uses one frozen + normalization transform. + + Args: + observation_dim: Flat observation dimension. + device: Device used for the running statistics. + normalize_mask: Boolean mask where ``True`` selects normalized fields. + initial_count: Positive pseudo-count used to stabilize the first update. + """ + + def __init__( + self, + observation_dim: int, + device: torch.device | str, + normalize_mask: torch.Tensor | None = None, + *, + initial_count: float = 1.0e-4, + ) -> None: + if observation_dim <= 0: + raise ValueError("observation_dim must be positive.") + if initial_count <= 0.0: + raise ValueError("initial_count must be positive.") + + self.observation_dim = int(observation_dim) + self.device = torch.device(device) + self.mean = torch.zeros(self.observation_dim, device=self.device) + self.var = torch.ones(self.observation_dim, device=self.device) + self.count = float(initial_count) + if normalize_mask is None: + normalize_mask = torch.ones( + self.observation_dim, + dtype=torch.bool, + device=self.device, + ) + normalize_mask = torch.as_tensor( + normalize_mask, + dtype=torch.bool, + device=self.device, + ) + if normalize_mask.shape != (self.observation_dim,): + raise ValueError( + "normalize_mask must have shape " + f"({self.observation_dim},), got {tuple(normalize_mask.shape)}." + ) + self.normalize_mask = normalize_mask.clone() + + @torch.no_grad() + def update(self, observations: torch.Tensor) -> None: + """Merge one observation batch into the running statistics. + + Args: + observations: Finite tensor shaped ``[batch, observation_dim]``. + + Raises: + ValueError: If the shape is incompatible or values are non-finite. + """ + observations = torch.as_tensor(observations, device=self.device) + if observations.ndim != 2 or observations.shape[1] != self.observation_dim: + raise ValueError( + "observations must have shape [batch, observation_dim], got " + f"{tuple(observations.shape)}." + ) + if observations.shape[0] == 0: + return + if not bool(torch.isfinite(observations).all()): + raise ValueError("observations must contain only finite values.") + + batch_mean = observations.mean(dim=0) + batch_var = observations.var(dim=0, unbiased=False) + batch_count = int(observations.shape[0]) + delta = batch_mean - self.mean + total_count = self.count + batch_count + self.mean.add_(delta * batch_count / total_count) + merged_m2 = ( + self.var * self.count + + batch_var * batch_count + + delta.square() * self.count * batch_count / total_count + ) + self.var.copy_(merged_m2 / total_count) + self.count = float(total_count) + + def normalize(self, observations: torch.Tensor) -> torch.Tensor: + """Apply the frozen running transform without detaching observations. + + Args: + observations: Tensor ending in the configured observation dimension. + + Returns: + Tensor with continuous fields normalized and semantic fields intact. + + Raises: + ValueError: If the trailing observation dimension is incompatible. + """ + if observations.shape[-1] != self.observation_dim: + raise ValueError( + "observations must end with observation_dim " + f"{self.observation_dim}, got {tuple(observations.shape)}." + ) + normalized = (observations - self.mean) / (self.var.sqrt() + 1.0e-8) + return torch.where(self.normalize_mask, normalized, observations) + + def state_dict(self) -> dict[str, Any]: + """Return a device-independent checkpoint payload. + + Returns: + Mapping containing mean, variance, count, and normalization mask. + """ + return { + "mean": self.mean.detach().cpu(), + "var": self.var.detach().cpu(), + "count": self.count, + "normalize_mask": self.normalize_mask.detach().cpu(), + } + + @torch.no_grad() + def load_state_dict(self, state_dict: Mapping[str, Any]) -> None: + """Restore statistics while validating their observation layout. + + Args: + state_dict: Payload produced by :meth:`state_dict`. + + Raises: + ValueError: If dimensions or the pseudo-count are invalid. + """ + mean = torch.as_tensor(state_dict["mean"], device=self.device) + var = torch.as_tensor(state_dict["var"], device=self.device) + mask = torch.as_tensor( + state_dict.get("normalize_mask", self.normalize_mask), + dtype=torch.bool, + device=self.device, + ) + expected_shape = (self.observation_dim,) + if mean.shape != expected_shape or var.shape != expected_shape: + raise ValueError( + "Normalizer checkpoint shape does not match observation_dim " + f"{self.observation_dim}." + ) + if mask.shape != expected_shape: + raise ValueError("Normalizer checkpoint mask has an incompatible shape.") + count = float(state_dict["count"]) + if count <= 0.0: + raise ValueError("Normalizer checkpoint count must be positive.") + self.mean.copy_(mean) + self.var.copy_(var) + self.normalize_mask.copy_(mask) + self.count = count diff --git a/embodichain/learning/rl/train.py b/embodichain/learning/rl/train.py index 2f442e805..5a626c4e5 100644 --- a/embodichain/learning/rl/train.py +++ b/embodichain/learning/rl/train.py @@ -44,7 +44,7 @@ from embodichain.learning.rl.routing import get_trainer_class from embodichain.learning.rl.utils import dict_to_tensordict, flatten_dict_observation from embodichain.learning.rl.utils.trainer import Trainer -from embodichain.utils import logger +from embodichain.utils import logger, set_seed from embodichain.lab.gym.utils.registration import ( build_env, discover_task_packages, @@ -188,9 +188,10 @@ def _train_learning_env( raise ValueError("CUDA was requested but is not available.") if device.type == "cuda": torch.cuda.set_device(device) - torch.cuda.manual_seed_all(seed) - np.random.seed(seed) - torch.manual_seed(seed) + set_seed( + seed, + deterministic=bool(trainer_cfg.get("torch_deterministic", False)), + ) env_block = trainer_cfg["learning_env"] if isinstance(env_block, str): @@ -251,9 +252,23 @@ def _train_learning_env( diff_cfg = DifferentiableTrainerCfg( segment_length=segment_length, update_horizon=update_horizon, + rollout_mode=str(trainer_cfg.get("rollout_mode", "segmented")), + gradient_accumulation_steps=int( + trainer_cfg.get("gradient_accumulation_steps", 1) + ), deterministic_actions=bool( trainer_cfg.get("deterministic_actions", False) ), + clip_actions_to_space=bool( + trainer_cfg.get("clip_actions_to_space", False) + ), + action_adjoint_max_norm=float( + trainer_cfg.get("action_adjoint_max_norm", 0.0) + ), + normalize_observations=bool( + trainer_cfg.get("normalize_observations", False) + ), + rollout_seed=seed, checkpoint_dir=str(checkpoint_dir), experiment_name=exp_name, save_frequency_updates=int( @@ -274,7 +289,12 @@ def _train_learning_env( writer=writer, eval_env=eval_env, ) - default_steps = iterations * update_horizon * num_envs + default_steps = ( + iterations + * update_horizon + * num_envs + * diff_cfg.gradient_accumulation_steps + ) else: buffer_size = int( trainer_cfg.get("buffer_size", trainer_cfg.get("rollout_steps", 256)) @@ -298,8 +318,15 @@ def _train_learning_env( best_eval_mode=trainer_cfg.get("best_eval_mode", "max"), ) default_steps = iterations * buffer_size * num_envs - total_timesteps = int(trainer_cfg.get("total_timesteps", default_steps)) - trainer.train(total_timesteps) + if ( + trainer_class is DifferentiableTrainer + and diff_cfg.rollout_mode == "complete" + and "total_timesteps" not in trainer_cfg + ): + trainer.train(total_updates=iterations) + else: + total_timesteps = int(trainer_cfg.get("total_timesteps", default_steps)) + trainer.train(total_timesteps) trainer.save_checkpoint() return trainer.get_summary() finally: @@ -466,11 +493,11 @@ def train_from_config( gpu_index = device.index if gpu_index is None: gpu_index = torch.cuda.current_device() - gym_env_cfg.sim_cfg.sim_device = torch.device(f"cuda:{gpu_index}") + gym_env_cfg.sim_cfg.device = torch.device(f"cuda:{gpu_index}") if hasattr(gym_env_cfg.sim_cfg, "gpu_id"): gym_env_cfg.sim_cfg.gpu_id = gpu_index else: - gym_env_cfg.sim_cfg.sim_device = torch.device("cpu") + gym_env_cfg.sim_cfg.device = torch.device("cpu") gym_env_cfg.sim_cfg.headless = headless gym_env_cfg.sim_cfg.render_cfg = RenderCfg(renderer=renderer) gym_env_cfg.sim_cfg.gpu_id = gpu_id @@ -485,7 +512,7 @@ def train_from_config( ) if rank == 0: logger.log_info( - f"Loaded gym_config from {gym_config_path} (env_id={gym_config_data['id']}, num_envs={gym_env_cfg.num_envs}, headless={gym_env_cfg.sim_cfg.headless}, renderer={gym_env_cfg.sim_cfg.render_cfg.renderer}, sim_device={gym_env_cfg.sim_cfg.sim_device})" + f"Loaded gym_config from {gym_config_path} (env_id={gym_config_data['id']}, num_envs={gym_env_cfg.num_envs}, headless={gym_env_cfg.sim_cfg.headless}, renderer={gym_env_cfg.sim_cfg.render_cfg.renderer}, device={gym_env_cfg.sim_cfg.device})" ) env = build_env(gym_config_data["id"], base_env_cfg=gym_env_cfg) diff --git a/embodichain/utils/configclass.py b/embodichain/utils/configclass.py index a2d0a5542..5813cfd49 100644 --- a/embodichain/utils/configclass.py +++ b/embodichain/utils/configclass.py @@ -154,6 +154,17 @@ def _combined(*args, **kwargs): return _combined +def _is_class_var_annotation(annotation: Any) -> bool: + """Return whether an eager or postponed annotation denotes ``ClassVar``.""" + if annotation is ClassVar or getattr(annotation, "__origin__", None) is ClassVar: + return True + if not isinstance(annotation, str): + return False + return annotation in {"ClassVar", "typing.ClassVar"} or annotation.startswith( + ("ClassVar[", "typing.ClassVar[") + ) + + def custom_post_init(obj): """Deepcopy all elements to avoid shared memory issues for mutable objects in dataclasses initialization. @@ -161,10 +172,13 @@ def custom_post_init(obj): proxy type i.e. a read only proxy for mapping objects. The error is thrown when using hierarchical data-classes for configuration. """ + annotations = obj.__class__.__dict__.get("__annotations__", {}) for key in dir(obj): # skip dunder members if key.startswith("__"): continue + if _is_class_var_annotation(annotations.get(key)): + continue # get data member value = getattr(obj, key) # check annotation @@ -538,8 +552,7 @@ class State: value = class_members.get(key, MISSING) # check if key belongs to ClassVar # in that case, we cannot use default_factory! - origin = getattr(ann[key], "__origin__", None) - if origin is ClassVar: + if _is_class_var_annotation(ann[key]): continue # check if f is MISSING # note: commented out for now since it causes issue with inheritance diff --git a/embodichain/utils/logger.py b/embodichain/utils/logger.py index 9011c0439..f8f961ea5 100644 --- a/embodichain/utils/logger.py +++ b/embodichain/utils/logger.py @@ -17,6 +17,8 @@ from __future__ import annotations import logging +import os +import re import time from typing import NoReturn @@ -64,8 +66,21 @@ def format(self, record: logging.LogRecord) -> str: return super().format(record) +class _ConsoleHandler(logging.StreamHandler): + """Keep terminal colors out of redirected log files and NO_COLOR output.""" + + def format(self, record: logging.LogRecord) -> str: + text = super().format(record) + if ( + "NO_COLOR" in os.environ + or not getattr(self.stream, "isatty", lambda: False)() + ): + text = re.sub(r"\x1b\[[0-9;]*m", "", text) + return text + + _DEFAULT_FORMATTER = _UTCFormatter(_LOG_FORMAT, datefmt=_DATE_FORMAT) -_DEFAULT_HANDLER = logging.StreamHandler() +_DEFAULT_HANDLER = _ConsoleHandler() _DEFAULT_HANDLER.setFormatter(_DEFAULT_FORMATTER) logging.basicConfig(level=logging.INFO, handlers=[_DEFAULT_HANDLER]) diff --git a/embodichain/utils/math.py b/embodichain/utils/math.py index 1e5842d6a..5b5fa6278 100644 --- a/embodichain/utils/math.py +++ b/embodichain/utils/math.py @@ -237,12 +237,12 @@ def quat_unique(q: torch.Tensor) -> torch.Tensor: rotation. This function ensures the real part of the quaternion is non-negative. Args: - q: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + q: The quaternion orientation in (x, y, z, w). Shape is (..., 4). Returns: Standardized quaternions. Shape is (..., 4). """ - return torch.where(q[..., 0:1] < 0, -q, q) + return torch.where(q[..., 3:4] < 0, -q, q) @torch.jit.script @@ -250,7 +250,7 @@ def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor: """Convert rotations given as quaternions to rotation matrices. Args: - quaternions: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + quaternions: The quaternion orientation in (x, y, z, w). Shape is (..., 4). Returns: Rotation matrices. The shape is (..., 3, 3). @@ -258,7 +258,7 @@ def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor: Reference: https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L41-L70 """ - r, i, j, k = torch.unbind(quaternions, -1) + i, j, k, r = torch.unbind(quaternions, -1) # pyre-fixme[58]: `/` is not supported for operand types `float` and `Tensor`. two_s = 2.0 / (quaternions * quaternions).sum(-1) @@ -282,14 +282,15 @@ def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor: def convert_quat( quat: torch.Tensor | np.ndarray, to: Literal["xyzw", "wxyz"] = "xyzw" ) -> torch.Tensor | np.ndarray: - """Converts quaternion from one convention to another. + """Convert a quaternion between ``wxyz`` and ``xyzw`` conventions. The convention to convert TO is specified as an optional argument. If to == 'xyzw', then the input is in 'wxyz' format, and vice-versa. Args: quat: The quaternion of shape (..., 4). - to: Convention to convert quaternion to.. Defaults to "xyzw". + to: Convention to convert the quaternion to. The input is interpreted as + the opposite convention. Defaults to ``"xyzw"``. Returns: The converted quaternion in specified convention. @@ -332,14 +333,14 @@ def quat_conjugate(q: torch.Tensor) -> torch.Tensor: """Computes the conjugate of a quaternion. Args: - q: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + q: The quaternion orientation in (x, y, z, w). Shape is (..., 4). Returns: - The conjugate quaternion in (w, x, y, z). Shape is (..., 4). + The conjugate quaternion in (x, y, z, w). Shape is (..., 4). """ shape = q.shape q = q.reshape(-1, 4) - return torch.cat((q[..., 0:1], -q[..., 1:]), dim=-1).view(shape) + return torch.cat((-q[..., :3], q[..., 3:4]), dim=-1).view(shape) @torch.jit.script @@ -347,11 +348,11 @@ def quat_inv(q: torch.Tensor, eps: float = 1e-9) -> torch.Tensor: """Computes the inverse of a quaternion. Args: - q: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + q: The quaternion orientation in (x, y, z, w). Shape is (N, 4). eps: A small value to avoid division by zero. Defaults to 1e-9. Returns: - The inverse quaternion in (w, x, y, z). Shape is (N, 4). + The inverse quaternion in (x, y, z, w). Shape is (N, 4). """ return quat_conjugate(q) / q.pow(2).sum(dim=-1, keepdim=True).clamp(min=eps) @@ -371,7 +372,7 @@ def quat_from_euler_xyz( yaw: Rotation around z-axis (in radians). Shape is (N,). Returns: - The quaternion in (w, x, y, z). Shape is (N, 4). + The quaternion in (x, y, z, w). Shape is (N, 4). """ cy = torch.cos(yaw * 0.5) sy = torch.sin(yaw * 0.5) @@ -385,7 +386,7 @@ def quat_from_euler_xyz( qy = cy * cr * sp + sy * sr * cp qz = sy * cr * cp - cy * sr * sp - return torch.stack([qw, qx, qy, qz], dim=-1) + return torch.stack([qx, qy, qz, qw], dim=-1) @torch.jit.script @@ -407,7 +408,7 @@ def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor: matrix: The rotation matrices. Shape is (..., 3, 3). Returns: - The quaternion in (w, x, y, z). Shape is (..., 4). + The quaternion in (x, y, z, w). Shape is (..., 4). Reference: https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L102-L161 @@ -454,16 +455,17 @@ def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor: # if not for numerical problems, quat_candidates[i] should be same (up to a sign), # forall i; we pick the best-conditioned one (with the largest denominator) - return quat_candidates[ + quaternion_wxyz = quat_candidates[ torch.nn.functional.one_hot(q_abs.argmax(dim=-1), num_classes=4) > 0.5, : ].reshape(batch_dim + (4,)) + return torch.cat([quaternion_wxyz[..., 1:], quaternion_wxyz[..., :1]], dim=-1) def xyz_quat_to_4x4_matrix(xyz_quat: torch.Tensor) -> torch.Tensor: - """Convert a 7D pose vector (x, y, z, qw, qx, qy, qz) to a 4x4 transformation matrix. + """Convert a 7D pose vector (x, y, z, qx, qy, qz, qw) to a 4x4 transformation matrix. Args: - xyz_quat: The pose vector in (x, y, z, qw, qx, qy, qz). Shape is (..., 7). + xyz_quat: The pose vector in (x, y, z, qx, qy, qz, qw). Shape is (..., 7). Returns: The transformation matrix. Shape is (..., 4, 4). @@ -492,7 +494,7 @@ def trans_matrix_to_xyz_quat(matrix: torch.Tensor) -> torch.Tensor: matrix: The pose transformation matrix in ((R, t), (0, 1)). Shape is (..., 4, 4). Returns: - The pose vector in (x, y, z, qw, qx, qy, qz). Shape is (..., 7). + The pose vector in (x, y, z, qx, qy, qz, qw). Shape is (..., 7). """ if matrix.shape[-2:] != (4, 4): raise ValueError(f"Invalid input shape {matrix.shape}, expected (..., 4, 4).") @@ -640,7 +642,7 @@ def euler_xyz_from_quat( The euler angles are assumed in XYZ extrinsic convention. Args: - quat: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + quat: The quaternion orientation in (x, y, z, w). Shape is (N, 4). wrap_to_2pi (bool): Whether to wrap output Euler angles into [0, 2π). If False, angles are returned in the default range (−π, π]. Defaults to False. @@ -651,7 +653,7 @@ def euler_xyz_from_quat( Reference: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles """ - q_w, q_x, q_y, q_z = quat[:, 0], quat[:, 1], quat[:, 2], quat[:, 3] + q_x, q_y, q_z, q_w = quat[:, 0], quat[:, 1], quat[:, 2], quat[:, 3] # roll (x-axis rotation) sin_roll = 2.0 * (q_w * q_x + q_y * q_z) cos_roll = 1 - 2 * (q_x * q_x + q_y * q_y) @@ -680,7 +682,7 @@ def axis_angle_from_quat(quat: torch.Tensor, eps: float = 1.0e-6) -> torch.Tenso """Convert rotations given as quaternions to axis/angle. Args: - quat: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + quat: The quaternion orientation in (x, y, z, w). Shape is (..., 4). eps: The tolerance for Taylor approximation. Defaults to 1.0e-6. Returns: @@ -690,21 +692,21 @@ def axis_angle_from_quat(quat: torch.Tensor, eps: float = 1.0e-6) -> torch.Tenso Reference: https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L526-L554 """ - # Modified to take in quat as [q_w, q_x, q_y, q_z] - # Quaternion is [q_w, q_x, q_y, q_z] = [cos(theta/2), n_x * sin(theta/2), n_y * sin(theta/2), n_z * sin(theta/2)] + # Modified to take in quat as [q_x, q_y, q_z, q_w] + # Quaternion is [q_x, q_y, q_z, q_w] = [n_x * sin(theta/2), n_y * sin(theta/2), n_z * sin(theta/2), cos(theta/2)] # Axis-angle is [a_x, a_y, a_z] = [theta * n_x, theta * n_y, theta * n_z] # Thus, axis-angle is [q_x, q_y, q_z] / (sin(theta/2) / theta) # When theta = 0, (sin(theta/2) / theta) is undefined # However, as theta --> 0, we can use the Taylor approximation 1/2 - theta^2 / 48 - quat = quat * (1.0 - 2.0 * (quat[..., 0:1] < 0.0)) - mag = torch.linalg.norm(quat[..., 1:], dim=-1) - half_angle = torch.atan2(mag, quat[..., 0]) + quat = quat * (1.0 - 2.0 * (quat[..., 3:4] < 0.0)) + mag = torch.linalg.norm(quat[..., :3], dim=-1) + half_angle = torch.atan2(mag, quat[..., 3]) angle = 2.0 * half_angle # check whether to apply Taylor approximation sin_half_angles_over_angles = torch.where( angle.abs() > eps, torch.sin(half_angle) / angle, 0.5 - angle * angle / 48 ) - return quat[..., 1:4] / sin_half_angles_over_angles.unsqueeze(-1) + return quat[..., :3] / sin_half_angles_over_angles.unsqueeze(-1) @torch.jit.script @@ -716,12 +718,12 @@ def quat_from_angle_axis(angle: torch.Tensor, axis: torch.Tensor) -> torch.Tenso axis: The axis of rotation. Shape is (N, 3). Returns: - The quaternion in (w, x, y, z). Shape is (N, 4). + The quaternion in (x, y, z, w). Shape is (N, 4). """ theta = (angle / 2).unsqueeze(-1) xyz = normalize(axis) * theta.sin() w = theta.cos() - return normalize(torch.cat([w, xyz], dim=-1)) + return normalize(torch.cat([xyz, w], dim=-1)) @torch.jit.script @@ -729,11 +731,11 @@ def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: """Multiply two quaternions together. Args: - q1: The first quaternion in (w, x, y, z). Shape is (..., 4). - q2: The second quaternion in (w, x, y, z). Shape is (..., 4). + q1: The first quaternion in (x, y, z, w). Shape is (..., 4). + q2: The second quaternion in (x, y, z, w). Shape is (..., 4). Returns: - The product of the two quaternions in (w, x, y, z). Shape is (..., 4). + The product of the two quaternions in (x, y, z, w). Shape is (..., 4). Raises: ValueError: Input shapes of ``q1`` and ``q2`` are not matching. @@ -747,8 +749,8 @@ def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: q1 = q1.reshape(-1, 4) q2 = q2.reshape(-1, 4) # extract components from quaternions - w1, x1, y1, z1 = q1[:, 0], q1[:, 1], q1[:, 2], q1[:, 3] - w2, x2, y2, z2 = q2[:, 0], q2[:, 1], q2[:, 2], q2[:, 3] + x1, y1, z1, w1 = q1[:, 0], q1[:, 1], q1[:, 2], q1[:, 3] + x2, y2, z2, w2 = q2[:, 0], q2[:, 1], q2[:, 2], q2[:, 3] # perform multiplication ww = (z1 + x1) * (x2 + y2) yy = (w1 - y1) * (w2 + z2) @@ -760,7 +762,7 @@ def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: y = qq - yy + (w1 - x1) * (y2 + z2) z = qq - zz + (z1 + y1) * (w2 - x2) - return torch.stack([w, x, y, z], dim=-1).view(shape) + return torch.stack([x, y, z, w], dim=-1).view(shape) @torch.jit.script @@ -768,21 +770,21 @@ def yaw_quat(quat: torch.Tensor) -> torch.Tensor: """Extract the yaw component of a quaternion. Args: - quat: The orientation in (w, x, y, z). Shape is (..., 4) + quat: The orientation in (x, y, z, w). Shape is (..., 4) Returns: A quaternion with only yaw component. """ shape = quat.shape quat_yaw = quat.view(-1, 4) - qw = quat_yaw[:, 0] - qx = quat_yaw[:, 1] - qy = quat_yaw[:, 2] - qz = quat_yaw[:, 3] + qx = quat_yaw[:, 0] + qy = quat_yaw[:, 1] + qz = quat_yaw[:, 2] + qw = quat_yaw[:, 3] yaw = torch.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)) quat_yaw = torch.zeros_like(quat_yaw) - quat_yaw[:, 3] = torch.sin(yaw / 2) - quat_yaw[:, 0] = torch.cos(yaw / 2) + quat_yaw[:, 2] = torch.sin(yaw / 2) + quat_yaw[:, 3] = torch.cos(yaw / 2) quat_yaw = normalize(quat_yaw) return quat_yaw.view(shape) @@ -792,8 +794,8 @@ def quat_box_minus(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: """The box-minus operator (quaternion difference) between two quaternions. Args: - q1: The first quaternion in (w, x, y, z). Shape is (N, 4). - q2: The second quaternion in (w, x, y, z). Shape is (N, 4). + q1: The first quaternion in (x, y, z, w). Shape is (N, 4). + q2: The second quaternion in (x, y, z, w). Shape is (N, 4). Returns: The difference between the two quaternions. Shape is (N, 3). @@ -812,7 +814,7 @@ def quat_box_plus( """The box-plus operator (quaternion update) to apply an increment to a quaternion. Args: - q: The initial quaternion in (w, x, y, z). Shape is (N, 4). + q: The initial quaternion in (x, y, z, w). Shape is (N, 4). delta: The axis-angle perturbation. Shape is (N, 3). eps: A small value to avoid division by zero. Defaults to 1e-6. @@ -837,7 +839,7 @@ def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: """Apply a quaternion rotation to a vector. Args: - quat: The quaternion in (w, x, y, z). Shape is (..., 4). + quat: The quaternion in (x, y, z, w). Shape is (..., 4). vec: The vector in (x, y, z). Shape is (..., 3). Returns: @@ -849,9 +851,9 @@ def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: quat = quat.reshape(-1, 4) vec = vec.reshape(-1, 3) # extract components from quaternions - xyz = quat[:, 1:] + xyz = quat[:, :3] t = xyz.cross(vec, dim=-1) * 2 - return (vec + quat[:, 0:1] * t + xyz.cross(t, dim=-1)).view(shape) + return (vec + quat[:, 3:4] * t + xyz.cross(t, dim=-1)).view(shape) @torch.jit.script @@ -859,7 +861,7 @@ def quat_apply_inverse(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: """Apply an inverse quaternion rotation to a vector. Args: - quat: The quaternion in (w, x, y, z). Shape is (..., 4). + quat: The quaternion in (x, y, z, w). Shape is (..., 4). vec: The vector in (x, y, z). Shape is (..., 3). Returns: @@ -871,9 +873,9 @@ def quat_apply_inverse(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: quat = quat.reshape(-1, 4) vec = vec.reshape(-1, 3) # extract components from quaternions - xyz = quat[:, 1:] + xyz = quat[:, :3] t = xyz.cross(vec, dim=-1) * 2 - return (vec - quat[:, 0:1] * t + xyz.cross(t, dim=-1)).view(shape) + return (vec - quat[:, 3:4] * t + xyz.cross(t, dim=-1)).view(shape) @torch.jit.script @@ -881,7 +883,7 @@ def quat_apply_yaw(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: """Rotate a vector only around the yaw-direction. Args: - quat: The orientation in (w, x, y, z). Shape is (N, 4). + quat: The orientation in (x, y, z, w). Shape is (N, 4). vec: The vector in (x, y, z). Shape is (N, 3). Returns: @@ -896,8 +898,8 @@ def quat_error_magnitude(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: """Computes the rotation difference between two quaternions. Args: - q1: The first quaternion in (w, x, y, z). Shape is (..., 4). - q2: The second quaternion in (w, x, y, z). Shape is (..., 4). + q1: The first quaternion in (x, y, z, w). Shape is (..., 4). + q2: The second quaternion in (x, y, z, w). Shape is (..., 4). Returns: Angular error between input quaternions in radians. @@ -952,7 +954,7 @@ def is_identity_pose(pos: torch.tensor, rot: torch.tensor) -> bool: Args: pos: The cartesian position. Shape is (N, 3). - rot: The quaternion in (w, x, y, z). Shape is (N, 4). + rot: The quaternion in (x, y, z, w). Shape is (N, 4). Returns: True if all the input poses result in identity transform. Otherwise, False. @@ -960,7 +962,7 @@ def is_identity_pose(pos: torch.tensor, rot: torch.tensor) -> bool: # create identity transformations pos_identity = torch.zeros_like(pos) rot_identity = torch.zeros_like(rot) - rot_identity[..., 0] = 1 + rot_identity[..., 3] = 1 # compare input to identity return torch.allclose(pos, pos_identity) and torch.allclose(rot, rot_identity) @@ -979,10 +981,10 @@ def combine_frame_transforms( Args: t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). - q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (x, y, z, w). Shape is (N, 4). t12: Position of frame 2 w.r.t. frame 1. Shape is (N, 3). Defaults to None, in which case the position is assumed to be zero. - q12: Quaternion orientation of frame 2 w.r.t. frame 1 in (w, x, y, z). Shape is (N, 4). + q12: Quaternion orientation of frame 2 w.r.t. frame 1 in (x, y, z, w). Shape is (N, 4). Defaults to None, in which case the orientation is assumed to be identity. Returns: @@ -1028,7 +1030,7 @@ def rigid_body_twist_transform( v0: Linear velocity of 0 in frame 0. Shape is (N, 3). w0: Angular velocity of 0 in frame 0. Shape is (N, 3). t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). - q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (x, y, z, w). Shape is (N, 4). Returns: A tuple containing: @@ -1054,10 +1056,10 @@ def subtract_frame_transforms( Args: t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). - q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (x, y, z, w). Shape is (N, 4). t02: Position of frame 2 w.r.t. frame 0. Shape is (N, 3). Defaults to None, in which case the position is assumed to be zero. - q02: Quaternion orientation of frame 2 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + q02: Quaternion orientation of frame 2 w.r.t. frame 0 in (x, y, z, w). Shape is (N, 4). Defaults to None, in which case the orientation is assumed to be identity. Returns: @@ -1090,9 +1092,9 @@ def compute_pose_error( Args: t01: Position of source frame. Shape is (N, 3). - q01: Quaternion orientation of source frame in (w, x, y, z). Shape is (N, 4). + q01: Quaternion orientation of source frame in (x, y, z, w). Shape is (N, 4). t02: Position of target frame. Shape is (N, 3). - q02: Quaternion orientation of target frame in (w, x, y, z). Shape is (N, 4). + q02: Quaternion orientation of target frame in (x, y, z, w). Shape is (N, 4). rot_error_type: The rotation error type to return: "quat", "axis_angle". Defaults to "axis_angle". @@ -1111,7 +1113,7 @@ def compute_pose_error( # Compute quaternion error (i.e., difference quaternion) # Reference: https://personal.utdallas.edu/~sxb027100/dock/quaternion.html # q_current_norm = q_current * q_current_conj - source_quat_norm = quat_mul(q01, quat_conjugate(q01))[:, 0] + source_quat_norm = quat_mul(q01, quat_conjugate(q01))[:, 3] # q_current_inv = q_current_conj / q_current_norm source_quat_inv = quat_conjugate(q01) / source_quat_norm.unsqueeze(-1) # q_error = q_target * q_current_inv @@ -1148,7 +1150,7 @@ def apply_delta_pose( Args: source_pos: Position of source frame. Shape is (N, 3). - source_rot: Quaternion orientation of source frame in (w, x, y, z). Shape is (N, 4).. + source_rot: Quaternion orientation of source frame in (x, y, z, w). Shape is (N, 4).. delta_pose: Position and orientation displacements. Shape is (N, 6). eps: The tolerance to consider orientation displacement as zero. Defaults to 1.0e-6. @@ -1167,7 +1169,7 @@ def apply_delta_pose( angle = torch.linalg.vector_norm(rot_actions, dim=1) axis = rot_actions / angle.unsqueeze(-1) # change from axis-angle to quat convention - identity_quat = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device).repeat( + identity_quat = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device).repeat( num_poses, 1 ) rot_delta_quat = torch.where( @@ -1205,7 +1207,7 @@ def transform_points( points: Points to transform. Shape is (N, P, 3) or (P, 3). pos: Position of the target frame. Shape is (N, 3) or (3,). Defaults to None, in which case the position is assumed to be zero. - quat: Quaternion orientation of the target frame in (w, x, y, z). Shape is (N, 4) or (4,). + quat: Quaternion orientation of the target frame in (x, y, z, w). Shape is (N, 4) or (4,). Defaults to None, in which case the orientation is assumed to be identity. Returns: @@ -1567,10 +1569,10 @@ def default_orientation(num: int, device: str) -> torch.Tensor: device: Device to create tensor on. Returns: - Identity quaternion in (w, x, y, z). Shape is (num, 4). + Identity quaternion in (x, y, z, w). Shape is (num, 4). """ quat = torch.zeros((num, 4), dtype=torch.float32, device=device) - quat[..., 0] = 1.0 + quat[..., 3] = 1.0 return quat @@ -1584,7 +1586,7 @@ def random_orientation(num: int, device: str) -> torch.Tensor: device: Device to create tensor on. Returns: - Sampled quaternion in (w, x, y, z). Shape is (num, 4). + Sampled quaternion in (x, y, z, w). Shape is (num, 4). Reference: https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.random.html @@ -1604,7 +1606,7 @@ def random_yaw_orientation(num: int, device: str) -> torch.Tensor: device: Device to create tensor on. Returns: - Sampled quaternion in (w, x, y, z). Shape is (num, 4). + Sampled quaternion in (x, y, z, w). Shape is (num, 4). """ roll = torch.zeros(num, dtype=torch.float32, device=device) pitch = torch.zeros(num, dtype=torch.float32, device=device) @@ -1802,12 +1804,12 @@ def convert_camera_frame_orientation_convention( - :obj:`"world"` - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention Args: - orientation: Quaternion of form `(w, x, y, z)` with shape (..., 4) in source convention. + orientation: Quaternion of form `(x, y, z, w)` with shape (..., 4) in source convention. origin: Convention to convert from. Defaults to "opengl". target: Convention to convert to. Defaults to "ros". Returns: - Quaternion of form `(w, x, y, z)` with shape (..., 4) in target convention + Quaternion of form `(x, y, z, w)` with shape (..., 4) in target convention """ if target == origin: return orientation.clone() @@ -2013,12 +2015,12 @@ def quat_slerp(q1: torch.Tensor, q2: torch.Tensor, tau: float) -> torch.Tensor: This function does not support batch processing. Args: - q1: First quaternion in (w, x, y, z) format. - q2: Second quaternion in (w, x, y, z) format. + q1: First quaternion in (x, y, z, w) format. + q2: Second quaternion in (x, y, z, w) format. tau: Interpolation coefficient between 0 (q1) and 1 (q2). Returns: - Interpolated quaternion in (w, x, y, z) format. + Interpolated quaternion in (x, y, z, w) format. """ assert isinstance(q1, torch.Tensor), "Input must be a torch tensor" assert isinstance(q2, torch.Tensor), "Input must be a torch tensor" diff --git a/embodichain/utils/nms.py b/embodichain/utils/nms.py index ca1047405..5ece184bb 100644 --- a/embodichain/utils/nms.py +++ b/embodichain/utils/nms.py @@ -139,8 +139,7 @@ def _poses_to_components(poses: torch.Tensor) -> tuple[torch.Tensor, torch.Tenso # dtype and autograd relationship. poses_f32 = poses.detach().to(dtype=torch.float32).contiguous() positions = poses_f32[:, :3, 3].contiguous() - quaternions_wxyz = quat_from_matrix(poses_f32[:, :3, :3]) - quaternions = torch.cat([quaternions_wxyz[:, 1:], quaternions_wxyz[:, :1]], dim=-1) + quaternions = quat_from_matrix(poses_f32[:, :3, :3]) quaternions = quaternions / torch.linalg.vector_norm( quaternions, dim=-1, keepdim=True ).clamp_min(torch.finfo(quaternions.dtype).eps) diff --git a/embodichain_tasks/configs/components/embodiments/cobotmagic.yaml b/embodichain_tasks/configs/components/embodiments/cobotmagic.yaml index f979d3bd3..b39a4f0ba 100644 --- a/embodichain_tasks/configs/components/embodiments/cobotmagic.yaml +++ b/embodichain_tasks/configs/components/embodiments/cobotmagic.yaml @@ -41,7 +41,7 @@ sensor: extrinsics: parent: right_link6 pos: [-0.08, 0.0, 0.04] - quat: [0.15304635, 0.69034543, -0.69034543, -0.15304635] + quat: [0.69034543, -0.69034543, -0.15304635, 0.15304635] - sensor_type: Camera uid: cam_left_wrist width: 640 @@ -51,7 +51,7 @@ sensor: extrinsics: parent: left_link6 pos: [-0.08, 0.0, 0.04] - quat: [0.15304635, 0.69034543, -0.69034543, -0.15304635] + quat: [0.69034543, -0.69034543, -0.15304635, 0.15304635] skill_profile: contract_id: single_arm_parallel_gripper diff --git a/embodichain_tasks/configs/components/embodiments/dual_ur5_dh_pgi_140_80.yaml b/embodichain_tasks/configs/components/embodiments/dual_ur5_dh_pgi_140_80.yaml index 861b277a2..b672a1c9c 100644 --- a/embodichain_tasks/configs/components/embodiments/dual_ur5_dh_pgi_140_80.yaml +++ b/embodichain_tasks/configs/components/embodiments/dual_ur5_dh_pgi_140_80.yaml @@ -32,7 +32,7 @@ simulation: dual_arm: ["left_joint[0-9]", "right_joint[0-9]"] left_hand: [left_gripper_finger1_joint_1] right_hand: [right_gripper_finger1_joint_1] - drive_pros: + joint_drive_props: stiffness: "left_joint[0-9]": 10000.0 "right_joint[0-9]": 10000.0 @@ -59,8 +59,9 @@ simulation: gripper_fingers: link_names_expr: ["(left|right)_gripper_finger[12]_link_1"] attrs: - dynamic_friction: 2.0 - static_friction: 2.0 + material_props: + dynamic_friction: 2.0 + static_friction: 2.0 solver_cfg: left_arm: class_type: URSolver diff --git a/embodichain_tasks/configs/components/embodiments/ur5_dh_pgi_140_80.yaml b/embodichain_tasks/configs/components/embodiments/ur5_dh_pgi_140_80.yaml index 83c1ad34f..645736142 100644 --- a/embodichain_tasks/configs/components/embodiments/ur5_dh_pgi_140_80.yaml +++ b/embodichain_tasks/configs/components/embodiments/ur5_dh_pgi_140_80.yaml @@ -11,7 +11,7 @@ simulation: control_parts: hand: - gripper_finger1_joint_1 - drive_pros: + joint_drive_props: stiffness: gripper_finger1_joint_1: 1000.0 damping: diff --git a/embodichain_tasks/configs/tasks/classic_control/cart_pole/env.json b/embodichain_tasks/configs/tasks/classic_control/cart_pole/env.json index 085d94a3d..25d5821e1 100644 --- a/embodichain_tasks/configs/tasks/classic_control/cart_pole/env.json +++ b/embodichain_tasks/configs/tasks/classic_control/cart_pole/env.json @@ -1,5 +1,6 @@ { "id": "CartPoleRL", + "physics": "default", "max_episodes": 5, "max_episode_steps": 500, "env": { @@ -46,7 +47,7 @@ "init_pos": [0.0, 0.0, 0.5], "init_rot": [0.0, 0.0, 0.0], "init_qpos": [-0.2, 0.07], - "drive_pros": { + "joint_drive_props": { "stiffness": { "slider_to_cart": 1e1, "cart_to_pole":1e-2 diff --git a/embodichain_tasks/configs/tasks/classic_control/cart_pole/env.yaml b/embodichain_tasks/configs/tasks/classic_control/cart_pole/env.yaml index e5d50843b..4b3dc6cd7 100644 --- a/embodichain_tasks/configs/tasks/classic_control/cart_pole/env.yaml +++ b/embodichain_tasks/configs/tasks/classic_control/cart_pole/env.yaml @@ -1,4 +1,5 @@ id: CartPoleRL +physics: default max_episodes: 5 max_episode_steps: 500 env: @@ -43,7 +44,7 @@ robot: init_qpos: - -0.2 - 0.07 - drive_pros: + joint_drive_props: stiffness: slider_to_cart: 10.0 cart_to_pole: 0.01 diff --git a/embodichain_tasks/configs/tasks/manipulation/hand_over/env.yaml b/embodichain_tasks/configs/tasks/manipulation/hand_over/env.yaml index 0f65df344..6d9d1b7d5 100644 --- a/embodichain_tasks/configs/tasks/manipulation/hand_over/env.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/hand_over/env.yaml @@ -1,4 +1,5 @@ environment_id: hand_over +physics: default max_episodes: 1 max_episode_steps: 1200 num_envs: 1 @@ -20,10 +21,12 @@ simulation: shape_type: Cube size: [0.8, 1.2, 0.02] attrs: - mass: 10.0 - dynamic_friction: 0.9 - static_friction: 0.95 - restitution: 0.01 + mass_props: + mass: 10.0 + material_props: + dynamic_friction: 0.9 + static_friction: 0.95 + restitution: 0.01 body_type: static init_pos: [0.0, 0.0, 0.49] init_rot: [0.0, 0.0, 0.0] @@ -33,19 +36,25 @@ simulation: shape_type: Mesh fpath: SodaCan/simple_cola_can.obj compute_uv: false + collision: + approximation: convex_decomposition + max_hulls: 16 attrs: - mass: 0.33 - dynamic_friction: 0.97 - static_friction: 0.99 - angular_damping: 1.0 - linear_damping: 0.5 - contact_offset: 0.001 - rest_offset: 0.0 - restitution: 0.01 - min_position_iters: 32 - min_velocity_iters: 8 - max_depenetration_velocity: 2.0 - max_convex_hull_num: 16 + mass_props: + mass: 0.33 + rigid_props: + angular_damping: 1.0 + linear_damping: 0.5 + min_position_iters: 32 + min_velocity_iters: 8 + max_depenetration_velocity: 2.0 + collision_props: + contact_offset: 0.001 + rest_offset: 0.0 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + restitution: 0.01 init_pos: [0.0, 0.02, 0.62] init_rot: [90.0, 0.0, 0.0] body_scale: [0.56, 0.56, 0.56] diff --git a/embodichain_tasks/configs/tasks/manipulation/hand_over/task_program/integration.yaml b/embodichain_tasks/configs/tasks/manipulation/hand_over/task_program/integration.yaml index e3c7a020b..29ae5f3c5 100644 --- a/embodichain_tasks/configs/tasks/manipulation/hand_over/task_program/integration.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/hand_over/task_program/integration.yaml @@ -45,4 +45,4 @@ runtime_services: handover_pose_providers: - kind: configured_pose final_position: [0.0, -0.2, 0.6] - final_quaternion_wxyz: [0.7071067812, 0.7071067812, 0.0, 0.0] + final_quaternion_xyzw: [0.7071067812, 0.0, 0.0, 0.7071067812] diff --git a/embodichain_tasks/configs/tasks/manipulation/hand_over/task_program/program.yaml b/embodichain_tasks/configs/tasks/manipulation/hand_over/task_program/program.yaml index b5313ffc2..7be55beb5 100644 --- a/embodichain_tasks/configs/tasks/manipulation/hand_over/task_program/program.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/hand_over/task_program/program.yaml @@ -4,7 +4,7 @@ targets: kind: cyclic_pose values: - position: [0.0, -0.2, 0.6] - quaternion_wxyz: [0.7071067812, 0.7071067812, 0.0, 0.0] + quaternion_xyzw: [0.7071067812, 0.0, 0.0, 0.7071067812] program: kind: segment diff --git a/embodichain_tasks/configs/tasks/manipulation/open_drawer/env.yaml b/embodichain_tasks/configs/tasks/manipulation/open_drawer/env.yaml index 46820e459..563441b6d 100644 --- a/embodichain_tasks/configs/tasks/manipulation/open_drawer/env.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/open_drawer/env.yaml @@ -1,4 +1,5 @@ environment_id: open_drawer +physics: default max_episodes: 1 max_episode_steps: 600 num_envs: 1 @@ -20,12 +21,15 @@ simulation: init_pos: [-1.1, 0.0, 0.0] init_rot: [0.0, 0.0, 90.0] init_qpos: [0.0] - fix_base: true - drive_pros: + root_props: + fixed_base: true + asset_physics_mode: overlay + joint_drive_props: drive_type: none attrs: - static_friction: 1.0 - dynamic_friction: 1.0 + material_props: + static_friction: 1.0 + dynamic_friction: 1.0 env: sim_steps_per_control: 4 diff --git a/embodichain_tasks/configs/tasks/manipulation/push_cube/env.json b/embodichain_tasks/configs/tasks/manipulation/push_cube/env.json index 1399faf0a..c37a6711e 100644 --- a/embodichain_tasks/configs/tasks/manipulation/push_cube/env.json +++ b/embodichain_tasks/configs/tasks/manipulation/push_cube/env.json @@ -1,5 +1,6 @@ { "id": "PushCubeRL", + "physics": "default", "max_episodes": 5, "max_episode_steps": 100, "env": { @@ -135,7 +136,7 @@ "init_pos": [0.0, 0.0, 0.0], "init_rot": [0.0, 0.0, 0.0], "init_qpos": [0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.04, 0.04], - "drive_pros": { + "joint_drive_props": { "drive_type": "force", "stiffness": 100000.0, "damping": 1000.0, @@ -166,17 +167,25 @@ "body_type": "dynamic", "init_pos": [-0.6, -0.4, 0.05], "attrs": { - "mass": 2.0, - "static_friction": 1.0, - "dynamic_friction": 0.8, - "linear_damping": 2.0, - "angular_damping": 2.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "restitution": 0.1, - "max_depenetration_velocity": 10.0, - "max_linear_velocity": 1.0, - "max_angular_velocity": 1.0 + "mass_props": { + "mass": 2.0 + }, + "rigid_props": { + "linear_damping": 2.0, + "angular_damping": 2.0, + "max_depenetration_velocity": 10.0, + "max_linear_velocity": 1.0, + "max_angular_velocity": 1.0 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 1.0, + "dynamic_friction": 0.8, + "restitution": 0.1 + } } } ], diff --git a/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/env.yaml b/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/env.yaml index 1862ae91e..d245a4403 100644 --- a/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/env.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/env.yaml @@ -1,4 +1,5 @@ environment_id: repeated_pick_place +physics: default max_episodes: 1 max_episode_steps: 1200 num_envs: 1 @@ -18,14 +19,16 @@ simulation: shape_type: Cube size: [0.05, 0.05, 0.05] body_type: dynamic - max_convex_hull_num: 16 init_pos: [-0.42, -0.08, 0.025] attrs: - mass: 0.05 - dynamic_friction: 0.97 - static_friction: 0.99 - linear_damping: 0.2 - angular_damping: 0.2 + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 rigid_object_group: [] articulation: [] diff --git a/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task_program/program.yaml b/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task_program/program.yaml index dbef58552..680e48fba 100644 --- a/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task_program/program.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task_program/program.yaml @@ -4,9 +4,9 @@ targets: kind: cyclic_pose values: - position: [-0.40, 0.48, 0.10] - quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + quaternion_xyzw: [0.0, 0.0, 0.0, 1.0] - position: [-0.42, -0.08, 0.10] - quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + quaternion_xyzw: [0.0, 0.0, 0.0, 1.0] program: kind: repeat count: 3 diff --git a/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/env.json b/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/env.json index 55bdc919e..9a5b33980 100644 --- a/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/env.json +++ b/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/env.json @@ -1,5 +1,6 @@ { "id": "BlocksRankingRGB-v1", + "physics": "default", "max_episodes": 5, "max_episode_steps": 600, "env": { @@ -143,10 +144,14 @@ "fpath": "CircleTableSimple/circle_table_simple.ply" }, "attrs" : { - "mass": 10.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.01 + "mass_props": { + "mass": 10.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01 + } }, "body_scale": [1, 1, 1], "body_type": "kinematic", @@ -162,19 +167,26 @@ "size": [0.05, 0.05, 0.05] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.725, -0.015, 0.86], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] }, { "uid":"block_2", @@ -183,19 +195,26 @@ "size": [0.05, 0.05, 0.05] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.725, -0.015, 0.86], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] }, { "uid":"block_3", @@ -204,19 +223,26 @@ "size": [0.05, 0.05, 0.05] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.725, -0.015, 0.86], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] } ] } diff --git a/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_size/env.json b/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_size/env.json index a0117c133..c1c8d8535 100644 --- a/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_size/env.json +++ b/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_size/env.json @@ -1,5 +1,6 @@ { "id": "BlocksRankingSize-v1", + "physics": "default", "max_episodes": 5, "env": { "events": { @@ -130,10 +131,14 @@ "fpath": "CircleTableSimple/circle_table_simple.ply" }, "attrs" : { - "mass": 10.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.01 + "mass_props": { + "mass": 10.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01 + } }, "body_scale": [1, 1, 1], "body_type": "kinematic", @@ -149,19 +154,26 @@ "size": [0.063, 0.063, 0.063] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.725, -0.015, 0.86], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] }, { "uid":"block_2", @@ -170,19 +182,26 @@ "size": [0.051, 0.051, 0.051] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.725, -0.015, 0.86], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] }, { "uid":"block_3", @@ -191,19 +210,26 @@ "size": [0.039, 0.039, 0.039] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.725, -0.015, 0.86], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] } ] } diff --git a/embodichain_tasks/configs/tasks/manipulation/tableware/match_object_container/env.json b/embodichain_tasks/configs/tasks/manipulation/tableware/match_object_container/env.json index 79a3df4a1..e3bd5172d 100644 --- a/embodichain_tasks/configs/tasks/manipulation/tableware/match_object_container/env.json +++ b/embodichain_tasks/configs/tasks/manipulation/tableware/match_object_container/env.json @@ -1,5 +1,6 @@ { "id": "MatchObjectContainer-v1", + "physics": "default", "max_episodes": 5, "env": { "events": { @@ -137,10 +138,14 @@ "fpath": "CircleTableSimple/circle_table_simple.ply" }, "attrs" : { - "mass": 10.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.01 + "mass_props": { + "mass": 10.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01 + } }, "body_scale": [1, 1, 1], "body_type": "kinematic", @@ -156,20 +161,27 @@ "size": [0.04, 0.04, 0.04] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.565, -0.075, 0.86], "init_rot": [0, 0, 0], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] }, { "uid":"block_sphere_1", @@ -178,74 +190,103 @@ "radius": 0.025 }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.635, -0.075, 0.86], "init_rot": [0, 0, 0], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] }, { "uid":"container_cube", "shape": { "shape_type": "Mesh", - "fpath": "ContainerMetal/container_metal.obj" + "fpath": "ContainerMetal/container_metal.obj", + "collision": { + "approximation": "convex_decomposition", + "max_hulls": 8 + } }, "body_type": "dynamic", "attrs" : { - "mass": 0.5, - "static_friction": 1.0, - "dynamic_friction": 1.0, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 2.0, - "linear_damping": 2.0, - "angular_damping": 2.0, - "max_linear_velocity": 5.0, - "max_angular_velocity": 10.0, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.5 + }, + "rigid_props": { + "max_depenetration_velocity": 2.0, + "linear_damping": 2.0, + "angular_damping": 2.0, + "max_linear_velocity": 5.0, + "max_angular_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 1.0, + "dynamic_friction": 1.0, + "restitution": 0.0 + } }, "init_pos": [0.875, -0.25, 0.86], "init_rot": [0, 0, 0], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 8 + "body_scale":[1, 1, 1] }, { "uid":"container_sphere", "shape": { "shape_type": "Mesh", - "fpath": "ContainerMetal/container_metal.obj" + "fpath": "ContainerMetal/container_metal.obj", + "collision": { + "approximation": "convex_decomposition", + "max_hulls": 8 + } }, "body_type": "dynamic", "attrs" : { - "mass": 0.5, - "static_friction": 1.0, - "dynamic_friction": 1.0, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 2.0, - "linear_damping": 2.0, - "angular_damping": 2.0, - "max_linear_velocity": 5.0, - "max_angular_velocity": 10.0, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.5 + }, + "rigid_props": { + "max_depenetration_velocity": 2.0, + "linear_damping": 2.0, + "angular_damping": 2.0, + "max_linear_velocity": 5.0, + "max_angular_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 1.0, + "dynamic_friction": 1.0, + "restitution": 0.0 + } }, "init_pos": [0.875, 0.25, 0.86], "init_rot": [0, 0, 0], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 8 + "body_scale":[1, 1, 1] }, { "uid":"block_cube_2", @@ -254,20 +295,27 @@ "size": [0.04, 0.04, 0.04] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.565, 0.075, 0.86], "init_rot": [0, 0, 0], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] }, { "uid":"block_sphere_2", @@ -276,20 +324,27 @@ "radius": 0.025 }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.635, 0.075, 0.86], "init_rot": [0, 0, 0], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] } ] } diff --git a/embodichain_tasks/configs/tasks/manipulation/tableware/place_object_drawer/env.json b/embodichain_tasks/configs/tasks/manipulation/tableware/place_object_drawer/env.json index 1aca032df..9d5118aab 100644 --- a/embodichain_tasks/configs/tasks/manipulation/tableware/place_object_drawer/env.json +++ b/embodichain_tasks/configs/tasks/manipulation/tableware/place_object_drawer/env.json @@ -1,5 +1,6 @@ { "id": "PlaceObjectDrawer-v1", + "physics": "default", "max_episodes": 5, "env": { "events": { @@ -67,10 +68,14 @@ "fpath": "CircleTableSimple/circle_table_simple.ply" }, "attrs" : { - "mass": 10.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.01 + "mass_props": { + "mass": 10.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01 + } }, "body_scale": [1, 1, 1], "body_type": "kinematic", @@ -83,27 +88,38 @@ "uid":"object", "shape": { "shape_type": "Mesh", - "fpath": "ToyDuck/toy_duck.glb" + "fpath": "ToyDuck/toy_duck.glb", + "collision": { + "approximation": "convex_decomposition", + "max_hulls": 8 + } }, "attrs" : { - "mass": 0.01, - "static_friction": 1.0, - "dynamic_friction": 1.0, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 2.0, - "linear_damping": 2.0, - "angular_damping": 2.0, - "max_linear_velocity": 5.0, - "max_angular_velocity": 10.0, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.01 + }, + "rigid_props": { + "max_depenetration_velocity": 2.0, + "linear_damping": 2.0, + "angular_damping": 2.0, + "max_linear_velocity": 5.0, + "max_angular_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 1.0, + "dynamic_friction": 1.0, + "restitution": 0.0 + } }, "init_pos": [0.725, -0.1, 0.86], "init_rot": [0, 0, 0], - "body_scale":[0.2, 0.2, 0.2], - "max_convex_hull_num": 8 + "body_scale":[0.2, 0.2, 0.2] } ], "articulation": [ diff --git a/embodichain_tasks/configs/tasks/manipulation/tableware/pour_water/env.yaml b/embodichain_tasks/configs/tasks/manipulation/tableware/pour_water/env.yaml index 75c9159e9..9fa3ea405 100644 --- a/embodichain_tasks/configs/tasks/manipulation/tableware/pour_water/env.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/tableware/pour_water/env.yaml @@ -1,4 +1,5 @@ environment_id: pour_water +physics: default max_episodes: 5 max_episode_steps: 600 num_envs: 1 @@ -20,10 +21,12 @@ simulation: fpath: CircleTableSimple/circle_table_simple.ply compute_uv: true attrs: - mass: 10.0 - static_friction: 0.95 - dynamic_friction: 0.9 - restitution: 0.01 + mass_props: + mass: 10.0 + material_props: + static_friction: 0.95 + dynamic_friction: 0.9 + restitution: 0.01 body_scale: [1, 1, 1] body_type: kinematic init_pos: [0.725, 0.0, 0.825] @@ -34,33 +37,45 @@ simulation: shape_type: Mesh fpath: PaperCup/paper_cup.ply compute_uv: true + collision: + approximation: convex_decomposition + max_hulls: 8 attrs: - mass: 0.01 - contact_offset: 0.003 - rest_offset: 0.001 - restitution: 0.01 - max_depenetration_velocity: 10.0 - min_position_iters: 32 - min_velocity_iters: 8 + mass_props: + mass: 0.01 + rigid_props: + max_depenetration_velocity: 10.0 + min_position_iters: 32 + min_velocity_iters: 8 + collision_props: + contact_offset: 0.003 + rest_offset: 0.001 + material_props: + restitution: 0.01 init_pos: [0.75, 0.1, 0.9] body_scale: [0.75, 0.75, 1.0] - max_convex_hull_num: 8 - uid: bottle shape: shape_type: Mesh fpath: ScannedBottle/kashijia_processed.ply compute_uv: true + collision: + approximation: convex_decomposition + max_hulls: 8 attrs: - mass: 0.01 - contact_offset: 0.003 - rest_offset: 0.001 - restitution: 0.01 - max_depenetration_velocity: 10.0 - min_position_iters: 32 - min_velocity_iters: 8 + mass_props: + mass: 0.01 + rigid_props: + max_depenetration_velocity: 10.0 + min_position_iters: 32 + min_velocity_iters: 8 + collision_props: + contact_offset: 0.003 + rest_offset: 0.001 + material_props: + restitution: 0.01 init_pos: [0.75, -0.1, 0.932] body_scale: [1, 1, 1] - max_convex_hull_num: 8 rigid_object_group: [] articulation: [] @@ -80,25 +95,30 @@ env: - [0.6, 0.6, 0.6] - [1, 1, 1] intensity_range: [50.0, 100.0] - random_table_material: - func: randomize_visual_material - mode: reset - params: - entity_cfg: - uid: table - texture_path: BackgroundTexture/100 - p_original: 0.0 - p_library: 1.0 - p_solid: 0.0 - random_bottle_material: - func: randomize_visual_material - mode: reset - params: - entity_cfg: - uid: bottle - p_original: 0.0 - p_library: 0.0 - p_solid: 1.0 + random_table_material: + func: randomize_visual_material + mode: reset + params: + entity_cfg: + uid: table + texture_path: BackgroundTexture/100 + p_original: 0.0 + p_library: 1.0 + p_solid: 0.0 + random_bottle_material: + func: randomize_visual_material + mode: reset + params: + entity_cfg: + uid: bottle + p_original: 0.0 + p_library: 0.0 + p_solid: 1.0 + base_color_range: + - [0.2, 0.2, 0.2] + - [1.0, 1.0, 1.0] + metallic_range: [0.0, 0.2] + roughness_range: [0.35, 0.85] settle_pour_objects_on_reset: func: wait_for_dynamic_objects_to_settle mode: reset diff --git a/embodichain_tasks/configs/tasks/manipulation/tableware/pour_water/task_program/program.yaml b/embodichain_tasks/configs/tasks/manipulation/tableware/pour_water/task_program/program.yaml index eb46d5f40..6603f0175 100644 --- a/embodichain_tasks/configs/tasks/manipulation/tableware/pour_water/task_program/program.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/tableware/pour_water/task_program/program.yaml @@ -4,7 +4,7 @@ targets: kind: cyclic_pose values: - position: [0.75, -0.1, 0.962] - quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + quaternion_xyzw: [0.0, 0.0, 0.0, 1.0] program: kind: segment name: pour_and_return_bottle diff --git a/embodichain_tasks/configs/tasks/manipulation/tableware/scoop_ice/env.json b/embodichain_tasks/configs/tasks/manipulation/tableware/scoop_ice/env.json index 675566b67..f656a6952 100644 --- a/embodichain_tasks/configs/tasks/manipulation/tableware/scoop_ice/env.json +++ b/embodichain_tasks/configs/tasks/manipulation/tableware/scoop_ice/env.json @@ -1,5 +1,6 @@ { "id": "ScoopIce-v1", + "physics": "default", "max_episodes": 5, "max_episode_steps": 600, "num_envs": 1, @@ -114,7 +115,7 @@ "extrinsics": { "parent": "right_ee", "pos": [0.09, 0.05, 0.04], - "quat": [0.36497168, -0.11507513, 0.88111957, 0.27781593] + "quat": [-0.11507513, 0.88111957, 0.27781593, 0.36497168] } }, { @@ -127,7 +128,7 @@ "extrinsics": { "parent": "left_ee", "pos": [0.09, -0.05, 0.04], - "quat": [0.27781593, 0.88111957, -0.11507513, 0.36497168] + "quat": [0.88111957, -0.11507513, 0.36497168, 0.27781593] } } ], @@ -141,10 +142,14 @@ "fpath": "CircleTableSimple/circle_table_simple.ply" }, "attrs" : { - "mass": 1.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.05 + "mass_props": { + "mass": 1.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.05 + } }, "body_type": "kinematic", "init_pos": [0.80, 0, 0.54], @@ -156,34 +161,52 @@ "uid": "scoop", "shape": { "shape_type": "Mesh", - "fpath": "ScoopIceNewEnv/scoop.ply" + "fpath": "ScoopIceNewEnv/scoop.ply", + "collision": { + "approximation": "convex_decomposition", + "max_hulls": 8 + } }, "attrs" : { - "mass": 0.5, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.0, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.5 + }, + "rigid_props": { + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.0 + } }, - "max_convex_hull_num": 8, "init_pos": [0, 10, 10] }, { "uid": "paper_cup", "shape": { "shape_type": "Mesh", - "fpath": "PaperCup/paper_cup.ply" + "fpath": "PaperCup/paper_cup.ply", + "collision": { + "approximation": "convex_decomposition", + "max_hulls": 16 + } }, "attrs" : { - "mass": 0.5, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.0, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.5 + }, + "rigid_props": { + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.0 + } }, - "max_convex_hull_num": 16, "init_pos": [0, 10, 10] } ], @@ -196,15 +219,23 @@ "rigid_objects": { "obj": { "attrs" : { - "mass": 0.004, - "contact_offset": 0.001, - "rest_offset": 0, - "dynamic_friction": 0.05, - "static_friction": 0.1, - "restitution": 0.00, - "min_position_iters": 32, - "min_velocity_iters": 8, - "max_depenetration_velocity": 1.0 + "mass_props": { + "mass": 0.004 + }, + "rigid_props": { + "min_position_iters": 32, + "min_velocity_iters": 8, + "max_depenetration_velocity": 1.0 + }, + "collision_props": { + "contact_offset": 0.001, + "rest_offset": 0 + }, + "material_props": { + "dynamic_friction": 0.05, + "static_friction": 0.1, + "restitution": 0.0 + } }, "shape": { "shape_type": "Mesh" @@ -222,12 +253,18 @@ "init_pos": [0.635, -0.04, 0.94], "init_rot": [0, 0, -80], "attrs": { - "mass": 1.0, - "dynamic_friction": 0.05, - "static_friction": 0.1, - "max_depenetration_velocity": 1.0 + "mass_props": { + "mass": 1.0 + }, + "rigid_props": { + "max_depenetration_velocity": 1.0 + }, + "material_props": { + "dynamic_friction": 0.05, + "static_friction": 0.1 + } }, - "drive_pros": { + "joint_drive_props": { "stiffness": 1.0, "damping": 0.1, "max_effort": 100.0 diff --git a/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/env.json b/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/env.json index 8c8f54125..f306b2d00 100644 --- a/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/env.json +++ b/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/env.json @@ -1,5 +1,6 @@ { "id": "StackBlocksTwo-v1", + "physics": "default", "max_episodes": 5, "max_episode_steps": 600, "env": { @@ -86,10 +87,14 @@ "fpath": "CircleTableSimple/circle_table_simple.ply" }, "attrs" : { - "mass": 10.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.01 + "mass_props": { + "mass": 10.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01 + } }, "body_scale": [1, 1, 1], "body_type": "kinematic", @@ -105,19 +110,26 @@ "size": [0.05, 0.05, 0.05] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.75, -0.1, 0.9], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] }, { "uid":"block_2", @@ -126,19 +138,26 @@ "size": [0.05, 0.05, 0.05] }, "attrs" : { - "mass": 0.05, - "static_friction": 0.5, - "dynamic_friction": 0.5, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 1e1, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.05 + }, + "rigid_props": { + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 0.5, + "dynamic_friction": 0.5, + "restitution": 0.0 + } }, "init_pos": [0.75, 0.1, 0.9], - "body_scale":[1, 1, 1], - "max_convex_hull_num": 1 + "body_scale":[1, 1, 1] } ] } diff --git a/embodichain_tasks/configs/tasks/manipulation/tableware/stack_cups/env.json b/embodichain_tasks/configs/tasks/manipulation/tableware/stack_cups/env.json index f50728fe4..daa18d53c 100644 --- a/embodichain_tasks/configs/tasks/manipulation/tableware/stack_cups/env.json +++ b/embodichain_tasks/configs/tasks/manipulation/tableware/stack_cups/env.json @@ -1,5 +1,6 @@ { "id": "StackCups-v1", + "physics": "default", "max_episodes": 5, "env": { "events": { @@ -85,10 +86,14 @@ "fpath": "CircleTableSimple/circle_table_simple.ply" }, "attrs" : { - "mass": 10.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.01 + "mass_props": { + "mass": 10.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01 + } }, "body_scale": [1, 1, 1], "body_type": "kinematic", @@ -101,53 +106,75 @@ "uid":"cup_1", "shape": { "shape_type": "Mesh", - "fpath": "PaperCup/paper_cup.ply" + "fpath": "PaperCup/paper_cup.ply", + "collision": { + "approximation": "convex_decomposition", + "max_hulls": 8 + } }, "attrs" : { - "mass": 0.01, - "static_friction": 1.0, - "dynamic_friction": 1.0, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 2.0, - "linear_damping": 2.0, - "angular_damping": 2.0, - "max_linear_velocity": 5.0, - "max_angular_velocity": 10.0, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.01 + }, + "rigid_props": { + "max_depenetration_velocity": 2.0, + "linear_damping": 2.0, + "angular_damping": 2.0, + "max_linear_velocity": 5.0, + "max_angular_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 1.0, + "dynamic_friction": 1.0, + "restitution": 0.0 + } }, "init_pos": [0.70, -0.1, 0.86], "init_rot": [0, 0, 0], - "body_scale":[0.75, 0.75, 1.0], - "max_convex_hull_num": 8 + "body_scale":[0.75, 0.75, 1.0] }, { "uid":"cup_2", "shape": { "shape_type": "Mesh", - "fpath": "PaperCup/paper_cup.ply" + "fpath": "PaperCup/paper_cup.ply", + "collision": { + "approximation": "convex_decomposition", + "max_hulls": 8 + } }, "attrs" : { - "mass": 0.01, - "static_friction": 1.0, - "dynamic_friction": 1.0, - "restitution": 0.0, - "contact_offset": 0.003, - "rest_offset": 0.001, - "max_depenetration_velocity": 2.0, - "linear_damping": 2.0, - "angular_damping": 2.0, - "max_linear_velocity": 5.0, - "max_angular_velocity": 10.0, - "min_position_iters": 32, - "min_velocity_iters": 8 + "mass_props": { + "mass": 0.01 + }, + "rigid_props": { + "max_depenetration_velocity": 2.0, + "linear_damping": 2.0, + "angular_damping": 2.0, + "max_linear_velocity": 5.0, + "max_angular_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8 + }, + "collision_props": { + "contact_offset": 0.003, + "rest_offset": 0.001 + }, + "material_props": { + "static_friction": 1.0, + "dynamic_friction": 1.0, + "restitution": 0.0 + } }, "init_pos": [0.80, -0.1, 0.86], "init_rot": [0, 0, 0], - "body_scale":[0.75, 0.75, 1.0], - "max_convex_hull_num": 8 + "body_scale":[0.75, 0.75, 1.0] } ] } diff --git a/embodichain_tasks/configs/tasks/special/simple_task/env_ur10.json b/embodichain_tasks/configs/tasks/special/simple_task/env_ur10.json index 41faec8ae..115f8e163 100644 --- a/embodichain_tasks/configs/tasks/special/simple_task/env_ur10.json +++ b/embodichain_tasks/configs/tasks/special/simple_task/env_ur10.json @@ -1,5 +1,6 @@ { "id": "SimpleTask-v1", + "physics": "default", "max_episodes": 24, "env": { "events": { @@ -87,10 +88,14 @@ "compute_uv": true }, "attrs" : { - "mass": 10.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.01 + "mass_props": { + "mass": 10.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01 + } }, "body_scale": [1, 1, 1], "body_type": "kinematic", diff --git a/embodichain_tasks/configs/tasks/special/stay_still_save/env_async_ur10.json b/embodichain_tasks/configs/tasks/special/stay_still_save/env_async_ur10.json index 16329668c..fa70d38d7 100644 --- a/embodichain_tasks/configs/tasks/special/stay_still_save/env_async_ur10.json +++ b/embodichain_tasks/configs/tasks/special/stay_still_save/env_async_ur10.json @@ -1,5 +1,6 @@ { "id": "StayStillSave-v1", + "physics": "default", "max_episodes": 4, "num_envs": 4, "env": { @@ -67,10 +68,14 @@ "compute_uv": true }, "attrs": { - "mass": 10.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.01 + "mass_props": { + "mass": 10.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01 + } }, "body_scale": [1, 1, 1], "body_type": "kinematic", diff --git a/embodichain_tasks/configs/tasks/special/stay_still_save/env_ur10.json b/embodichain_tasks/configs/tasks/special/stay_still_save/env_ur10.json index b6121fc08..3c58745a4 100644 --- a/embodichain_tasks/configs/tasks/special/stay_still_save/env_ur10.json +++ b/embodichain_tasks/configs/tasks/special/stay_still_save/env_ur10.json @@ -1,5 +1,6 @@ { "id": "StayStillSave-v1", + "physics": "default", "max_episodes": 1, "env": { "dataset": { @@ -63,10 +64,14 @@ "compute_uv": true }, "attrs": { - "mass": 10.0, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "restitution": 0.01 + "mass_props": { + "mass": 10.0 + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01 + } }, "body_scale": [1, 1, 1], "body_type": "kinematic", diff --git a/embodichain_tasks/embodichain_tasks/special/franka_reach_apg.py b/embodichain_tasks/embodichain_tasks/special/franka_reach_apg.py new file mode 100644 index 000000000..c3d76ce8b --- /dev/null +++ b/embodichain_tasks/embodichain_tasks/special/franka_reach_apg.py @@ -0,0 +1,513 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Franka FR3 reach task with differentiable Newton kinematics (APG). + +Built on :class:`DifferentiableEnv`. The Warp-tape bridge +produces ``action.grad`` that flows back through a differentiable +forward-kinematics path (``newton.eval_fk``). The configured semi-implicit +solver is not advanced; the task runs FK directly, matching the reference APG +implementation in +``/root/sources/analytic_policy_gradients/envs/franka_reach_env.py``. +""" + +from __future__ import annotations + +from typing import Any, Callable + +import numpy as np +import torch +import warp as wp +import newton +import newton.utils + +from embodichain.lab.gym.envs.differentiable_env import DifferentiableEnv +from embodichain.lab.gym.envs.embodied_env import EmbodiedEnvCfg +from embodichain.lab.gym.utils.registration import register_env +from embodichain.lab.sim.cfg import ( + ArticulationRootPropertiesCfg, + NewtonPhysicsCfg, + RobotCfg, + URDFCfg, +) +from embodichain.lab.sim.sim_manager import SimulationManagerCfg + +__all__ = ["FrankaReachApgEnv"] + +# Franka FR3 arm has 7 actuated arm joints; the URDF also has 2 finger +# joints (9 dof total). We only control the 7 arm joints. +FRANKA_NUM_ARM_JOINTS = 7 +FRANKA_EE_BODY = "fr3_hand_tcp" +DEFAULT_ACTION_SCALE = 0.2 +DEFAULT_MAX_EPISODE_STEPS = 30 +TARGET_POS_RANGE = { + "x": (0.05, 0.70), + "y": (-0.45, 0.45), + "z": (0.20, 0.95), +} + + +@wp.kernel +def _set_joint_targets_kernel( + action: wp.array(dtype=wp.float32), + current_q: wp.array(dtype=wp.float32), + target_q: wp.array(dtype=wp.float32), + limit_lo: wp.array(dtype=wp.float32), + limit_hi: wp.array(dtype=wp.float32), + action_scale: wp.float32, + n_joints_per_env: wp.int32, + n_arm: wp.int32, + total: wp.int32, +): + """Compute new joint q: target = clamp(current + action * scale, lo, hi).""" + tid = wp.tid() + if tid < total: + env_idx = tid / n_arm + j = tid % n_arm + off = env_idx * n_joints_per_env + j + new_q = current_q[off] + action[tid] * action_scale + target_q[off] = wp.clamp(new_q, limit_lo[j], limit_hi[j]) + + +@wp.kernel +def _reach_reward_kernel( + body_q: wp.array(dtype=wp.transformf), + ee_body_indices: wp.array(dtype=wp.int32), + target_pos: wp.array(dtype=wp.vec3f), + reward_out: wp.array(dtype=wp.float32), +): + """Position-only reach reward (smoke task): -0.2*dist + 0.1*exp(-dist^2/0.02).""" + env_idx = wp.tid() + ee_transform = body_q[ee_body_indices[env_idx]] + eef_pos = wp.transform_get_translation(ee_transform) + diff = eef_pos - target_pos[env_idx] + pos_dist = wp.sqrt(wp.dot(diff, diff) + wp.float32(1e-8)) + reward_out[env_idx] = wp.float32(-0.2) * pos_dist + wp.float32(0.1) * wp.exp( + -pos_dist * pos_dist / wp.float32(0.02) + ) + + +@register_env("FrankaReachApg-v0") +class FrankaReachApgEnv(DifferentiableEnv): + """Differentiable Franka FR3 reach task for analytic policy gradients. + + The environment resolves the Franka FR3 URDF via + ``newton.utils.download_asset("franka_emika_panda")`` (network-dependent) + or an explicit ``urdf_path`` kwarg override. The robot is added through + the standard EmbodiChain ``sim.add_robot(cfg.robot)`` flow driven by + :class:`EmbodiedEnv`/``BaseEnv.__init__``. + + The differentiable path is: + + action -> new_joint_q (action kernel) -> eval_fk -> body_q + -> reward kernel -> reward_wp -> tape.backward -> action.grad + + The configured semi-implicit solver is part of Newton scene setup but is + never advanced. This matches the current kinematics-only + :class:`DifferentiableEnv` contract. + """ + + metadata = {"render_modes": ["human"], "default_num_envs": 4} + + def __init__( + self, + cfg: EmbodiedEnvCfg | None = None, + *, + num_envs: int = 4, + urdf_path: str | None = None, + action_scale: float = DEFAULT_ACTION_SCALE, + max_episode_steps: int = DEFAULT_MAX_EPISODE_STEPS, + device: str = "cuda:0", + ) -> None: + self._urdf_path = urdf_path + self._action_scale = float(action_scale) + self._max_episode_steps = int(max_episode_steps) + self._device_str = device + + if cfg is None: + urdf = urdf_path or self._resolve_default_urdf() + robot_cfg = RobotCfg( + uid="franka", + urdf_cfg=URDFCfg().set_urdf(urdf), + root_props=ArticulationRootPropertiesCfg(fixed_base=True), + ) + cfg = EmbodiedEnvCfg( + sim_cfg=SimulationManagerCfg( + physics_cfg=NewtonPhysicsCfg( + device=device, + requires_grad=True, + solver_cfg={"solver_type": "semi_implicit"}, + use_cuda_graph=False, + ), + num_envs=num_envs, + headless=True, + ), + robot=robot_cfg, + num_envs=num_envs, + max_episode_steps=max_episode_steps, + ) + # Bug 1 fix: cfg.robot is set BEFORE super().__init__() so that + # EmbodiedEnv._init_sim_state -> BaseEnv._setup_scene -> + # _setup_robot -> sim.add_robot(cfg.robot) has a valid robot to + # add. BaseEnv.__init__ also calls finalize_newton_physics() once + # the scene is built, so we do NOT re-finalize here. + super().__init__(cfg) + # EmbodiedEnv has added the robot and BaseEnv has finalized the + # Newton model. Cache joint-limit Warp arrays and EE body indices. + self._cache_franka_buffers() + self._init_targets() + + # -- scene setup ----------------------------------------------------- # + + def _resolve_default_urdf(self) -> str: + """Resolve the Franka URDF via Newton's asset cache. + + Raises: + FileNotFoundError: If the URDF cannot be downloaded or + located. + """ + try: + urdf = newton.utils.download_asset("franka_emika_panda") / ( + "urdf/fr3_franka_hand.urdf" + ) + if urdf.exists(): + return str(urdf) + except Exception: + pass + raise FileNotFoundError("Franka URDF not available; pass urdf_path explicitly.") + + def _cache_franka_buffers(self) -> None: + """Cache joint-limit Warp arrays, EE body indices, and FK state.""" + runtime = self.sim.differentiable_runtime + model = runtime.model + # Warp's ``wp.zeros`` / ``wp.launch`` reject ``torch.device`` + # directly (``Invalid device identifier: cuda:0``), so cache the + # Warp-compatible device string up-front. + self._wp_device = model.device + # ``model.joint_limit_lower`` is a ``wp.array``; convert via + # ``.numpy()`` before slicing (``np.asarray`` on a wp.array slice + # raises "Item indexing is not supported on wp.array objects"). + lo = model.joint_limit_lower.numpy()[:FRANKA_NUM_ARM_JOINTS].astype(np.float32) + hi = model.joint_limit_upper.numpy()[:FRANKA_NUM_ARM_JOINTS].astype(np.float32) + self._limit_lo_t = torch.from_numpy(lo).to(self.device) + self._limit_hi_t = torch.from_numpy(hi).to(self.device) + self._limit_lo_wp = wp.array(lo, dtype=wp.float32, device=self._wp_device) + self._limit_hi_wp = wp.array(hi, dtype=wp.float32, device=self._wp_device) + self._n_joints_per_env = int(len(model.joint_q) // self.sim.num_envs) + # Every taped forward replaces these with private primal buffers before + # the bridge opens its tape. They must never alias manager live state. + self._current_joint_q_snapshot: wp.array | None = None + self._fk_state = model.state() + self._new_joint_q: wp.array | None = None + # Per-env global EE body indices into the flat body_q array. + self._ee_global_idx = self._compute_ee_body_indices() + self._ee_idx_wp = wp.array( + np.asarray(self._ee_global_idx, dtype=np.int32), + dtype=wp.int32, + device=self._wp_device, + ) + self._ee_idx_t = torch.tensor( + self._ee_global_idx, dtype=torch.long, device=self.device + ) + + def _compute_ee_body_indices(self) -> list[int]: + """Scan model.body_label for the EE body per env. + + Each cloned arena produces a full set of Franka bodies in the + shared Newton model. We pick the ``FRANKA_EE_BODY`` body for + each env block (one global index per env). + """ + model = self.sim.differentiable_runtime.model + n_envs = self.sim.num_envs + n_per_env = len(model.body_label) // n_envs + idx_per_env: list[int] = [] + for i in range(n_envs): + for j in range(n_per_env): + global_idx = i * n_per_env + j + if FRANKA_EE_BODY in str(model.body_label[global_idx]): + idx_per_env.append(global_idx) + break + if len(idx_per_env) != n_envs: + raise RuntimeError( + f"Expected {n_envs} '{FRANKA_EE_BODY}' bodies, " + f"found {len(idx_per_env)}." + ) + return idx_per_env + + def _init_targets(self) -> None: + n = self.sim.num_envs + device = self.device + self.target_pos = torch.zeros(n, 3, device=device) + self.target_quat = torch.zeros(n, 4, device=device) + self.last_action = torch.zeros(n, FRANKA_NUM_ARM_JOINTS, device=device) + self.step_count = torch.zeros(n, dtype=torch.int32, device=device) + self._sample_new_targets(torch.arange(n, device=device)) + + def _sample_new_targets(self, env_ids: torch.Tensor) -> None: + n = env_ids.numel() + d = self.device + self.target_pos[env_ids, 0] = TARGET_POS_RANGE["x"][0] + torch.rand( + n, device=d + ) * (TARGET_POS_RANGE["x"][1] - TARGET_POS_RANGE["x"][0]) + self.target_pos[env_ids, 1] = TARGET_POS_RANGE["y"][0] + torch.rand( + n, device=d + ) * (TARGET_POS_RANGE["y"][1] - TARGET_POS_RANGE["y"][0]) + self.target_pos[env_ids, 2] = TARGET_POS_RANGE["z"][0] + torch.rand( + n, device=d + ) * (TARGET_POS_RANGE["z"][1] - TARGET_POS_RANGE["z"][0]) + # Identity orientation: the smoke task uses position-only reward. + self.target_quat[env_ids] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=d).expand( + n, -1 + ) + + # -- DifferentiableEnv contract -------------------------------------- # + + def _build_sim_state_dict(self, action: torch.Tensor) -> dict: + """Detach FK primal buffers before the parent opens a Warp tape.""" + runtime = self.sim.differentiable_runtime + self._current_joint_q_snapshot = wp.clone(runtime.current_state.joint_q) + self._fk_state = runtime.model.state() + return super()._build_sim_state_dict(action) + + def _make_kinematic_step_fn(self) -> Callable[[], Any]: + """Explicit FK hook: compute body_q from new_joint_q via ``eval_fk``. + + The environment intentionally bypasses its configured solver and runs + forward kinematics directly inside the tape. ``self._new_joint_q`` is populated by + :meth:`_apply_action_kernel` before this callable runs. + """ + env = self + model = env.sim.differentiable_runtime.model + + def _step(): + newton.eval_fk( + model, + env._new_joint_q, + env._fk_state.joint_qd, + env._fk_state, + ) + return env._fk_state + + return _step + + def _apply_action_kernel(self, action_wp: Any, tape: Any) -> None: + """Launch the action-to-control kernel inside the open tape. + + Writes ``new_joint_q = clamp(current_q + action * scale, lo, hi)`` + into a freshly allocated ``self._new_joint_q`` Warp array. The + explicit kinematic hook then consumes this array via ``newton.eval_fk``. + """ + n = self.sim.num_envs + total = n * FRANKA_NUM_ARM_JOINTS + if self._current_joint_q_snapshot is None: + raise RuntimeError( + "Franka kinematics requires a detached joint_q snapshot " + "before opening its Warp tape." + ) + # Allocate a fresh new_joint_q each call so each forward pass + # has its own grad graph (the tape records the kernel writes). + self._new_joint_q = wp.zeros( + n * self._n_joints_per_env, + dtype=wp.float32, + device=self._wp_device, + requires_grad=True, + ) + wp.launch( + _set_joint_targets_kernel, + dim=total, + inputs=[ + action_wp, + self._current_joint_q_snapshot, + self._new_joint_q, + self._limit_lo_wp, + self._limit_hi_wp, + wp.float32(self._action_scale), + wp.int32(self._n_joints_per_env), + wp.int32(FRANKA_NUM_ARM_JOINTS), + wp.int32(total), + ], + device=self._wp_device, + ) + + def _read_outputs(self, final_state: Any) -> dict: + """Launch the reward kernel and build obs INSIDE the open tape. + + Reward is written into a grad-tracked ``reward_wp`` Warp array, + then exposed as a torch tensor via ``wp.to_torch`` (zero-copy). + The obs is built from ``wp.to_torch(final_state.joint_q)`` and + ``wp.to_torch(final_state.body_q)`` (also tape-tracked). + """ + n = self.sim.num_envs + device = self._wp_device + + # Grad-tracked reward output array. The kernel launches inside + # the open tape so reward_wp carries gradient back through the + # reward kernel -> body_q -> FK -> new_joint_q -> action_wp. + reward_wp = wp.zeros(n, dtype=wp.float32, device=device, requires_grad=True) + target_pos_wp = wp.from_torch( + self.target_pos.detach().clone().contiguous(), dtype=wp.vec3 + ) + wp.launch( + _reach_reward_kernel, + dim=n, + inputs=[final_state.body_q, self._ee_idx_wp, target_pos_wp], + outputs=[reward_wp], + device=device, + ) + + joint_q_t = wp.to_torch(final_state.joint_q).view(n, -1) + body_q_flat = wp.to_torch(final_state.body_q).view(-1, 7) + ee_pose = body_q_flat[self._ee_idx_t] + obs = torch.cat( + [ + joint_q_t[:, :FRANKA_NUM_ARM_JOINTS], + ee_pose, + self.target_pos, + self.target_quat, + self.last_action, + ], + dim=-1, + ) + + reward_t = wp.to_torch(reward_wp) + pos_dist = (ee_pose[:, :3] - self.target_pos).norm(dim=-1).detach() + terminated = pos_dist < 0.01 + truncated = self.step_count >= self._max_episode_steps + + return { + "_order": ("obs", "reward", "terminated", "truncated"), + "_grad_track": { + "obs": None, + "reward": reward_wp, + "terminated": None, + "truncated": None, + }, + "obs": obs, + "reward": reward_t, + "terminated": terminated, + "truncated": truncated, + } + + # -- gym overrides --------------------------------------------------- # + + def step(self, action: torch.Tensor): + """Step the env, then advance the cached joint_q for the next call. + + The parent :meth:`DifferentiableEnv.step` runs the + differentiable bridge. After it returns, we update + both Spawn live states for non-terminal envs so the next step starts + from the new configuration. The tape reads a per-forward detached + snapshot, so this continuation cannot overwrite its primal input. + """ + if not isinstance(action, torch.Tensor): + action = torch.as_tensor(action, dtype=torch.float32) + clamped_action = torch.clamp(action.to(self.device), -1.0, 1.0) + # Advance step_count BEFORE the bridge runs so _read_outputs + # computes truncated against the post-step value. + self.step_count += 1 + result = super().step(clamped_action) + obs, reward, terminated, truncated, info = result + done_mask = terminated | truncated + live = (~done_mask).nonzero(as_tuple=False).squeeze(-1) + if live.numel() > 0: + with torch.no_grad(): + runtime = self.sim.differentiable_runtime + current_q = wp.to_torch(runtime.current_state.joint_q).view( + self.sim.num_envs, -1 + ) + cur = current_q[live, :FRANKA_NUM_ARM_JOINTS] + delta = clamped_action[live].detach() * self._action_scale + lo = self._limit_lo_t.unsqueeze(0).expand_as(cur) + hi = self._limit_hi_t.unsqueeze(0).expand_as(cur) + next_q = torch.clamp(cur + delta, lo, hi) + for state in runtime.live_states: + joint_q = wp.to_torch(state.joint_q).view(self.sim.num_envs, -1) + joint_q[live, :FRANKA_NUM_ARM_JOINTS] = next_q + newton.eval_fk( + runtime.model, + state.joint_q, + state.joint_qd, + state, + ) + self.last_action = clamped_action.detach().clone() + return obs, reward, terminated, truncated, info + + def reset( + self, + *, + seed: int | None = None, + options: dict | None = None, + ): + """Reset joint_q, targets, and step_count for the touched envs. + + Args: + seed: Optional RNG seed for deterministic resets. + options: Optional dict; supports ``{"reset_ids": }`` + for partial resets. No-grad terminal steps use it for + auto-reset; grad-tracked terminal steps expose the IDs in + ``info`` for an explicit reset after backward. + + Returns: + Tuple of ``(obs, info)``. + """ + if seed is not None: + torch.manual_seed(seed) + if options is None: + options = {} + reset_ids = options.get("reset_ids") + if reset_ids is None: + env_ids = torch.arange(self.sim.num_envs, device=self.device) + else: + env_ids = torch.as_tensor(reset_ids, dtype=torch.long, device=self.device) + with torch.no_grad(): + self.step_count[env_ids] = 0 + self.last_action[env_ids] = 0.0 + self._sample_new_targets(env_ids) + runtime = self.sim.differentiable_runtime + for state in runtime.live_states: + joint_q = wp.to_torch(state.joint_q).view(self.sim.num_envs, -1) + joint_q[env_ids] = 0.0 + newton.eval_fk( + runtime.model, + state.joint_q, + state.joint_qd, + state, + ) + obs = self._initial_obs() + return obs, {} + + def _initial_obs(self) -> torch.Tensor: + """Compute the initial observation from the live Spawn state.""" + with torch.no_grad(): + state = self.sim.differentiable_runtime.current_state + n = self.sim.num_envs + joint_q_t = wp.to_torch(state.joint_q).view(n, -1) + body_q_flat = wp.to_torch(state.body_q).view(-1, 7) + ee_pose = body_q_flat[self._ee_idx_t] + obs = torch.cat( + [ + joint_q_t[:, :FRANKA_NUM_ARM_JOINTS], + ee_pose, + self.target_pos, + self.target_quat, + self.last_action, + ], + dim=-1, + ) + return obs.detach() + + def close(self) -> None: + """Close the environment and release resources.""" + self.sim.destroy() diff --git a/examples/sim/demo/cloth_twist.py b/examples/sim/demo/cloth_twist.py new file mode 100644 index 000000000..02c9dedae --- /dev/null +++ b/examples/sim/demo/cloth_twist.py @@ -0,0 +1,381 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Twist two fixed cloth edges in opposite directions with Newton VBD.""" + +from __future__ import annotations + +import argparse +import math +from pathlib import Path + +import numpy as np + +from embodichain.data import get_data_path +from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + SurfaceElementPropertiesCfg, + SurfaceDeformableObjectCfg, + SurfaceDeformablePhysicsCfg, + NewtonPhysicsCfg, + RenderCfg, +) +from embodichain.lab.sim.material import VisualMaterialCfg +from embodichain.lab.sim.objects import SurfaceDeformableObject +from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.visualization import visualization_cfg_from_args +from embodichain.utils import logger + +ASSET_DATASET = "DeformableDemoData" +TEXTURE_FILE_MAP = { + "mianbu": "mianbu.png", + "shabu": "shabu.png", + "mabu": "mabu.png", + "pige": "pige.png", + "jinduan": "jinduan.png", + "niuzai": "niuzai.png", +} + +FPS = 60 +NUM_SUBSTEPS = 10 +SOLVER_ITERATIONS = 4 +DEFAULT_FRAMES = 1000 +ROTATION_ANGULAR_VELOCITY = math.pi / 3.0 +ROTATION_END_TIME = 30.0 +MESH_SCALE = 0.01 +GRID_SIZE = 50 +CLOTH_POSITION = (0.0, 0.0, 0.75) + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the cloth-twist demo.""" + parser = argparse.ArgumentParser(description=__doc__) + add_env_launcher_args_to_parser(parser) + parser.add_argument( + "--iterations", + type=int, + default=DEFAULT_FRAMES, + help="Number of 60 Hz simulation frames.", + ) + parser.add_argument( + "--cloth-material", + choices=tuple(TEXTURE_FILE_MAP), + default="jinduan", + help="Texture preset packaged with the DexSim reference demo.", + ) + parser.set_defaults(device="cuda", physics="newton") + args = parser.parse_args() + if args.physics != "newton": + parser.error("Cloth requires --physics newton.") + if not str(args.device).startswith("cuda"): + parser.error("EmbodiChain cloth currently requires a CUDA device.") + if args.num_envs != 1: + parser.error("This cloth demo currently supports --num_envs 1.") + if args.iterations <= 0: + parser.error("--iterations must be positive.") + return args + + +def prepare_cloth_asset( + material_name: str, +) -> tuple[Path, np.ndarray, np.ndarray, np.ndarray]: + """Resolve the reference assets and load an order-preserving mesh. + + Args: + material_name: Texture preset selected on the command line. + + Returns: + The texture path, scaled vertices, triangles, and per-vertex UVs. + + Raises: + FileNotFoundError: If the downloaded reference package is incomplete. + RuntimeError: If the source mesh does not have the expected topology. + """ + asset_root = Path(get_data_path(f"{ASSET_DATASET}/cloth_twist")).parent + source_mesh = asset_root / "cloth_twist" / "cloth_twist_square_cloth.obj" + texture_path = asset_root / "textures" / TEXTURE_FILE_MAP[material_name] + + missing = [path for path in (source_mesh, texture_path) if not path.is_file()] + if missing: + raise FileNotFoundError( + "Cloth twist asset package is incomplete; missing: " + + ", ".join(str(path) for path in missing) + ) + + vertices: list[list[float]] = [] + texcoords: list[list[float]] = [] + faces: list[list[int]] = [] + face_uvs: list[list[int]] = [] + for line in source_mesh.read_text(encoding="utf-8").splitlines(): + if line.startswith("v "): + _, x, y, z = line.split()[:4] + vertices.append([float(x), float(y), float(z)]) + elif line.startswith("vt "): + _, u, v = line.split()[:3] + texcoords.append([float(u), float(v)]) + elif line.startswith("f "): + references = [item.split("/") for item in line.split()[1:]] + if len(references) != 3: + raise RuntimeError(f"{source_mesh} must contain triangle faces only.") + faces.append([int(reference[0]) - 1 for reference in references]) + face_uvs.append( + [ + int(reference[1]) - 1 if len(reference) > 1 and reference[1] else -1 + for reference in references + ] + ) + + vertices_array = np.asarray(vertices, dtype=np.float32) * MESH_SCALE + triangles = np.asarray(faces, dtype=np.int32) + expected_vertices = GRID_SIZE * GRID_SIZE + expected_faces = 2 * (GRID_SIZE - 1) * (GRID_SIZE - 1) + if vertices_array.shape != (expected_vertices, 3) or triangles.shape != ( + expected_faces, + 3, + ): + raise RuntimeError( + "cloth_twist_square_cloth.obj does not match the expected " + f"{GRID_SIZE} x {GRID_SIZE} topology." + ) + + vertex_uvs = np.full((len(vertices_array), 2), np.nan, dtype=np.float32) + for face, face_uv in zip(triangles, face_uvs, strict=True): + for vertex_index, texcoord_index in zip(face, face_uv, strict=True): + if texcoord_index < 0: + continue + uv = np.asarray(texcoords[texcoord_index], dtype=np.float32) + if np.isnan(vertex_uvs[vertex_index, 0]): + vertex_uvs[vertex_index] = uv + elif not np.allclose(vertex_uvs[vertex_index], uv, atol=1.0e-6): + raise RuntimeError( + f"{source_mesh} maps multiple UVs to vertex {vertex_index}." + ) + vertex_uvs[np.isnan(vertex_uvs[:, 0])] = 0.0 + return texture_path, vertices_array, triangles, vertex_uvs + + +def build_twist_trajectory( + vertices: np.ndarray, + frame_count: int, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Build fixed-node flags and opposite edge-rotation offsets. + + Args: + vertices: Scaled source vertices before the configured cloth pose. + frame_count: Number of outer 60 Hz simulation frames. + + Returns: + Shared node indices, per-node particle flags, and batched offsets. + """ + left_edge = np.asarray( + [GRID_SIZE - 1 + row * GRID_SIZE for row in range(GRID_SIZE)], + dtype=np.int32, + ) + right_edge = np.asarray( + [row * GRID_SIZE for row in range(GRID_SIZE)], + dtype=np.int32, + ) + node_indices = np.concatenate((left_edge, right_edge)) + + particle_flags = np.ones(len(vertices), dtype=np.int32) + particle_flags[node_indices] = 0 + + angle = np.pi / 2.0 + cloth_rotation = np.asarray( + [ + [np.cos(angle), -np.sin(angle), 0.0], + [np.sin(angle), np.cos(angle), 0.0], + [0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + posed_vertices = vertices @ cloth_rotation.T + selected_positions = posed_vertices[node_indices] + rotation_axes = np.asarray( + [[0.0, 1.0, 0.0]] * len(left_edge) + [[0.0, -1.0, 0.0]] * len(right_edge), + dtype=np.float32, + ) + roots = ( + np.sum(selected_positions * rotation_axes, axis=1, keepdims=True) + * rotation_axes + ) + radial_vectors = selected_positions - roots + axis_cross_radial = np.cross(rotation_axes, radial_vectors) + axis_dot_radial = np.sum( + rotation_axes * radial_vectors, + axis=1, + keepdims=True, + ) + + sample_count = frame_count * NUM_SUBSTEPS + times = np.minimum( + np.arange(sample_count, dtype=np.float32) / (FPS * NUM_SUBSTEPS), + ROTATION_END_TIME, + ) + theta = times * ROTATION_ANGULAR_VELOCITY + cosine = np.cos(theta)[:, None, None] + sine = np.sin(theta)[:, None, None] + rotated_radial = ( + cosine * radial_vectors[None] + + sine * axis_cross_radial[None] + + (1.0 - cosine) * rotation_axes[None] * axis_dot_radial[None] + ) + target_positions = roots[None] + rotated_radial + offsets = target_positions - selected_positions[None] + return node_indices, particle_flags, offsets[None].astype(np.float32) + + +def initialize_simulation(args: argparse.Namespace) -> SimulationManager: + """Create the zero-gravity Newton VBD simulation manager.""" + cfg = SimulationManagerCfg( + width=1920, + height=1080, + # Create the native window only after ``main()`` has prepared the + # complete Spawn scene. + headless=True, + device=args.device, + gpu_id=args.gpu_id, + num_envs=args.num_envs, + arena_space=args.arena_space, + render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=NewtonPhysicsCfg( + physics_dt=1.0 / FPS, + device=args.device, + gravity=(0.0, 0.0, 0.0), + num_substeps=NUM_SUBSTEPS, + use_cuda_graph=False, + solver_cfg={ + "solver_type": "vbd", + "iterations": SOLVER_ITERATIONS, + "particle_enable_self_contact": True, + "particle_self_contact_radius": 0.002, + "particle_self_contact_margin": 0.0035, + "particle_enable_tile_solve": True, + "soft_contact_ke": 1.0e3, + "soft_contact_kd": 1.0e-1, + "soft_contact_mu": 0.2, + }, + ), + visualization=visualization_cfg_from_args(args), + ) + return SimulationManager(cfg) + + +def create_cloth( + sim: SimulationManager, + texture_path: Path, + vertices: np.ndarray, + triangles: np.ndarray, + uv_coords: np.ndarray, + particle_flags: np.ndarray, +) -> SurfaceDeformableObject: + """Declare the textured cloth with reference VBD material parameters.""" + return sim.add_deformable_object( + SurfaceDeformableObjectCfg( + uid="twist_cloth", + shape=MeshCfg( + vertices=vertices, + triangles=triangles, + uv_coords=uv_coords, + visual_material=VisualMaterialCfg( + uid="twist_cloth_material", + base_color=[1.0, 1.0, 1.0, 1.0], + base_color_texture=str(texture_path), + roughness=0.8, + metallic=0.0, + ), + ), + init_pos=CLOTH_POSITION, + init_rot=(0.0, 0.0, 90.0), + particle_flags=particle_flags, + attrs=SurfaceDeformablePhysicsCfg( + density=0.2, + surface_props=SurfaceElementPropertiesCfg( + tri_ke=1.0e3, + tri_ka=1.0e3, + tri_kd=2.0e-4, + edge_ke=1.0e-3, + edge_kd=1.0e-2, + ), + ), + ) + ) + + +def configure_window_camera(sim: SimulationManager) -> None: + """Frame the vertical cloth in the native viewer.""" + window = sim.get_world().get_windows() + if window is not None: + window.set_look_at( + eye=np.asarray([2.25, 0.0, CLOTH_POSITION[2]], dtype=np.float32), + look_at=np.asarray(CLOTH_POSITION, dtype=np.float32), + up=np.asarray([0.0, 0.0, 1.0], dtype=np.float32), + ) + + +def main() -> None: + """Create the scene and execute the finite cloth-twist trajectory.""" + args = parse_arguments() + texture_path, vertices, triangles, uv_coords = prepare_cloth_asset( + args.cloth_material + ) + node_indices, particle_flags, offsets = build_twist_trajectory( + vertices, + args.iterations, + ) + sim = initialize_simulation(args) + + try: + cloth = create_cloth( + sim, + texture_path, + vertices, + triangles, + uv_coords, + particle_flags, + ) + sim.register_kinematic_nodal_trajectory( + cloth.uid, + node_indices, + offsets, + rebuild_self_contact_bvh=True, + ) + sim.prepare() + + if not args.headless and sim.open_window(): + configure_window_camera(sim) + + particle_count = cloth.data.n_nodes + logger.log_info( + f"Running cloth twist for {args.iterations} frames at {FPS} Hz " + f"with {particle_count} particles." + ) + for frame in range(args.iterations): + sim.update(step=1) + if frame % 50 == 0: + logger.log_info( + f"Frame {frame}/{args.iterations}, sim_time={frame / FPS:.2f}s" + ) + logger.log_info("Cloth twist simulation complete.") + except KeyboardInterrupt: + logger.log_info("\nExit") + finally: + sim.destroy() + + +if __name__ == "__main__": + main() diff --git a/examples/sim/demo/fold_tshirt.py b/examples/sim/demo/fold_tshirt.py new file mode 100644 index 000000000..df70cd74b --- /dev/null +++ b/examples/sim/demo/fold_tshirt.py @@ -0,0 +1,682 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Fold a live Newton cloth T-shirt with a trajectory-driven DexForce W1.""" + +from __future__ import annotations + +import argparse +import math +import time +from pathlib import Path + +import numpy as np + +from embodichain.data import get_data_path +from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + SurfaceElementPropertiesCfg, + ArticulationRootPropertiesCfg, + SurfaceDeformableObjectCfg, + SurfaceDeformablePhysicsCfg, + JointDrivePropertiesCfg, + NewtonCollisionPropertiesCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + RigidBodyPhysicsCfg, + RigidObjectCfg, + RobotCfg, + RenderCfg, +) +from embodichain.lab.sim.material import VisualMaterialCfg +from embodichain.lab.sim.objects import SurfaceDeformableObject, RigidObject, Robot +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg +from embodichain.lab.visualization import visualization_cfg_from_args +from embodichain.utils import logger + +DEFAULT_DT = 1.0 / 60.0 +DEFAULT_TRAJECTORY_TIME_SCALE = 4.0 +NUM_SUBSTEPS = 9 +SOLVER_ITERATIONS = 20 +FPS_LOG_INTERVAL = 120 + +ASSET_DATASET = "DeformableDemoData" +ANNIVERSARY_MATERIAL = "anniversary" +ANNIVERSARY_VISUAL_OBJ = "shirt_with_front_anniversary_decal_fold_atlas.obj" +ANNIVERSARY_TEXTURE = "shirt_with_front_anniversary_decal_fold_atlas.png" +TEXTURE_FILE_MAP = { + "mianbu": "mianbu.png", + "shabu": "shabu.png", + "mabu": "mabu.png", + "pige": "pige.png", + "jinduan": "jinduan.png", + "niuzai": "niuzai.png", + "wenli": "wenli.png", +} + +TABLE_POSITION = (0.55, 0.0, 1.15) +TABLE_SIZE = (0.52, 1.24, 0.05) +GROUND_POSITION = (0.0, 0.0, -0.01) +GROUND_SIZE = (8.0, 8.0, 0.02) +SHIRT_POSITION = (0.55, 0.0, 1.189) +SHIRT_SCALE = 0.0080 * 0.8 +CONTACT_SCRIPT_TRANSITIONS = np.asarray( + [4.21, 16.8, 19.0, 22.18, 27.4, 31.4], + dtype=np.float32, +) +CONTACT_SCRIPT_TO_SIMULATION_OFFSET = 1.2 + + +def parse_arguments() -> argparse.Namespace: + """Parse and validate command-line arguments.""" + parser = argparse.ArgumentParser(description=__doc__) + add_env_launcher_args_to_parser(parser) + parser.add_argument( + "--steps", + type=int, + default=None, + help="Physics frames to run; defaults to the complete cached trajectory.", + ) + parser.add_argument( + "--dt", + type=float, + default=DEFAULT_DT, + help="Outer EmbodiChain physics timestep in seconds.", + ) + parser.add_argument( + "--trajectory-time-scale", + type=float, + default=DEFAULT_TRAJECTORY_TIME_SCALE, + help="Playback-speed multiplier for the cached W1 trajectory.", + ) + parser.add_argument( + "--static-w1", + action="store_true", + help="Keep W1 at the first trajectory pose for scene inspection.", + ) + parser.add_argument( + "--disable-w1-collision", + action="store_true", + help="Disable particle collision on all W1 links for debugging.", + ) + parser.add_argument( + "--disable-cuda-graph", + action="store_true", + help="Use direct Newton stepping and enable particle-friction scheduling.", + ) + parser.add_argument( + "--real-time", + action="store_true", + help="Sleep after each frame to approximate wall-clock playback.", + ) + parser.add_argument( + "--cloth-material", + choices=(ANNIVERSARY_MATERIAL, *TEXTURE_FILE_MAP), + default=ANNIVERSARY_MATERIAL, + help=( + "Use the authored anniversary atlas or one of the tiled fabric " + "textures packaged with the reference scene." + ), + ) + parser.add_argument( + "--w1-urdf", + type=Path, + default=None, + help="Optional W1 URDF override.", + ) + parser.add_argument( + "--trajectory", + type=Path, + default=None, + help="Optional cached W1 trajectory override.", + ) + parser.set_defaults(device="cuda", physics="newton", renderer="rt") + args = parser.parse_args() + + if args.physics != "newton": + parser.error("T-shirt folding requires --physics newton.") + if not str(args.device).startswith("cuda"): + parser.error("Newton DexUni cloth simulation requires a CUDA device.") + if args.num_envs != 1: + parser.error("This trajectory scene currently supports --num_envs 1.") + if not math.isfinite(args.dt) or args.dt <= 0.0: + parser.error("--dt must be finite and positive.") + if ( + not math.isfinite(args.trajectory_time_scale) + or args.trajectory_time_scale <= 0.0 + ): + parser.error("--trajectory-time-scale must be finite and positive.") + if args.steps is not None and args.steps < 0: + parser.error("--steps must be non-negative.") + return args + + +def resolve_assets( + args: argparse.Namespace, +) -> tuple[Path, Path, Path, Path, Path | None, Path | None]: + """Resolve the W1, trajectory, simulation mesh, and selected visual assets.""" + asset_root = Path(get_data_path(f"{ASSET_DATASET}/fold_tshirt")) + texture_root = asset_root.parent / "textures" + + urdf_path = asset_root / "W1-hand-obj" / "DexforceW1V021_visual_collision.urdf" + trajectory_path = asset_root / "fold_tshirt.npz" + shirt_asset_root = asset_root / "shirt_front_decal_asset" + shirt_mesh_path = shirt_asset_root / "shirt_mesh.txt" + ground_texture_path = texture_root / "ground.png" + visual_mesh_path: Path | None = None + texture_path: Path | None = None + + if args.w1_urdf is not None: + urdf_path = args.w1_urdf.expanduser().resolve() + if args.trajectory is not None: + trajectory_path = args.trajectory.expanduser().resolve() + if args.cloth_material == ANNIVERSARY_MATERIAL: + visual_mesh_path = shirt_asset_root / ANNIVERSARY_VISUAL_OBJ + texture_path = shirt_asset_root / ANNIVERSARY_TEXTURE + else: + texture_path = texture_root / TEXTURE_FILE_MAP[args.cloth_material] + + required = [ + urdf_path, + trajectory_path, + shirt_mesh_path, + ground_texture_path, + ] + if visual_mesh_path is not None: + required.extend( + [ + visual_mesh_path, + visual_mesh_path.with_suffix(".mtl"), + ] + ) + if texture_path is not None: + required.append(texture_path) + missing = [str(path) for path in required if not path.is_file()] + if missing: + raise FileNotFoundError( + "W1 fold asset package is incomplete; missing: " + ", ".join(missing) + ) + return ( + urdf_path, + trajectory_path, + shirt_mesh_path, + ground_texture_path, + visual_mesh_path, + texture_path, + ) + + +def load_shirt_mesh(path: Path) -> tuple[np.ndarray, np.ndarray]: + """Load and normalize the reference shirt simulation mesh.""" + vertices: list[list[float]] = [] + triangles: list[list[int]] = [] + section: str | None = None + for raw_line in path.read_text(encoding="utf-8").splitlines(): + line = raw_line.strip() + if not line: + continue + if line == "shirt_vertices": + section = "vertices" + continue + if line == "shirt_indices": + section = "indices" + continue + if section is None or ":" not in line: + raise RuntimeError(f"Unexpected shirt mesh line: {line!r}.") + _, raw_values = line.split(":", 1) + values = raw_values.split() + if len(values) < 3: + raise RuntimeError(f"Incomplete shirt mesh line: {line!r}.") + if section == "vertices": + vertices.append([float(value) for value in values[:3]]) + else: + triangles.append([int(value) for value in values[:3]]) + + vertex_array = np.asarray(vertices, dtype=np.float32) + triangle_array = np.asarray(triangles, dtype=np.int32) + if vertex_array.ndim != 2 or vertex_array.shape[1:] != (3,) or not len(vertices): + raise RuntimeError(f"{path} contains no valid shirt vertices.") + if ( + triangle_array.ndim != 2 + or triangle_array.shape[1:] != (3,) + or not len(triangles) + ): + raise RuntimeError(f"{path} contains no valid shirt triangles.") + if np.any(triangle_array < 0) or np.any(triangle_array >= len(vertex_array)): + raise RuntimeError(f"{path} contains out-of-range triangle indices.") + + minimum = vertex_array.min(axis=0) + maximum = vertex_array.max(axis=0) + vertex_array[:, :2] -= 0.5 * (minimum[:2] + maximum[:2]) + vertex_array[:, 2] -= minimum[2] + return vertex_array * SHIRT_SCALE, triangle_array + + +def vertex_normals(vertices: np.ndarray, triangles: np.ndarray) -> np.ndarray: + """Compute area-weighted per-vertex normals.""" + normals = np.zeros_like(vertices) + for triangle in triangles: + normal = np.cross( + vertices[triangle[1]] - vertices[triangle[0]], + vertices[triangle[2]] - vertices[triangle[0]], + ) + normals[triangle] += normal + lengths = np.linalg.norm(normals, axis=1, keepdims=True) + valid = lengths[:, 0] > 0.0 + normals[valid] /= lengths[valid] + return normals + + +def planar_uv(vertices: np.ndarray) -> np.ndarray: + """Generate the tiled planar UVs used by the fabric material variants.""" + uv_coords = vertices[:, :2].copy() + uv_coords -= uv_coords.min(axis=0) + uv_coords /= np.maximum(uv_coords.max(axis=0), 1.0e-8) + uv_coords[:, 1] = 1.0 - uv_coords[:, 1] + return uv_coords * 2.0 + + +def load_trajectory(path: Path, time_scale: float) -> tuple[np.ndarray, float]: + """Load the cached W1 public-qpos trajectory and compute playback FPS.""" + with np.load(path) as archive: + trajectory = np.asarray(archive["robot_qpos"], dtype=np.float32) + source_dt = float(np.asarray(archive["dt"]).reshape(-1)[0]) + if trajectory.ndim != 2 or trajectory.shape[0] == 0 or trajectory.shape[1] < 3: + raise ValueError( + f"Expected a non-empty trajectory with shape [frames, dof], got " + f"{trajectory.shape}." + ) + if not np.isfinite(trajectory).all(): + raise ValueError("W1 trajectory must contain only finite values.") + if not math.isfinite(source_dt) or source_dt <= 0.0: + raise ValueError("W1 trajectory dt must be finite and positive.") + + # The cache already follows the public Spawn qpos order. Keep all columns + # intact while locking the three leg joints so the torso stays at table height. + trajectory = np.ascontiguousarray(trajectory, dtype=np.float32).copy() + trajectory[:, :3] = 0.0 + trajectory_fps = (1.0 / source_dt) * time_scale / DEFAULT_TRAJECTORY_TIME_SCALE + return trajectory, trajectory_fps + + +def initialize_simulation( + args: argparse.Namespace, + *, + use_cuda_graph: bool, +) -> SimulationManager: + """Create the EmbodiChain manager with the reference DexUni settings.""" + cfg = SimulationManagerCfg( + width=1920, + height=1080, + # Defer native-window creation until the complete Spawn scene has been + # materialized below. ``main()`` opens it explicitly after ``prepare()`` + # unless the caller requested ``--headless``. + headless=True, + device=args.device, + gpu_id=args.gpu_id, + num_envs=args.num_envs, + arena_space=args.arena_space, + render_cfg=RenderCfg(renderer=args.renderer, spp=1), + physics_cfg=NewtonPhysicsCfg( + physics_dt=args.dt, + device=args.device, + num_substeps=NUM_SUBSTEPS, + use_cuda_graph=use_cuda_graph, + solver_cfg={ + "solver_type": "dexuni", + "iterations": SOLVER_ITERATIONS, + "particle_enable_self_contact": True, + "particle_self_contact_radius": 0.002, + "particle_self_contact_margin": 0.002, + "particle_topological_contact_filter_threshold": 1, + "particle_rest_shape_contact_exclusion_radius": 0.005, + "particle_vertex_contact_buffer_size": 96, + "particle_edge_contact_buffer_size": 128, + "particle_collision_detection_interval": -1, + "self_contact_bvh_rebuild_interval_frames": 15, + "rigid_contact_max": 0, + "step_rigid_bodies": False, + "soft_contact_margin": 0.008, + "soft_contact_ke": 3.0e5, + "soft_contact_kd": 5.0e-2, + "soft_contact_mu": 0.5, + }, + # DexUni owns its particle-shape contacts and collision detection. + collision_cfg=None, + ), + visualization=visualization_cfg_from_args(args), + ) + return SimulationManager(cfg) + + +def create_table(sim: SimulationManager) -> RigidObject: + """Declare the static folding table.""" + return sim.add_rigid_object( + RigidObjectCfg( + uid="table", + shape=CubeCfg( + size=list(TABLE_SIZE), + visual_material=VisualMaterialCfg( + uid="fold_table_material", + base_color=[0.35, 0.42, 0.48, 1.0], + roughness=0.7, + metallic=0.0, + ), + ), + attrs=RigidBodyPhysicsCfg( + collision_props=NewtonCollisionPropertiesCfg( + collision_enabled=True, has_particle_collision=True + ), + material_props=NewtonRigidBodyMaterialCfg( + static_friction=0.5, dynamic_friction=0.5, ke=500000.0, kd=1e-06 + ), + ), + body_type="static", + init_pos=TABLE_POSITION, + ) + ) + + +def create_ground(sim: SimulationManager, texture_path: Path) -> RigidObject: + """Declare the textured static ground used by the reference scene.""" + return sim.add_rigid_object( + RigidObjectCfg( + uid="ground", + shape=CubeCfg( + size=list(GROUND_SIZE), + visual_material=VisualMaterialCfg( + uid="fold_ground_material", + base_color=[1.0, 1.0, 1.0, 1.0], + base_color_texture=str(texture_path), + roughness=0.65, + metallic=0.0, + ), + ), + attrs=RigidBodyPhysicsCfg( + collision_props=NewtonCollisionPropertiesCfg( + collision_enabled=True, has_particle_collision=True + ), + material_props=NewtonRigidBodyMaterialCfg( + static_friction=0.5, dynamic_friction=0.5, ke=500000.0, kd=1e-06 + ), + ), + body_type="static", + init_pos=GROUND_POSITION, + ) + ) + + +def create_w1( + sim: SimulationManager, + urdf_path: Path, + initial_qpos: np.ndarray, + *, + particle_collision_enabled: bool, +) -> Robot: + """Declare the fixed-base W1 using its exact source URDF.""" + robot = sim.add_robot( + RobotCfg( + uid="w1", + fpath=str(urdf_path), + asset_physics_mode="overlay", + root_props=ArticulationRootPropertiesCfg( + fixed_base=True, + self_collision_enabled=False, + ), + # Preserve source drive modes; the runtime control writes kinematic + # joint state directly at every Newton substep. + joint_drive_props=JointDrivePropertiesCfg( + # Several hand joints author zero limits in the URDF, which + # produce invalid MuJoCo actfrcrange values without this overlay. + max_effort=180.0, + max_velocity=4.0, + ), + attrs=RigidBodyPhysicsCfg( + collision_props=NewtonCollisionPropertiesCfg( + collision_enabled=True, + has_particle_collision=particle_collision_enabled, + ), + material_props=NewtonRigidBodyMaterialCfg( + static_friction=0.25, dynamic_friction=0.25, ke=300000.0, kd=0.0001 + ), + ), + init_qpos=initial_qpos, + build_pk_chain=False, + ) + ) + if robot is None: + raise RuntimeError("Failed to declare the DexForce W1 robot.") + return robot + + +def create_shirt( + sim: SimulationManager, + vertices: np.ndarray, + triangles: np.ndarray, + *, + visual_mesh_path: Path | None, + texture_path: Path | None, +) -> SurfaceDeformableObject: + """Declare the low-resolution cloth and its independently bound visual mesh.""" + if visual_mesh_path is not None: + # The OBJ carries seam-duplicated vertices, authored UVs, and its own + # double-sided MTL. Leaving visual_material unset preserves that MTL. + visual_shape = MeshCfg(fpath=str(visual_mesh_path)) + else: + if texture_path is None: + raise ValueError("A fabric texture is required without an atlas mesh.") + visual_shape = MeshCfg( + vertices=vertices, + triangles=triangles, + normals=vertex_normals(vertices, triangles), + uv_coords=planar_uv(vertices), + visual_material=VisualMaterialCfg( + uid="fold_shirt_material", + base_color=[1.0, 1.0, 1.0, 1.0], + base_color_texture=str(texture_path), + roughness=0.8, + metallic=0.0, + ), + ) + + return sim.add_deformable_object( + SurfaceDeformableObjectCfg( + uid="shirt", + shape=MeshCfg(vertices=vertices, triangles=triangles), + visual_shape=visual_shape, + visual_binding_mode="nearest_vertex", + init_pos=SHIRT_POSITION, + init_rot=(0.0, 0.0, -90.0), + particle_radius=0.008, + attrs=SurfaceDeformablePhysicsCfg( + density=200.0, + surface_props=SurfaceElementPropertiesCfg( + tri_ke=1.5e3, + tri_ka=1.5e3, + tri_kd=1.0e-5, + edge_ke=1.2, + edge_kd=0.1, + ), + ), + ) + ) + + +def register_runtime_controls( + sim: SimulationManager, + trajectory: np.ndarray, + trajectory_fps: float, + trajectory_time_scale: float, + *, + static_w1: bool, + use_cuda_graph: bool, +) -> None: + """Register the folding trajectory and phase-dependent contact materials.""" + transition_times = ( + CONTACT_SCRIPT_TRANSITIONS + CONTACT_SCRIPT_TO_SIMULATION_OFFSET + ) / trajectory_time_scale + contact_times = np.concatenate([np.zeros(1, dtype=np.float32), transition_times]) + + def friction_track(values: tuple[float, ...]) -> tuple[tuple[float, float], ...]: + return tuple( + (float(sample_time), value) + for sample_time, value in zip(contact_times, values, strict=True) + ) + + particle_friction = friction_track((0.5, 1.2, 0.0, 0.5, 1.2, 0.0, 0.5)) + w1_friction = friction_track((0.25, 1.2, 0.0, 0.25, 1.2, 0.0, 0.25)) + table_friction = friction_track((0.5, 0.12, 0.5, 0.5, 0.12, 0.5, 0.5)) + + # This global particle control is host-side. Graph mode keeps the initial + # soft_contact_mu while the graph-compatible rigid schedules still run. + if not use_cuda_graph: + sim.register_particle_contact_material_schedule( + {"dynamic_friction": particle_friction} + ) + sim.register_contact_material_schedule( + "w1", + {"dynamic_friction": w1_friction}, + ) + sim.register_contact_material_schedule( + "table", + {"dynamic_friction": table_friction}, + ) + sim.register_contact_material_schedule( + "ground", + {"dynamic_friction": table_friction}, + ) + if not static_w1: + sim.register_kinematic_joint_trajectory( + "w1", + trajectory[None], + fps=trajectory_fps, + ) + + +def configure_window_camera(sim: SimulationManager) -> None: + """Frame both W1 arms and the shirt on the table.""" + window = sim.get_world().get_windows() + if window is not None: + window.set_look_at( + eye=np.asarray([1.15, -2.10, 1.65], dtype=np.float32), + look_at=np.asarray([0.55, 0.0, 1.18], dtype=np.float32), + up=np.asarray([0.0, 0.0, 1.0], dtype=np.float32), + ) + + +def main() -> None: + """Build the EmbodiChain scene and play the complete folding trajectory.""" + args = parse_arguments() + ( + urdf_path, + trajectory_path, + shirt_mesh_path, + ground_texture_path, + visual_mesh_path, + texture_path, + ) = resolve_assets(args) + vertices, triangles = load_shirt_mesh(shirt_mesh_path) + trajectory, trajectory_fps = load_trajectory( + trajectory_path, + args.trajectory_time_scale, + ) + if args.steps is None: + args.steps = int(math.ceil(len(trajectory) / trajectory_fps / args.dt)) + + use_cuda_graph = not args.disable_cuda_graph + sim = initialize_simulation(args, use_cuda_graph=use_cuda_graph) + try: + # Replace EmbodiChain's default grid plane with the reference wood floor. + sim.set_ground_plane_visibility(False) + create_ground(sim, ground_texture_path) + create_table(sim) + robot = create_w1( + sim, + urdf_path, + trajectory[0], + particle_collision_enabled=not args.disable_w1_collision, + ) + shirt = create_shirt( + sim, + vertices, + triangles, + visual_mesh_path=visual_mesh_path, + texture_path=texture_path, + ) + register_runtime_controls( + sim, + trajectory, + trajectory_fps, + args.trajectory_time_scale, + static_w1=args.static_w1, + use_cuda_graph=use_cuda_graph, + ) + sim.prepare() + + if trajectory.shape[1] != robot.dof: + raise ValueError( + f"W1 trajectory has {trajectory.shape[1]} columns, but the " + f"articulation has {robot.dof} DOFs." + ) + if not args.headless and sim.open_window(): + sim.set_emission_light([1.0, 1.0, 1.0], 90.0) + configure_window_camera(sim) + + particle_count = shirt.data.n_nodes + logger.log_info( + "Running W1 T-shirt fold | " + f"driver={'static' if args.static_w1 else 'kinematic-trajectory'} | " + f"cuda_graph={use_cuda_graph} | " + f"frames={len(trajectory)} | trajectory_fps={trajectory_fps:.3f} | " + f"dof={robot.dof} | cloth_particles={particle_count} | " + f"material={args.cloth_material}" + ) + + fps_window_start: float | None = None + fps_window_steps = 0 + for frame in range(args.steps): + frame_start = time.perf_counter() + sim.update(step=1) + if args.real_time: + elapsed = time.perf_counter() - frame_start + time.sleep(max(0.0, args.dt - elapsed)) + + frame_end = time.perf_counter() + if fps_window_start is None: + # Exclude one-time Warp compilation and CUDA Graph capture. + fps_window_start = frame_end + else: + fps_window_steps += 1 + if (frame + 1) % FPS_LOG_INTERVAL == 0 or frame == args.steps - 1: + elapsed = frame_end - fps_window_start + fps = ( + fps_window_steps / elapsed + if elapsed > 0.0 and fps_window_steps > 0 + else 0.0 + ) + logger.log_info(f"Frame {frame + 1}/{args.steps}, FPS={fps:.1f}") + fps_window_start = frame_end + fps_window_steps = 0 + logger.log_info("W1 T-shirt folding simulation complete.") + except KeyboardInterrupt: + logger.log_info("\nExit") + finally: + sim.destroy() + + +if __name__ == "__main__": + main() diff --git a/examples/sim/demo/grasp_cup_to_caffe.py b/examples/sim/demo/grasp_cup_to_caffe.py index 183e36b9d..c0ddadc6f 100644 --- a/examples/sim/demo/grasp_cup_to_caffe.py +++ b/examples/sim/demo/grasp_cup_to_caffe.py @@ -26,21 +26,22 @@ import torch from tqdm import tqdm from typing import Union -from scipy.spatial.transform import Rotation as R from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.visualization import visualization_cfg_from_args from embodichain.lab.sim.objects import Robot, RigidObject from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, LightCfg, MarkerCfg, JointDrivePropertiesCfg, RigidObjectCfg, - RigidBodyAttributesCfg, + MassPropertiesCfg, + RigidBodyPhysicsCfg, ArticulationCfg, ) from embodichain.compute.trajectory import interpolate_with_distance -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.data import get_data_path from embodichain.utils import logger from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser @@ -58,6 +59,12 @@ def parse_arguments(): description="Create and simulate a robot in SimulationManager" ) add_env_launcher_args_to_parser(parser) + parser.add_argument( + "--seed", + type=int, + default=0, + help="Seed for scene XY perturbations; use a negative value for random runs.", + ) return parser.parse_args() @@ -71,10 +78,22 @@ def initialize_simulation(args) -> SimulationManager: Returns: SimulationManager: Configured simulation manager instance. """ + physics_cfg = physics_cfg_for_backend(args.physics) + if args.physics == "newton": + # This contact-heavy URDF scene needs Newton's collision pipeline; + # MuJoCo's native contact path is not reliable for these convex meshes. + physics_cfg.solver_cfg = { + "solver_type": "mujoco_warp", + "use_mujoco_contacts": False, + "nconmax": 16384, + "njmax": 65536, + } + config = SimulationManagerCfg( headless=True, - sim_device=args.device, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg, physics_dt=1.0 / 100.0, num_envs=args.num_envs, arena_space=2.5, @@ -178,11 +197,14 @@ def create_table(sim: SimulationManager) -> RigidObject: uid="table", shape=MeshCfg( fpath=get_data_path("MultiW1Data/table_a.obj"), + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=8, + ), ), - attrs=RigidBodyAttributesCfg( - mass=0.5, + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=0.5), ), - max_convex_hull_num=8, body_type="kinematic", init_pos=[1.1, -0.5, 0.08], init_rot=[0.0, 0.0, 0.0], @@ -206,11 +228,15 @@ def create_caffe(sim: SimulationManager) -> Robot: fpath=get_data_path("MultiW1Data/cafe/cafe.urdf"), init_pos=[1.05, -0.5, 0.79], init_rot=[0, 0, -30], - attrs=RigidBodyAttributesCfg( - mass=1.0, + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), ), - drive_pros=JointDrivePropertiesCfg( - stiffness=1.0, damping=0.1, max_effort=100.0, drive_type="force" + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", + stiffness=1.0, + damping=0.1, + max_effort=100.0, ), ) container = sim.add_articulation(cfg=container_cfg) @@ -232,10 +258,9 @@ def create_cup(sim: SimulationManager) -> RigidObject: shape=MeshCfg( fpath=get_data_path("MultiW1Data/paper_cup_2.obj"), ), - attrs=RigidBodyAttributesCfg( - mass=0.3, + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=0.3), ), - max_convex_hull_num=1, body_type="dynamic", init_pos=[0.86, -0.76, 0.841], init_rot=[0.0, 0.0, 0.0], @@ -274,7 +299,11 @@ def create_trajectory( cup_position = cup.get_local_pose(to_matrix=True)[:, :3, 3] # grasp cup waypoint generation - rest_right_qpos = robot.get_qpos()[:, right_arm_ids] # [num_envs, dof] + # Build the task trajectory from the authored hold target. The measured + # pose after the first physics step includes backend-specific gravity and + # constraint settling, which can send the redundant arm IK to a different + # solution before the task even starts. + rest_right_qpos = robot.get_qpos(target=True)[:, right_arm_ids] right_arm_xpos = robot.compute_fk( qpos=rest_right_qpos, name="right_arm", to_matrix=True ) @@ -426,18 +455,20 @@ def main(): caffe = create_caffe(sim) cup = create_cup(sim) - sim.update(step=1) + sim.prepare() - # apply random perturbation + # Apply initialization-time poses before Newton captures its CUDA graph. + # Seed here so backend initialization cannot consume a different random + # prefix and make Default/Newton comparisons use different scenes. + if args.seed >= 0: + np.random.seed(args.seed) apply_random_xy_perturbation(cup, max_perturbation=0.05) apply_random_xy_perturbation(caffe, max_perturbation=0.05) + sim.update(step=1) if not args.headless: sim.open_window() - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - run_simulation(sim, robot, cup, caffe) logger.log_info("\n Press Ctrl+C to exit simulation loop.") diff --git a/examples/sim/demo/pick_up_cloth.py b/examples/sim/demo/pick_up_cloth.py index 836c012b4..baec6ba99 100644 --- a/examples/sim/demo/pick_up_cloth.py +++ b/examples/sim/demo/pick_up_cloth.py @@ -14,44 +14,53 @@ # limitations under the License. # ---------------------------------------------------------------------------- -""" -This script demonstrates the creation and simulation of a robot with a soft object, -and performs a pressing task in a simulated environment. -""" +"""Pick up a cloth with a UR10 gripper using the Newton DexUni solver.""" from __future__ import annotations import argparse +import os +import tempfile +from collections.abc import Sequence + import numpy as np -import time import open3d as o3d import torch +from scipy.spatial.transform import Rotation -from dexsim.utility.path import get_resources_data_path - -from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.visualization import visualization_cfg_from_args -from embodichain.lab.sim.objects import Robot, SoftObject -from embodichain.compute.trajectory import interpolate_with_distance -from embodichain.lab.sim.shapes import MeshCfg from embodichain.data import get_data_path -from embodichain.utils import logger +from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.cfg import ( - RenderCfg, + SurfaceElementPropertiesCfg, + SurfaceDeformableObjectCfg, + SurfaceDeformablePhysicsCfg, + MassPropertiesCfg, + NewtonCollisionPropertiesCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + RigidBodyPhysicsCfg, RigidObjectCfg, - RigidBodyAttributesCfg, - LightCfg, - ClothObjectCfg, - ClothPhysicalAttributesCfg, + RenderCfg, ) +from embodichain.lab.sim.objects import SurfaceDeformableObject, RigidObject, Robot from embodichain.lab.sim.robots import URRobotCfg -import os -from embodichain.lab.sim.shapes import MeshCfg, CubeCfg -import tempfile -from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg +from embodichain.compute.trajectory import interpolate_with_nums +from embodichain.lab.visualization import visualization_cfg_from_args + +CLOTH_SIZE = 0.3 +CLOTH_GRID_CELLS = 50 +CLOTH_PARTICLE_RADIUS = 0.003 +CLOTH_RIGID_CONTACT_KE = 1.0e6 +CLOTH_RIGID_CONTACT_KD = 5.0e-2 +CLOTH_RIGID_CONTACT_MU = 0.5 -def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]): +def create_robot( + sim: SimulationManager, + position: Sequence[float] = (0.0, 0.0, 0.0), +) -> Robot: """ Create and configure a robot with an arm and a dexterous hand in the simulation. @@ -71,11 +80,32 @@ def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]): {"component_type": "hand", "urdf_path": gripper_urdf_path}, ] }, - "drive_pros": { + "joint_drive_props": { "stiffness": {"FINGER[1-2]": 1e2}, "damping": {"FINGER[1-2]": 1e1}, "max_effort": {"FINGER[1-2]": 1e3}, "drive_type": "force", + "target_mode": "position_velocity", + }, + "link_attrs": { + "gripper_collision": { + "link_names_expr": ["hand_base_link", "finger[1-2]"], + "attrs": { + "collision_props": { + "collision_enabled": True, + "contact_offset": 0.008, + "rest_offset": 0.002, + "backend": "newton", + "has_particle_collision": True, + }, + "material_props": { + "dynamic_friction": 2.0, + "backend": "newton", + "ke": 300000.0, + "kd": 0.0001, + }, + }, + }, }, "control_parts": { "hand": ["FINGER[1-2]"], @@ -106,29 +136,33 @@ def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]): return sim.add_robot(cfg=cfg) -def create_padding_box(sim: SimulationManager): +def create_padding_box(sim: SimulationManager) -> RigidObject: padding_box_cfg = RigidObjectCfg( uid="padding_box", shape=CubeCfg( size=[0.02, 0.07, 0.05], ), - attrs=RigidBodyAttributesCfg( - mass=1.0, - static_friction=0.01, - dynamic_friction=0.00, - restitution=0.01, - min_position_iters=32, - min_velocity_iters=8, + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), + material_props=NewtonRigidBodyMaterialCfg( + static_friction=CLOTH_RIGID_CONTACT_MU, + dynamic_friction=CLOTH_RIGID_CONTACT_MU, + restitution=0.01, + ke=CLOTH_RIGID_CONTACT_KE, + kd=CLOTH_RIGID_CONTACT_KD, + ), + collision_props=NewtonCollisionPropertiesCfg(has_particle_collision=True), ), body_type="kinematic", init_pos=[0.5, 0.0, 0.026], init_rot=[0.0, 0.0, 0.0], ) - padding_box = sim.add_rigid_object(cfg=padding_box_cfg) - return padding_box + return sim.add_rigid_object(cfg=padding_box_cfg) -def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): +def create_2d_grid_mesh( + width: float, height: float, nx: int = 1, ny: int = 1 +) -> tuple[torch.Tensor, torch.Tensor]: """Create a flat rectangle in the XY plane centered at `origin`. The rectangle is subdivided into an `nx` by `ny` grid (cells) and @@ -144,7 +178,7 @@ def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): # Vectorized vertex positions using PyTorch x_lin = torch.linspace(-w / 2.0, w / 2.0, steps=nx + 1, dtype=torch.float64) y_lin = torch.linspace(-h / 2.0, h / 2.0, steps=ny + 1, dtype=torch.float64) - yy, xx = torch.meshgrid(y_lin, x_lin) # shapes: (ny+1, nx+1) + yy, xx = torch.meshgrid(y_lin, x_lin, indexing="ij") xx_flat = xx.reshape(-1) yy_flat = yy.reshape(-1) zz_flat = torch.full_like(xx_flat, 0, dtype=torch.float64) @@ -162,8 +196,13 @@ def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): return verts, faces -def create_cloth(sim: SimulationManager): - cloth_verts, cloth_faces = create_2d_grid_mesh(width=0.3, height=0.3, nx=12, ny=12) +def create_cloth(sim: SimulationManager) -> SurfaceDeformableObject: + cloth_verts, cloth_faces = create_2d_grid_mesh( + width=CLOTH_SIZE, + height=CLOTH_SIZE, + nx=CLOTH_GRID_CELLS, + ny=CLOTH_GRID_CELLS, + ) cloth_mesh = o3d.geometry.TriangleMesh( vertices=o3d.utility.Vector3dVector(cloth_verts.to("cpu").numpy()), triangles=o3d.utility.Vector3iVector(cloth_faces.to("cpu").numpy()), @@ -171,42 +210,112 @@ def create_cloth(sim: SimulationManager): cloth_save_path = os.path.join(tempfile.gettempdir(), "cloth_mesh.ply") o3d.io.write_triangle_mesh(cloth_save_path, cloth_mesh) - cloth = sim.add_cloth_object( - cfg=ClothObjectCfg( + cloth = sim.add_deformable_object( + cfg=SurfaceDeformableObjectCfg( uid="cloth", shape=MeshCfg(fpath=cloth_save_path), init_pos=[0.5, 0.0, 0.3], init_rot=[0, 0, 0], - physical_attr=ClothPhysicalAttributesCfg( - mass=0.01, - youngs=1e10, - poissons=0.4, - thickness=0.06, - bending_stiffness=0.01, - bending_damping=0.1, - dynamic_friction=0.95, - min_position_iters=30, + # Keep the collision shell close to the rendered surface. A large + # radius can make the gripper carry cloth while visibly separated. + particle_radius=CLOTH_PARTICLE_RADIUS, + attrs=SurfaceDeformablePhysicsCfg( + # Give gravity enough authority to produce fabric-like drape. + # Stretch remains firmer than bending so the cloth folds + # instead of behaving like an elastic sheet. + density=0.05, + surface_props=SurfaceElementPropertiesCfg( + tri_ke=2.0e2, + tri_ka=2.0e2, + tri_kd=1.0e-5, + edge_ke=0.005, + edge_kd=0.01, + ), ), ) ) return cloth -def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tensor): +def get_grasp_traj( + sim: SimulationManager, + robot: Robot, + grasp_xpos: torch.Tensor, +) -> torch.Tensor: + """Build the full robot trajectory without materializing the scene. + + DexUni cloth manipulation uses an external kinematic articulation. The + trajectory therefore has to be registered before ``sim.prepare()`` so the + Newton runtime can interpolate joint poses and velocities at every + substep. + """ num_envs = sim.num_envs - rest_arm_qpos = robot.get_qpos("arm") + arm_joint_names = robot.cfg.control_parts["arm"] + arm_dof = len(arm_joint_names) + initial_qpos = torch.as_tensor( + robot.cfg.init_qpos, + dtype=torch.float32, + device=sim.device, + ).reshape(1, -1) + hand_dof = initial_qpos.shape[1] - arm_dof + if hand_dof <= 0: + raise ValueError("The robot trajectory requires at least one hand DOF.") + rest_arm_qpos = initial_qpos[:, :arm_dof].repeat(num_envs, 1) + + solver_cfg = robot.cfg.solver_cfg["arm"] + solver_cfg.joint_names = list(arm_joint_names) + if solver_cfg.urdf_path is None: + solver_cfg.urdf_path = robot.cfg.fpath + solver = solver_cfg.init_solver(device=sim.device) + + root_pose_value = robot.cfg.init_local_pose + if root_pose_value is None: + root_pose_value = np.eye(4, dtype=np.float32) + root_pose_value[:3, :3] = Rotation.from_euler( + "xyz", robot.cfg.init_rot, degrees=True + ).as_matrix() + root_pose_value[:3, 3] = np.asarray(robot.cfg.init_pos, dtype=np.float32) + root_pose = torch.as_tensor( + root_pose_value, + dtype=torch.float32, + device=sim.device, + ).reshape(1, 4, 4) + root_pose = root_pose.repeat(num_envs, 1, 1) + root_pose_inv = torch.linalg.inv(root_pose) approach_xpos = grasp_xpos.clone() - approach_xpos[:, 2, 3] += 0.04 - _, qpos_approach = robot.compute_ik( - pose=approach_xpos, joint_seed=rest_arm_qpos, name="arm" + approach_xpos[:, 2, 3] += 0.06 + approach_xpos = torch.bmm(root_pose_inv, approach_xpos) + local_grasp_xpos = torch.bmm(root_pose_inv, grasp_xpos) + approach_success, qpos_approach = solver.get_ik( + target_xpos=approach_xpos, + qpos_seed=rest_arm_qpos, + ) + grasp_success, qpos_grasp = solver.get_ik( + target_xpos=local_grasp_xpos, + qpos_seed=qpos_approach, ) - _, qpos_grasp = robot.compute_ik( - pose=grasp_xpos, joint_seed=qpos_approach, name="arm" + if not bool(torch.all(approach_success & grasp_success)): + failed_envs = torch.nonzero( + ~(approach_success & grasp_success), as_tuple=False + ).flatten() + raise RuntimeError(f"IK failed for environment indices {failed_envs.tolist()}.") + + hand_open_qpos = initial_qpos[:, arm_dof : arm_dof + hand_dof].repeat(num_envs, 1) + # First close around the cloth ridge while the padding box supports it. + # After lifting clear of the box, close once more so the fingers—not the + # support reaction—provide the sustained normal force for the grasp. + hand_pregrasp_qpos = torch.full( + (num_envs, hand_dof), + 0.012, + dtype=torch.float32, + device=sim.device, ) - hand_open_qpos = torch.tensor([0.00, 0.00], dtype=torch.float32, device=sim.device) - hand_close_qpos = torch.tensor( - [0.025, 0.025], dtype=torch.float32, device=sim.device + hand_grasp_qpos = torch.full( + (num_envs, hand_dof), + 0.024, + dtype=torch.float32, + device=sim.device, ) arm_trajectory = torch.cat( @@ -216,86 +325,163 @@ def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tenso qpos_grasp[:, None, :], qpos_grasp[:, None, :], qpos_approach[:, None, :], + qpos_approach[:, None, :], rest_arm_qpos[:, None, :], ], dim=1, ) hand_trajectory = torch.cat( [ - hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[:, None, :], + hand_open_qpos[:, None, :], + hand_open_qpos[:, None, :], + hand_pregrasp_qpos[:, None, :], + hand_pregrasp_qpos[:, None, :], + hand_grasp_qpos[:, None, :], + hand_grasp_qpos[:, None, :], ], dim=1, ) all_trajectory = torch.cat([arm_trajectory, hand_trajectory], dim=-1) - interp_trajectory = interpolate_with_distance( - trajectory=all_trajectory, interp_num=220, device=sim.device + # Keep the original wall-clock timing while supplying one waypoint per + # 100 Hz frame. Newton further interpolates each frame over 12 substeps. + interp_trajectory = interpolate_with_nums( + trajectory=all_trajectory, + interp_nums=torch.tensor([180, 90, 180, 90, 90, 180]), + device=sim.device, ) return interp_trajectory -def main(): +def register_kinematic_trajectory( + sim: SimulationManager, + trajectory: torch.Tensor, + settle_steps: int, +) -> None: + """Register the grasp trajectory after an initial settling hold. + + Args: + sim: Simulation manager that owns the target robot. + trajectory: Batched robot joint positions with shape + ``(num_envs, frames, dof)``. + settle_steps: Number of initial physics frames held at row zero. + """ + if settle_steps < 0: + raise ValueError("settle_steps must be non-negative.") + + # Row zero is the initial state and each simulation frame advances to the + # next row. Keep settle_steps + 1 identical rows so settling consumes no + # part of the grasp motion. + hold = trajectory[:, :1, :].repeat(1, settle_steps + 1, 1) + playback = torch.cat([hold, trajectory[:, 1:, :]], dim=1) + + # SimulationManager owns Spawn path expansion and the pre-prepare runtime + # control lifecycle. DexSim supplies the substep q/qdot interpolation and FK. + sim.register_kinematic_joint_trajectory("UR10", playback) + + +def main() -> None: """ Main function to demonstrate robot simulation. - This function initializes the simulation, creates the robot and other objects, - and performs the press softbody task. + This function initializes the simulation, creates the robot and cloth, + and executes the pick-up trajectory. """ parser = argparse.ArgumentParser( description="Create a simulation scene with SimulationManager" ) add_env_launcher_args_to_parser(parser) + parser.set_defaults(device="cuda", physics="newton") args = parser.parse_args() + if args.physics != "newton": + parser.error("Cloth requires --physics newton.") + if not str(args.device).startswith("cuda"): + parser.error("Newton DexUni cloth simulation requires a CUDA device.") # Configure the simulation sim_cfg = SimulationManagerCfg( width=1920, height=1080, num_envs=args.num_envs, + arena_space=args.arena_space, + gpu_id=args.gpu_id, + # Create the native window only after the complete Spawn scene has + # been prepared below. headless=True, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device="cuda", + device=args.device, render_cfg=RenderCfg( renderer=args.renderer ), # Enable ray tracing for better visuals + physics_cfg=NewtonPhysicsCfg( + num_substeps=12, + solver_cfg={ + "solver_type": "dexuni", + "iterations": 24, + "particle_enable_self_contact": True, + "particle_self_contact_radius": 0.002, + "particle_self_contact_margin": 0.002, + "particle_topological_contact_filter_threshold": 1, + "particle_rest_shape_contact_exclusion_radius": 0.005, + "particle_vertex_contact_buffer_size": 96, + "particle_edge_contact_buffer_size": 128, + "particle_collision_detection_interval": -1, + "particle_enable_tile_solve": True, + "soft_contact_margin": 0.008, + "soft_contact_ke": CLOTH_RIGID_CONTACT_KE, + "soft_contact_kd": CLOTH_RIGID_CONTACT_KD, + "soft_contact_mu": CLOTH_RIGID_CONTACT_MU, + # Use the mixed material stiffness immediately instead of + # ramping new contacts from the low DexUni default. + "rigid_contact_k_start": CLOTH_RIGID_CONTACT_KE, + "rigid_body_particle_contact_buffer_size": 512, + "rigid_contact_max": 0, + # The registered runtime control advances and interpolates the + # robot kinematically at every Newton substep. + "step_rigid_bodies": False, + "self_contact_bvh_rebuild_interval_frames": 1, + }, + # DexUni owns its particle-shape contacts and collision detection. + collision_cfg=None, + ), visualization=visualization_cfg_from_args(args), ) # Create the simulation instance sim = SimulationManager(sim_cfg) - robot = create_robot(sim) - cloth = create_cloth(sim) - padding_box = create_padding_box(sim) - sim.init_gpu_physics() - if not args.headless: - sim.open_window() - sim.update(step=10) # Let the cloth settle before interaction + try: + robot = create_robot(sim) + create_cloth(sim) + create_padding_box(sim) - grasp_xpos = torch.tensor( - [ + grasp_xpos = torch.tensor( [ - [-1, 0, 0, 0.5], - [0, 1, 0, 0], - [0, 0, -1, 0.075], - [0, 0, 0, 1], + [ + [-1, 0, 0, 0.5], + [0, 1, 0, 0], + [0, 0, -1, 0.075], + [0, 0, 0, 1], + ], ], - ], - dtype=torch.float32, - device=sim.device, - ) - grasp_xpos = grasp_xpos.repeat(sim.num_envs, 1, 1) - grab_traj = get_grasp_traj(sim, robot, grasp_xpos) - input("Press Enter to start grabing cloth...") - - n_waypoint = grab_traj.shape[1] - for i in range(n_waypoint): - robot.set_qpos(grab_traj[:, i, :]) - sim.update(step=3) - input("Press Enter to exit the simulation...") + dtype=torch.float32, + device=sim.device, + ) + grasp_xpos = grasp_xpos.repeat(sim.num_envs, 1, 1) + grab_traj = get_grasp_traj(sim, robot, grasp_xpos) + settle_steps = 100 + register_kinematic_trajectory(sim, grab_traj, settle_steps) + + sim.prepare() + if not args.headless: + sim.open_window() + sim.update(step=settle_steps) + input("Press Enter to start grabbing the cloth...") + + # The initial trajectory row is already active after settling. + sim.update(step=grab_traj.shape[1] - 1) + input("Press Enter to exit the simulation...") + finally: + sim.destroy() if __name__ == "__main__": diff --git a/examples/sim/demo/press_softbody.py b/examples/sim/demo/press_softbody.py index 8da1c306d..54564844a 100644 --- a/examples/sim/demo/press_softbody.py +++ b/examples/sim/demo/press_softbody.py @@ -14,91 +14,133 @@ # limitations under the License. # ---------------------------------------------------------------------------- -""" -This script demonstrates the creation and simulation of a robot with a soft object, -and performs a pressing task in a simulated environment. -""" +"""Press a soft cow with a UR10 using the Newton DexUni solver.""" from __future__ import annotations import argparse + import numpy as np -import time import torch - from dexsim.utility.path import get_resources_data_path +from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.visualization import visualization_cfg_from_args -from embodichain.lab.sim.objects import Robot, SoftObject -from embodichain.compute.trajectory import interpolate_with_distance -from embodichain.lab.sim.shapes import MeshCfg -from embodichain.data import get_data_path -from embodichain.utils import logger from embodichain.lab.sim.cfg import ( + NewtonPhysicsCfg, RenderCfg, - LightCfg, - SoftObjectCfg, - SoftbodyVoxelAttributesCfg, - SoftbodyPhysicalAttributesCfg, + VolumeDeformableObjectCfg, + VolumeDeformablePhysicsCfg, + VolumeDeformableMeshingCfg, ) -from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.objects import Robot, VolumeDeformableObject from embodichain.lab.sim.robots import URRobotCfg +from embodichain.lab.sim.shapes import MeshCfg +from embodichain.compute.trajectory import interpolate_with_nums +from embodichain.lab.visualization import visualization_cfg_from_args +from embodichain.utils import logger +PHYSICS_DT = 1.0 / 100.0 +NUM_SUBSTEPS = 6 +SOLVER_ITERATIONS = 8 +SETTLE_STEPS = 100 -def parse_arguments(): - """ - Parse command-line arguments to configure the simulation. +SOFT_CONTACT_MARGIN = 0.003 +SOFT_CONTACT_KE = 5.0e4 +SOFT_CONTACT_KD = 1.0e-3 +SOFT_CONTACT_MU = 1.0 - Returns: - argparse.Namespace: Parsed arguments including number of environments, device, and rendering options. - """ - parser = argparse.ArgumentParser( - description="Create and simulate a robot in SimulationManager" - ) - add_env_launcher_args_to_parser(parser) - return parser.parse_args() +COW_POSITION = (0.45, -0.1, 0.12) +PRESS_POSITION = (0.5, -0.1, 0.04) +APPROACH_HEIGHT = 0.015 -def initialize_simulation(args): - """ - Initialize the simulation environment based on the provided arguments. +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the demo.""" + parser = argparse.ArgumentParser(description=__doc__) + add_env_launcher_args_to_parser(parser) + parser.set_defaults(device="cuda", physics="newton") + args = parser.parse_args() + if args.physics != "newton": + parser.error("Soft bodies require --physics newton.") + if not str(args.device).startswith("cuda"): + parser.error("EmbodiChain soft bodies currently require a CUDA device.") + return args - Args: - args (argparse.Namespace): Parsed command-line arguments. - Returns: - SimulationManager: Configured simulation manager instance. - """ +def initialize_simulation(args: argparse.Namespace) -> SimulationManager: + """Create the Newton DexUni simulation manager.""" config = SimulationManagerCfg( + width=1920, + height=1080, + # Create the native window only after ``main()`` has prepared the + # complete Spawn scene. headless=True, - sim_device="cuda", - render_cfg=RenderCfg(renderer=args.renderer), - physics_dt=1.0 / 100.0, + device=args.device, + gpu_id=args.gpu_id, num_envs=args.num_envs, + arena_space=args.arena_space, + render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=NewtonPhysicsCfg( + physics_dt=PHYSICS_DT, + device=args.device, + num_substeps=NUM_SUBSTEPS, + solver_cfg={ + "solver_type": "dexuni", + "iterations": SOLVER_ITERATIONS, + "particle_enable_self_contact": False, + "particle_self_contact_radius": 0.005, + "particle_self_contact_margin": 0.005, + "particle_topological_contact_filter_threshold": 3, + "particle_enable_tile_solve": True, + "rigid_body_particle_contact_buffer_size": 512, + "rigid_contact_k_start": SOFT_CONTACT_KE, + "rigid_contact_max": 0, + "soft_contact_margin": SOFT_CONTACT_MARGIN, + "soft_contact_ke": SOFT_CONTACT_KE, + "soft_contact_kd": SOFT_CONTACT_KD, + "soft_contact_mu": SOFT_CONTACT_MU, + # The registered trajectory updates the robot kinematically at + # every Newton substep; DexUni only needs to solve the soft body. + "step_rigid_bodies": False, + }, + # DexUni owns its particle-shape contacts and collision detection. + collision_cfg=None, + ), visualization=visualization_cfg_from_args(args), ) - sim = SimulationManager(config) - - return sim + return SimulationManager(config) -def create_robot(sim: SimulationManager): - """ - Create and configure a robot with an arm and a dexterous hand in the simulation. - - Args: - sim (SimulationManager): The simulation manager instance. - - Returns: - Robot: The configured robot instance added to the simulation. - """ +def create_robot(sim: SimulationManager) -> Robot: + """Add the UR10 and enable particle contact on its pressing flange.""" cfg = URRobotCfg.from_dict( { "robot_type": "ur10", "uid": "UR10", "solver_cfg": {"arm": {"tcp": np.eye(4)}}, + "link_attrs": { + "pressing_flange": { + # ee_link has no collision geometry in the UR10 asset; + # Link6 is the physical flange immediately above it. + "link_names_expr": ["Link6"], + "attrs": { + "collision_props": { + "collision_enabled": True, + "contact_offset": 0.004, + "rest_offset": 0.001, + "backend": "newton", + "has_particle_collision": True, + }, + "material_props": { + "dynamic_friction": SOFT_CONTACT_MU, + "backend": "newton", + "ke": SOFT_CONTACT_KE, + "kd": SOFT_CONTACT_KD, + }, + }, + }, + }, "init_qpos": [ 0.0, -np.pi / 2, @@ -109,97 +151,133 @@ def create_robot(sim: SimulationManager): ], } ) - return sim.add_robot(cfg=cfg) - + robot = sim.add_robot(cfg=cfg) + if robot is None: + raise RuntimeError("Failed to add the UR10 robot.") + return robot -def create_soft_cow(sim: SimulationManager) -> SoftObject: - """create soft cow object in the simulation - Args: - sim (SimulationManager): The simulation manager instance. - - Returns: - SoftObject: soft cow object - """ - cow: SoftObject = sim.add_soft_object( - cfg=SoftObjectCfg( +def create_soft_cow(sim: SimulationManager) -> VolumeDeformableObject: + """Add the tetrahedral soft cow used by the pressing task.""" + return sim.add_deformable_object( + cfg=VolumeDeformableObjectCfg( uid="cow", shape=MeshCfg( fpath=get_resources_data_path("Model", "cow", "cow2.obj"), ), - init_rot=[0, 90, 0], - init_pos=[0.45, -0.1, 0.12], - voxel_attr=SoftbodyVoxelAttributesCfg( - simulation_mesh_resolution=8, - maximal_edge_length=0.5, + init_rot=[0.0, 90.0, 0.0], + init_pos=COW_POSITION, + particle_radius=0.005, + meshing=VolumeDeformableMeshingCfg( + triangle_remesh_resolution=24, + simulation_mesh_resolution=16, + voxel_num_relaxation_iters=5, ), - physical_attr=SoftbodyPhysicalAttributesCfg( - youngs=5e3, + attrs=VolumeDeformablePhysicsCfg( + youngs=5.0e3, poissons=0.45, - density=100, - dynamic_friction=0.1, + density=100.0, + elasticity_damping=0.1, ), - ), + ) ) - return cow - - -def press_cow(sim: SimulationManager, robot: Robot): - """robot press cow softbody with its end link - - Args: - sim (SimulationManager): The simulation manager instance. - robot (Robot): The robot instance to be controlled. - """ - start_qpos = robot.get_qpos() - arm_ids = robot.get_joint_ids("arm") - arm_start_qpos = start_qpos[:, arm_ids] - - arm_start_xpos = robot.compute_fk(arm_start_qpos, name="arm", to_matrix=True) - press_xpos = arm_start_xpos.clone() - press_xpos[:, :3, 3] = torch.tensor([0.5, -0.1, 0.005], device=press_xpos.device) - approach_xpos = press_xpos.clone() - approach_xpos[:, 2, 3] += 0.05 - is_success, approach_qpos = robot.compute_ik( - approach_xpos, joint_seed=arm_start_qpos, name="arm" +def build_press_trajectory(sim: SimulationManager, robot: Robot) -> torch.Tensor: + """Compute the batched approach-and-press trajectory before preparation.""" + arm_joint_names = robot.cfg.control_parts["arm"] + initial_qpos = torch.as_tensor( + robot.cfg.init_qpos, + dtype=torch.float32, + device=sim.device, + ).reshape(1, -1) + initial_qpos = initial_qpos.repeat(sim.num_envs, 1) + + solver_cfg = robot.cfg.solver_cfg["arm"] + solver_cfg.joint_names = list(arm_joint_names) + if solver_cfg.urdf_path is None: + solver_cfg.urdf_path = robot.cfg.fpath + solver = solver_cfg.init_solver(device=sim.device) + + approach_pose = solver.get_fk(initial_qpos) + approach_pose[:, :3, 3] = torch.tensor( + [ + PRESS_POSITION[0], + PRESS_POSITION[1], + PRESS_POSITION[2] + APPROACH_HEIGHT, + ], + dtype=torch.float32, + device=sim.device, + ) + press_pose = approach_pose.clone() + press_pose[:, :3, 3] = torch.tensor( + PRESS_POSITION, + dtype=torch.float32, + device=sim.device, ) - arm_trajectory = torch.concatenate([arm_start_qpos, approach_qpos]) - interp_trajectory = interpolate_with_distance( - trajectory=arm_trajectory[None, :, :], interp_num=50, device=sim.device + approach_success, approach_qpos = solver.get_ik( + target_xpos=approach_pose, + qpos_seed=initial_qpos, + ) + press_success, press_qpos = solver.get_ik( + target_xpos=press_pose, + qpos_seed=approach_qpos, + ) + success = approach_success & press_success + if not bool(torch.all(success)): + failed_envs = torch.nonzero(~success, as_tuple=False).flatten().tolist() + raise RuntimeError(f"IK failed for environment indices {failed_envs}.") + + keyframes = torch.stack( + [initial_qpos, approach_qpos, press_qpos], + dim=1, + ) + # Move to the cow over 1.5 seconds, then press by 1.5 cm over 1 second. + return interpolate_with_nums( + trajectory=keyframes, + interp_nums=torch.tensor([150, 100]), + device=sim.device, ) - interp_trajectory = interp_trajectory[0] - for qpos in interp_trajectory: - robot.set_qpos(qpos.unsqueeze(0).repeat(sim.num_envs, 1), joint_ids=arm_ids) - sim.update(step=5) -def main(): - """ - Main function to demonstrate robot simulation. +def register_press_trajectory( + sim: SimulationManager, + trajectory: torch.Tensor, +) -> None: + """Register the press after an initial soft-body settling interval.""" + hold = trajectory[:, :1, :].repeat(1, SETTLE_STEPS + 1, 1) + playback = torch.cat([hold, trajectory[:, 1:, :]], dim=1) + sim.register_kinematic_joint_trajectory("UR10", playback) - This function initializes the simulation, creates the robot and other objects, - and performs the press softbody task. - """ + +def main() -> None: + """Create the scene, settle the cow, and execute the pressing motion.""" args = parse_arguments() sim = initialize_simulation(args) - robot = create_robot(sim) - soft_cow = create_soft_cow(sim) - sim.init_gpu_physics() - if not args.headless: - sim.open_window() + try: + robot = create_robot(sim) + create_soft_cow(sim) + trajectory = build_press_trajectory(sim, robot) + register_press_trajectory(sim, trajectory) + + sim.prepare() + if not args.headless: + sim.open_window() - press_cow(sim, robot) + sim.update(step=SETTLE_STEPS) + if not args.headless: + input("Press Enter to press the soft body...") + sim.update(step=trajectory.shape[1] - 1) - logger.log_info("\n Press Ctrl+C to exit simulation loop.") - try: + logger.log_info("\nPress Ctrl+C to exit the simulation loop.") while True: sim.update(step=10) except KeyboardInterrupt: - logger.log_info("\n Exit") + logger.log_info("\nExit") + finally: + sim.destroy() if __name__ == "__main__": diff --git a/examples/sim/demo/scoop_ice.py b/examples/sim/demo/scoop_ice.py index 8b7c4e6cb..aef6fdcbb 100644 --- a/examples/sim/demo/scoop_ice.py +++ b/examples/sim/demo/scoop_ice.py @@ -33,8 +33,9 @@ from embodichain.lab.sim.objects import Robot, RigidObject, RigidObjectGroup from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, RigidObjectCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, ArticulationCfg, RigidObjectGroupCfg, JointDrivePropertiesCfg, @@ -42,7 +43,7 @@ ) from embodichain.lab.sim.material import VisualMaterialCfg from embodichain.compute.trajectory import interpolate_with_distance -from embodichain.lab.sim.shapes import MeshCfg, CubeCfg +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, MeshCollisionCfg from embodichain.data import get_data_path from embodichain.utils import logger from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser @@ -61,7 +62,9 @@ def initialize_simulation(args): """ config = SimulationManagerCfg( headless=True, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), physics_dt=1.0 / 100.0, visualization=visualization_cfg_from_args(args), ) @@ -145,7 +148,7 @@ def create_robot(sim): "LEFT_HAND_PINKY", ], }, - "drive_pros": { + "joint_drive_props": { "stiffness": {"LEFT_[A-Z|_]+[0-9]?": 1e2}, "damping": {"LEFT_[A-Z|_]+[0-9]?": 1e1}, "max_effort": {"LEFT_[A-Z|_]+[0-9]?": 1e3}, @@ -182,16 +185,22 @@ def create_scoop(sim: SimulationManager): uid="scoop", shape=MeshCfg( fpath=get_data_path("ScoopIceNewEnv/scoop.ply"), + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=12, + ), ), - attrs=RigidBodyAttributesCfg( - mass=0.5, - static_friction=0.95, - dynamic_friction=0.9, - restitution=0.01, - min_position_iters=32, - min_velocity_iters=8, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 0.5}, + "rigid_props": {"min_position_iters": 32, "min_velocity_iters": 8}, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01, + }, + } ), - max_convex_hull_num=12, body_type="dynamic", init_pos=[0.6, 0.0, 0.09], init_rot=[0.0, 0.0, 0.0], @@ -207,13 +216,16 @@ def create_heave_ice(sim: SimulationManager): shape=MeshCfg( fpath=get_data_path("ScoopIceNewEnv/ice_mesh_small/ice_000.obj"), ), - attrs=RigidBodyAttributesCfg( - mass=0.5, - static_friction=0.95, - dynamic_friction=0.9, - restitution=0.01, - min_position_iters=32, - min_velocity_iters=8, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 0.5}, + "rigid_props": {"min_position_iters": 32, "min_velocity_iters": 8}, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01, + }, + } ), body_type="dynamic", init_pos=[10, 10, 0.08], @@ -229,13 +241,16 @@ def create_padding_box(sim: SimulationManager): shape=CubeCfg( size=[0.1, 0.16, 0.05], ), - attrs=RigidBodyAttributesCfg( - mass=1.0, - static_friction=0.95, - dynamic_friction=0.9, - restitution=0.01, - min_position_iters=32, - min_velocity_iters=8, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 1.0}, + "rigid_props": {"min_position_iters": 32, "min_velocity_iters": 8}, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01, + }, + } ), body_type="kinematic", init_pos=[0.6, 0.15, 0.025], @@ -251,15 +266,18 @@ def create_container(sim: SimulationManager): fpath=get_data_path("ScoopIceNewEnv/IceContainer/ice_container.urdf"), init_pos=[0.7, -0.4, 0.21], init_rot=[0, 0, -90], - attrs=RigidBodyAttributesCfg( - mass=1.0, - static_friction=0.95, - dynamic_friction=0.9, - restitution=0.01, - min_position_iters=32, - min_velocity_iters=8, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 1.0}, + "rigid_props": {"min_position_iters": 32, "min_velocity_iters": 8}, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01, + }, + } ), - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( stiffness=1.0, damping=0.1, max_effort=100.0, drive_type="force" ), ) @@ -277,15 +295,21 @@ def create_ice_cubes(sim: SimulationManager): "rigid_objects": { "obj": { "attrs": { - "mass": 0.003, - "contact_offset": 0.001, - "rest_offset": 0, - "dynamic_friction": 0.05, - "static_friction": 0.1, - "restitution": 0.01, - "min_position_iters": 32, - "min_velocity_iters": 4, - "max_depenetration_velocity": 1.0, + "mass_props": {"mass": 0.003}, + "rigid_props": { + "min_position_iters": 32, + "min_velocity_iters": 4, + "max_depenetration_velocity": 1.0, + }, + "collision_props": { + "contact_offset": 0.001, + "rest_offset": 0, + }, + "material_props": { + "dynamic_friction": 0.05, + "static_friction": 0.1, + "restitution": 0.01, + }, }, "shape": {"shape_type": "Mesh"}, "init_pos": [20.0, 0, 1.0], @@ -307,6 +331,7 @@ def create_ice_cubes(sim: SimulationManager): material_type="BSDF", ) ) + sim.prepare() ice_cubes.set_visual_material(mat=ice_mat) return ice_cubes @@ -543,6 +568,7 @@ def main(): scoop = create_scoop(sim) heave_ice = create_heave_ice(sim) ice_cubes = create_ice_cubes(sim) + sim.prepare() if not args.headless: sim.open_window() diff --git a/examples/sim/demo/softbody_to_cloth.py b/examples/sim/demo/softbody_to_cloth.py new file mode 100644 index 000000000..b51a4f6a0 --- /dev/null +++ b/examples/sim/demo/softbody_to_cloth.py @@ -0,0 +1,423 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Drop a volumetric soft body onto a cloth sheet with Newton VBD.""" + +from __future__ import annotations + +import argparse + +import numpy as np + +from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + SurfaceElementPropertiesCfg, + SurfaceDeformableObjectCfg, + SurfaceDeformablePhysicsCfg, + LightCfg, + NewtonPhysicsCfg, + RenderCfg, + VolumeDeformableObjectCfg, + VolumeDeformablePhysicsCfg, + VolumeDeformableMeshingCfg, +) +from embodichain.lab.sim.material import VisualMaterialCfg +from embodichain.lab.sim.objects import SurfaceDeformableObject, VolumeDeformableObject +from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.visualization import visualization_cfg_from_args +from embodichain.utils import logger + +__all__ = [ + "configure_window_camera", + "create_box_surface_mesh", + "create_cloth", + "create_cloth_grid_mesh", + "create_soft_body", + "initialize_simulation", + "main", + "parse_arguments", + "run_simulation", +] + +FPS = 60 +NUM_SUBSTEPS = 3 +SOLVER_ITERATIONS = 6 +DEFAULT_ITERATIONS = 500 + +CLOTH_SIZE = 2.0 +CLOTH_GRID_CELLS = 28 +CLOTH_POSITION = (-1.0, -1.0, 1.0) +SOFT_BODY_SIZE = (0.6, 0.6, 0.3) +SOFT_BODY_POSITION = (0.0, 0.0, 2.0) + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the coupled deformable demo. + + Returns: + The validated command-line arguments. + """ + parser = argparse.ArgumentParser(description=__doc__) + add_env_launcher_args_to_parser(parser) + parser.add_argument( + "--iterations", + type=int, + default=DEFAULT_ITERATIONS, + help="Number of outer 60 Hz simulation frames.", + ) + parser.set_defaults(device="cuda", physics="newton") + args = parser.parse_args() + if args.physics != "newton": + parser.error("Soft bodies and cloth require --physics newton.") + if not str(args.device).startswith("cuda"): + parser.error("EmbodiChain deformables currently require a CUDA device.") + if args.iterations <= 0: + parser.error("--iterations must be positive.") + return args + + +def initialize_simulation(args: argparse.Namespace) -> SimulationManager: + """Create the Newton VBD simulation manager. + + Args: + args: Parsed command-line arguments. + + Returns: + The configured simulation manager. + """ + cfg = SimulationManagerCfg( + width=1920, + height=1080, + # Create the native window only after ``main()`` has prepared the + # complete Spawn scene. + headless=True, + device=args.device, + gpu_id=args.gpu_id, + num_envs=args.num_envs, + arena_space=args.arena_space, + render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=NewtonPhysicsCfg( + physics_dt=1.0 / FPS, + device=args.device, + num_substeps=NUM_SUBSTEPS, + solver_cfg={ + "solver_type": "vbd", + "iterations": SOLVER_ITERATIONS, + "particle_enable_self_contact": True, + "particle_self_contact_radius": 0.01, + "particle_self_contact_margin": 0.02, + "particle_topological_contact_filter_threshold": 3, + "particle_rest_shape_contact_exclusion_radius": 0.05, + "particle_enable_tile_solve": True, + "soft_contact_ke": 1.0e5, + "soft_contact_kd": 1.0e-5, + "soft_contact_mu": 1.0, + }, + ), + visualization=visualization_cfg_from_args(args), + ) + sim = SimulationManager(cfg) + sim.set_emission_light(color=(0.5, 0.5, 0.5), intensity=90.0) + sim.add_light( + LightCfg( + uid="main_light", + intensity=100.0, + radius=10.0, + init_pos=(-4.0, 3.0, 4.5), + ) + ) + return sim + + +def create_box_surface_mesh( + size: tuple[float, float, float], + subdivisions: int = 6, +) -> tuple[np.ndarray, np.ndarray]: + """Create a watertight, subdivided box surface. + + Shared edge and corner vertices keep the mesh suitable for soft-body + tetrahedralization while providing enough render vertices to show its + deformation. + + Args: + size: Box dimensions along the X, Y, and Z axes. + subdivisions: Number of cells along each box edge. + + Returns: + Float32 vertices and int32 outward-facing triangle indices. + + Raises: + ValueError: If the size or subdivision count is invalid. + """ + size_array = np.asarray(size, dtype=np.float32) + if size_array.shape != (3,) or not np.isfinite(size_array).all(): + raise ValueError("size must contain three finite values.") + if np.any(size_array <= 0.0): + raise ValueError("All box dimensions must be positive.") + if subdivisions < 1: + raise ValueError("subdivisions must be at least one.") + + vertices: list[np.ndarray] = [] + triangles: list[tuple[int, int, int]] = [] + vertex_indices: dict[tuple[int, int, int], int] = {} + + def vertex_index(lattice_index: tuple[int, int, int]) -> int: + """Return a shared vertex index for one surface lattice point.""" + index = vertex_indices.get(lattice_index) + if index is not None: + return index + coordinate = ( + np.asarray(lattice_index, dtype=np.float32) / subdivisions - 0.5 + ) * size_array + index = len(vertices) + vertices.append(coordinate) + vertex_indices[lattice_index] = index + return index + + # Each (constant axis, side, U axis, V axis) tuple has U x V pointing + # outward, so both generated triangles have consistent winding. + face_specs = ( + (2, subdivisions, 0, 1), + (2, 0, 1, 0), + (0, subdivisions, 1, 2), + (0, 0, 2, 1), + (1, subdivisions, 2, 0), + (1, 0, 0, 2), + ) + for constant_axis, side, u_axis, v_axis in face_specs: + for v_index in range(subdivisions): + for u_index in range(subdivisions): + corners: list[int] = [] + for u_offset, v_offset in ((0, 0), (1, 0), (1, 1), (0, 1)): + lattice = [0, 0, 0] + lattice[constant_axis] = side + lattice[u_axis] = u_index + u_offset + lattice[v_axis] = v_index + v_offset + corners.append(vertex_index(tuple(lattice))) + triangles.append((corners[0], corners[1], corners[2])) + triangles.append((corners[0], corners[2], corners[3])) + + return ( + np.ascontiguousarray(vertices, dtype=np.float32), + np.ascontiguousarray(triangles, dtype=np.int32), + ) + + +def create_cloth_grid_mesh( + size: float = CLOTH_SIZE, + cells: int = CLOTH_GRID_CELLS, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Create the horizontal cloth grid and its two fixed edge selections. + + Args: + size: Cloth width and depth in metres. + cells: Number of grid cells along each axis. + + Returns: + Vertices, triangles, and fixed-node indices. + + Raises: + ValueError: If the size or cell count is invalid. + """ + if not np.isfinite(size) or size <= 0.0: + raise ValueError("size must be finite and positive.") + if cells < 1: + raise ValueError("cells must be at least one.") + + coordinates = np.linspace(0.0, size, cells + 1, dtype=np.float32) + yy, xx = np.meshgrid(coordinates, coordinates, indexing="ij") + vertices = np.stack( + (xx.reshape(-1), yy.reshape(-1), np.zeros(xx.size, dtype=np.float32)), + axis=1, + ) + + grid_indices = np.arange((cells + 1) ** 2, dtype=np.int32).reshape( + cells + 1, cells + 1 + ) + lower_left = grid_indices[:-1, :-1].reshape(-1) + lower_right = grid_indices[:-1, 1:].reshape(-1) + upper_left = grid_indices[1:, :-1].reshape(-1) + upper_right = grid_indices[1:, 1:].reshape(-1) + triangles = np.concatenate( + ( + np.stack((lower_left, lower_right, upper_right), axis=1), + np.stack((lower_left, upper_right, upper_left), axis=1), + ), + axis=0, + ).astype(np.int32, copy=False) + fixed_indices = np.flatnonzero( + np.isclose(vertices[:, 0], 0.0) | np.isclose(vertices[:, 0], size) + ).astype(np.int32, copy=False) + return ( + np.ascontiguousarray(vertices), + np.ascontiguousarray(triangles), + np.ascontiguousarray(fixed_indices), + ) + + +def create_soft_body(sim: SimulationManager) -> VolumeDeformableObject: + """Declare the falling soft box with the reference material parameters. + + Args: + sim: Simulation manager that owns the scene declaration. + + Returns: + The declared volume-deformable facade. + """ + vertices, triangles = create_box_surface_mesh(SOFT_BODY_SIZE) + return sim.add_deformable_object( + VolumeDeformableObjectCfg( + uid="falling_soft_body", + shape=MeshCfg( + vertices=vertices, + triangles=triangles, + visual_material=VisualMaterialCfg( + uid="soft_material", + base_color=[0.9, 0.5, 0.2, 1.0], + roughness=0.8, + ), + ), + init_pos=SOFT_BODY_POSITION, + meshing=VolumeDeformableMeshingCfg( + simulation_mesh_resolution=10, + ), + attrs=VolumeDeformablePhysicsCfg( + # This converts exactly to k_mu=8e3 and k_lambda=8e3. + youngs=2.0e4, + poissons=0.25, + density=1.5e2, + elasticity_damping=6.0e-4, + ), + ) + ) + + +def create_cloth(sim: SimulationManager) -> SurfaceDeformableObject: + """Declare the cloth sheet with both X edges fixed. + + Args: + sim: Simulation manager that owns the scene declaration. + + Returns: + The declared surface-deformable facade. + """ + vertices, triangles, fixed_indices = create_cloth_grid_mesh() + particle_flags = np.ones(len(vertices), dtype=np.int32) + particle_flags[fixed_indices] = 0 + return sim.add_deformable_object( + SurfaceDeformableObjectCfg( + uid="cloth_sheet", + shape=MeshCfg( + vertices=vertices, + triangles=triangles, + visual_material=VisualMaterialCfg( + uid="cloth_material", + base_color=[0.4, 0.6, 0.9, 1.0], + roughness=0.8, + ), + ), + init_pos=CLOTH_POSITION, + particle_radius=0.05, + particle_flags=particle_flags, + attrs=SurfaceDeformablePhysicsCfg( + density=5.0e-4, + surface_props=SurfaceElementPropertiesCfg( + tri_ke=1.0e5, + tri_ka=1.0e5, + tri_kd=1.0e-5, + edge_ke=0.01, + edge_kd=1.0e-2, + ), + ), + ) + ) + + +def configure_window_camera(sim: SimulationManager) -> None: + """Frame the falling soft body and suspended cloth in the viewer. + + Args: + sim: Prepared simulation manager with an open native window. + """ + window = sim.get_world().get_windows() + if window is not None: + window.set_look_at( + eye=np.asarray([3.0, 3.0, 4.0], dtype=np.float32), + look_at=np.asarray([0.0, 0.0, 1.0], dtype=np.float32), + up=np.asarray([0.0, 0.0, 1.0], dtype=np.float32), + ) + + +def run_simulation( + sim: SimulationManager, + soft_body: VolumeDeformableObject, + cloth: SurfaceDeformableObject, + iterations: int, +) -> None: + """Advance the coupled scene for a finite number of frames. + + Args: + sim: Prepared simulation manager. + soft_body: Falling volumetric deformable. + cloth: Suspended surface deformable. + iterations: Number of outer simulation frames. + """ + logger.log_info(f"Running soft body to cloth for {iterations} frames at {FPS} Hz.") + logger.log_info( + f"Soft body: {soft_body.data.n_nodes} particles, " + f"{soft_body.get_surface_triangles().shape[1]} surface triangles." + ) + logger.log_info( + f"Cloth: {cloth.data.n_nodes} particles, " + f"{cloth.get_surface_triangles().shape[1]} triangles." + ) + + for frame in range(iterations): + sim.update(step=1) + if frame % 50 == 0 or frame + 1 == iterations: + soft_height = float(soft_body.data.root_pos_w[:, 2].mean().item()) + cloth_height = float(cloth.data.root_pos_w[:, 2].mean().item()) + logger.log_info( + f"Frame {frame + 1}/{iterations}, " + f"sim_time={(frame + 1) / FPS:.2f}s, " + f"soft_mean_z={soft_height:.3f}m, " + f"cloth_mean_z={cloth_height:.3f}m" + ) + logger.log_info("Soft body to cloth simulation complete.") + + +def main() -> None: + """Build and run the coupled soft-body/cloth scene.""" + args = parse_arguments() + sim = initialize_simulation(args) + + try: + soft_body = create_soft_body(sim) + cloth = create_cloth(sim) + sim.prepare() + + if not args.headless and sim.open_window(): + configure_window_camera(sim) + run_simulation(sim, soft_body, cloth, args.iterations) + except KeyboardInterrupt: + logger.log_info("\nExit") + finally: + sim.destroy() + + +if __name__ == "__main__": + main() diff --git a/examples/sim/gizmo/gizmo_camera.py b/examples/sim/gizmo/gizmo_camera.py index 15b7e7fd0..88f38e3a5 100644 --- a/examples/sim/gizmo/gizmo_camera.py +++ b/examples/sim/gizmo/gizmo_camera.py @@ -31,7 +31,12 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.visualization import visualization_cfg_from_args from embodichain.lab.sim.sensors import Camera, CameraCfg -from embodichain.lab.sim.cfg import RigidObjectCfg, RigidBodyAttributesCfg, RenderCfg +from embodichain.lab.sim.cfg import ( + RigidObjectCfg, + RigidBodyPhysicsCfg, + RenderCfg, + physics_cfg_for_backend, +) from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser @@ -53,8 +58,9 @@ def main(): height=1080, headless=True, physics_dt=1.0 / 100.0, - sim_device=args.device, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), visualization=visualization_cfg_from_args(args), ) @@ -67,11 +73,15 @@ def main(): uid=f"cube_{i}", shape=CubeCfg(size=[0.1, 0.1, 0.1]), body_type="dynamic", - attrs=RigidBodyAttributesCfg( - mass=1.0, - dynamic_friction=0.5, - static_friction=0.5, - restitution=0.3, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 1.0}, + "material_props": { + "dynamic_friction": 0.5, + "static_friction": 0.5, + "restitution": 0.3, + }, + } ), init_pos=[0.5 + i * 0.3, 0.0, 0.5], ) @@ -96,6 +106,7 @@ def main(): # Add camera to simulation camera = sim.add_sensor(sensor_cfg=camera_cfg) + sim.prepare() if sim.is_use_gpu_physics: sim.init_gpu_physics() diff --git a/examples/sim/gizmo/gizmo_object.py b/examples/sim/gizmo/gizmo_object.py index 4e315d29c..33ab6f22c 100644 --- a/examples/sim/gizmo/gizmo_object.py +++ b/examples/sim/gizmo/gizmo_object.py @@ -24,8 +24,12 @@ import dexsim from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + RigidBodyPhysicsCfg, + RenderCfg, + physics_cfg_for_backend, +) from embodichain.lab.visualization import visualization_cfg_from_args -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RenderCfg from embodichain.lab.sim.shapes import CubeCfg from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg @@ -48,10 +52,11 @@ def main(): height=1080, headless=True, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device=args.device, + device=args.device, render_cfg=RenderCfg( renderer=args.renderer ), # Enable ray tracing for better visuals + physics_cfg=physics_cfg_for_backend(args.physics), visualization=visualization_cfg_from_args(args), ) @@ -65,11 +70,15 @@ def main(): uid="cube1", shape=CubeCfg(size=[0.1, 0.1, 0.1]), body_type="kinematic", - attrs=RigidBodyAttributesCfg( - mass=1.0, - dynamic_friction=0.5, - static_friction=0.5, - restitution=0.1, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 1.0}, + "material_props": { + "dynamic_friction": 0.5, + "static_friction": 0.5, + "restitution": 0.1, + }, + } ), init_pos=[0.0, 0.0, 1.0], ) @@ -79,15 +88,20 @@ def main(): uid="cube2", shape=CubeCfg(size=[0.1, 0.1, 0.1]), body_type="kinematic", - attrs=RigidBodyAttributesCfg( - mass=1.0, - dynamic_friction=0.5, - static_friction=0.5, - restitution=0.1, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 1.0}, + "material_props": { + "dynamic_friction": 0.5, + "static_friction": 0.5, + "restitution": 0.1, + }, + } ), init_pos=[0.3, 0.0, 1.0], ) ) + sim.prepare() native_window_opened = False if not args.headless: @@ -129,9 +143,6 @@ def main(): def run_simulation(sim: SimulationManager): """Run the simulation loop.""" - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - step_count = 0 gizmo_enabled = True try: diff --git a/examples/sim/gizmo/gizmo_robot.py b/examples/sim/gizmo/gizmo_robot.py index 2c7034364..885ae43d8 100644 --- a/examples/sim/gizmo/gizmo_robot.py +++ b/examples/sim/gizmo/gizmo_robot.py @@ -31,6 +31,7 @@ ) from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, RobotCfg, URDFCfg, JointDrivePropertiesCfg, @@ -62,8 +63,9 @@ def main(): height=1080, headless=True, physics_dt=1.0 / 100.0, - sim_device=args.device, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), visualization=visualization_cfg_from_args(args), ) @@ -111,7 +113,7 @@ def main(): "hand": ["FINGER[1-2]"], }, solver_cfg=solver_cfg, - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( stiffness={"Joint[0-9]": 1e4, "FINGER[1-2]": 1e2}, damping={"Joint[0-9]": 1e3, "FINGER[1-2]": 1e1}, max_effort={"Joint[0-9]": 1e5, "FINGER[1-2]": 1e3}, @@ -120,8 +122,7 @@ def main(): init_qpos=[0.0, -np.pi / 2, -np.pi / 2, np.pi / 2, -np.pi / 2, 0.0, 0.0, 0.0], ) robot = sim.add_robot(cfg=robot_cfg) - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + sim.prepare() # Set initial joint positions initial_qpos = torch.tensor( diff --git a/examples/sim/motion/planners/curobo_planner.py b/examples/sim/motion/planners/curobo_planner.py index 0600548c4..4bdec5fca 100644 --- a/examples/sim/motion/planners/curobo_planner.py +++ b/examples/sim/motion/planners/curobo_planner.py @@ -29,6 +29,7 @@ python examples/sim/motion/planners/curobo_planner.py --headless python examples/sim/motion/planners/curobo_planner.py --headless --num_envs 4 python examples/sim/motion/planners/curobo_planner.py --headless --device cuda:1 + python examples/sim/motion/planners/curobo_planner.py --headless --physics newton Requirements: an NVIDIA CUDA device and the CUDA-matched cuRobo V2 source package installed in the active environment. Installation instructions: @@ -65,7 +66,11 @@ MotionPolicy, ) from embodichain.data import get_data_path -from embodichain.lab.sim.cfg import RenderCfg, RigidBodyAttributesCfg +from embodichain.lab.sim.cfg import ( + RenderCfg, + RigidBodyPhysicsCfg, + physics_cfg_for_backend, +) from embodichain.lab.sim.objects import RigidObjectCfg, Robot, RigidObject from embodichain.lab.sim.motion.motion_generator import MotionGenCfg, MotionGenerator from embodichain.lab.sim.motion.planners.curobo.curobo_planner import ( @@ -103,9 +108,10 @@ def parse_args() -> argparse.Namespace: ) add_env_launcher_args_to_parser(parser) # This standalone example does not merge a gym config after parsing, so - # override the launcher's ``None`` sentinel with a concrete single-world - # default. - parser.set_defaults(arena_space=2.0, num_envs=1) + # override the launcher's ``None`` sentinels with concrete defaults. cuRobo + # itself requires CUDA; keeping that default explicit avoids passing the + # shared parser's omission sentinel into ``torch.device``. + parser.set_defaults(device="cuda", arena_space=2.0, num_envs=1) # Backward-compatible aliases used by older versions of this example. parser.add_argument( "--step-repeat", @@ -243,6 +249,7 @@ def _build_scene( arena_space: float = 2.0, gpu_id: int = 0, visualization: VisualizationCfg | None = None, + physics: str = "default", ) -> tuple[SimulationManager, Robot, RigidObject, torch.Tensor, str]: """Create the batched robot scene with an identical cuboid in each arena.""" sim = SimulationManager( @@ -253,6 +260,7 @@ def _build_scene( arena_space=arena_space, gpu_id=gpu_id, render_cfg=RenderCfg(renderer=renderer), + physics_cfg=physics_cfg_for_backend(physics), visualization=visualization or VisualizationCfg(), ) ) @@ -321,7 +329,7 @@ def _build_scene( "LEFT_HAND_PINKY", ], }, - "drive_pros": { + "joint_drive_props": { "stiffness": {"LEFT_[A-Z|_]+[0-9]?": 1e2}, "damping": {"LEFT_[A-Z|_]+[0-9]?": 1e1}, "max_effort": {"LEFT_[A-Z|_]+[0-9]?": 1e3}, @@ -457,11 +465,6 @@ def _build_scene( if robot is None: raise RuntimeError(f"Failed to add robot '{robot_type}' to the cuRobo demo.") target_xpos = _resolve_batched_target(target_xpos, robot.num_instances) - if robot_type == "w1": - # Keep the W1-specific IK diagnostic batched so it remains useful when - # checking solver and cuRobo reachability across multiple environments. - is_success, ik_qpos = robot.compute_ik(pose=target_xpos, name=control_part) - print(f"robot compute ik success: {is_success}, ik_qpos: {ik_qpos}") # This object is also exported into the cuRobo collision world below via # CuroboWorldCfg.rigid_objects, so the simulator and planner share geometry @@ -470,12 +473,21 @@ def _build_scene( cfg=RigidObjectCfg( uid="demo_block", shape=CubeCfg(size=demo_block_size), - attrs=RigidBodyAttributesCfg(), + # The grouped form is backend-neutral; the deprecated flat attrs + # configuration cannot be spawned by Newton. + attrs=RigidBodyPhysicsCfg(), body_type="kinematic", init_pos=demo_block_position, init_rot=(0.0, 0.0, 0.0), ) ) + sim.prepare() + + if robot_type == "w1": + # Keep the W1-specific IK diagnostic batched so it remains useful when + # checking solver and cuRobo reachability across multiple environments. + is_success, ik_qpos = robot.compute_ik(pose=target_xpos, name=control_part) + print(f"robot compute ik success: {is_success}, ik_qpos: {ik_qpos}") return sim, robot, demo_block, target_xpos, control_part @@ -697,9 +709,8 @@ def main() -> None: args.arena_space, effective_gpu_id, visualization_cfg_from_args(args), + physics=args.physics, ) - if sim.is_use_gpu_physics: - sim.init_gpu_physics() obstacles = [demo_block] obstacle_poses = _perturb_obstacles( diff --git a/examples/sim/motion/planners/neural_planner.py b/examples/sim/motion/planners/neural_planner.py index 0d00a3820..4f9c5bf17 100644 --- a/examples/sim/motion/planners/neural_planner.py +++ b/examples/sim/motion/planners/neural_planner.py @@ -34,7 +34,7 @@ from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.visualization import visualization_cfg_from_args -from embodichain.lab.sim.cfg import MarkerCfg, RenderCfg +from embodichain.lab.sim.cfg import MarkerCfg, RenderCfg, physics_cfg_for_backend from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.robots.franka_panda import FrankaPandaCfg from embodichain.lab.sim.motion.motion_generator import ( @@ -205,11 +205,12 @@ def main() -> None: sim = SimulationManager( SimulationManagerCfg( headless=args.headless, - sim_device=sim_device, + device=sim_device, num_envs=args.num_envs, arena_space=args.arena_space, gpu_id=effective_gpu_id, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), visualization=visualization_cfg_from_args(args), ) ) @@ -218,8 +219,7 @@ def main() -> None: arm_name = "arm" device = robot.device - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + sim.prepare() if not args.headless: sim.open_window() diff --git a/examples/sim/motion/solvers/differential_solver.py b/examples/sim/motion/solvers/differential_solver.py index d97b672c3..410edac92 100644 --- a/examples/sim/motion/solvers/differential_solver.py +++ b/examples/sim/motion/solvers/differential_solver.py @@ -43,11 +43,11 @@ def main( torch.set_printoptions(precision=5, sci_mode=False) # Set up simulation with specified device (CPU or CUDA) - sim_device = "cpu" + device = "cpu" num_envs = 9 # Number of parallel arenas/environments config = SimulationManagerCfg( headless=False, - sim_device=sim_device, + device=device, arena_space=1.5, num_envs=num_envs, visualization=visualization or VisualizationCfg(), @@ -81,6 +81,7 @@ def main( } robot: Robot = sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) + sim.prepare() # Prepare initial joint positions for all environments rad = torch.deg2rad(torch.tensor(45.0)) diff --git a/examples/sim/motion/solvers/neural_ik_solver.py b/examples/sim/motion/solvers/neural_ik_solver.py index 069108163..f6bf7b9b7 100644 --- a/examples/sim/motion/solvers/neural_ik_solver.py +++ b/examples/sim/motion/solvers/neural_ik_solver.py @@ -94,12 +94,12 @@ def main() -> None: np.set_printoptions(precision=5, suppress=True) torch.set_printoptions(precision=5, sci_mode=False) - sim_device = _resolve_device(args.device) + device = _resolve_device(args.device) num_envs = args.num_envs config = SimulationManagerCfg( headless=True, - sim_device=sim_device, + device=device, num_envs=num_envs, arena_space=2.0, visualization=visualization_cfg_from_args(args), @@ -128,6 +128,7 @@ def main() -> None: ) robot: Robot = sim.add_robot(cfg=cfg) + sim.prepare() sim.open_window() @@ -182,7 +183,7 @@ def main() -> None: ik_success_flags: list[torch.Tensor] = [] print( - f"\nRunning {num_steps} batch IK steps: num_envs={num_envs}, device='{sim_device}' ..." + f"\nRunning {num_steps} batch IK steps: num_envs={num_envs}, device='{device}' ..." ) ik_compute_begin = time.time() for step in range(num_steps): diff --git a/examples/sim/motion/solvers/opw_solver.py b/examples/sim/motion/solvers/opw_solver.py index f7cf72f35..723974e0d 100644 --- a/examples/sim/motion/solvers/opw_solver.py +++ b/examples/sim/motion/solvers/opw_solver.py @@ -40,10 +40,10 @@ def main(visualization: VisualizationCfg | None = None) -> None: torch.set_printoptions(precision=5, sci_mode=False) # Initialize simulation - sim_device = "cpu" + device = "cpu" config = SimulationManagerCfg( headless=False, - sim_device=sim_device, + device=device, visualization=visualization or VisualizationCfg(), ) sim = SimulationManager(config) @@ -88,6 +88,7 @@ def main(visualization: VisualizationCfg | None = None) -> None: # Add robot to simulation robot: Robot = sim.add_robot(cfg=CobotMagicCfg.from_dict(cfg_dict)) + sim.prepare() # Left arm control arm_name = "left_arm" diff --git a/examples/sim/motion/solvers/pink_solver.py b/examples/sim/motion/solvers/pink_solver.py index 931447008..f4c20469f 100644 --- a/examples/sim/motion/solvers/pink_solver.py +++ b/examples/sim/motion/solvers/pink_solver.py @@ -40,10 +40,10 @@ def main(visualization: VisualizationCfg | None = None) -> None: torch.set_printoptions(precision=5, sci_mode=False) # Set up simulation with specified device (CPU or CUDA) - sim_device = "cpu" + device = "cpu" config = SimulationManagerCfg( headless=False, - sim_device=sim_device, + device=device, visualization=visualization or VisualizationCfg(), ) sim = SimulationManager(config) @@ -75,6 +75,7 @@ def main(visualization: VisualizationCfg | None = None) -> None: } robot: Robot = sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) + sim.prepare() # Define a sample target pose as a 1x4x4 homogeneous matrix rad = torch.deg2rad(torch.tensor(45.0)) diff --git a/examples/sim/motion/solvers/pinocchio_solver.py b/examples/sim/motion/solvers/pinocchio_solver.py index ceef5739e..4706f31d5 100644 --- a/examples/sim/motion/solvers/pinocchio_solver.py +++ b/examples/sim/motion/solvers/pinocchio_solver.py @@ -41,10 +41,10 @@ def main(visualization: VisualizationCfg | None = None) -> None: torch.set_printoptions(precision=5, sci_mode=False) # Initialize simulation - sim_device = "cpu" + device = "cpu" config = SimulationManagerCfg( headless=False, - sim_device=sim_device, + device=device, visualization=visualization or VisualizationCfg(), ) sim = SimulationManager(config) @@ -75,6 +75,7 @@ def main(visualization: VisualizationCfg | None = None) -> None: } robot: Robot = sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) + sim.prepare() arm_name = "left_arm" # Set initial joint positions for left arm qpos_seed = torch.tensor( diff --git a/examples/sim/motion/solvers/pytorch_solver.py b/examples/sim/motion/solvers/pytorch_solver.py index ccca00bb1..55b396169 100644 --- a/examples/sim/motion/solvers/pytorch_solver.py +++ b/examples/sim/motion/solvers/pytorch_solver.py @@ -41,11 +41,11 @@ def main(visualization: VisualizationCfg | None = None) -> None: torch.set_printoptions(precision=5, sci_mode=False) # Initialize simulation environment (CPU or CUDA) - sim_device = "cpu" + device = "cpu" num_envs = 9 # Number of parallel environments config = SimulationManagerCfg( headless=False, - sim_device=sim_device, + device=device, arena_space=2.0, num_envs=num_envs, visualization=visualization or VisualizationCfg(), @@ -81,6 +81,7 @@ def main(visualization: VisualizationCfg | None = None) -> None: # Add robot to simulation robot: Robot = sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) + sim.prepare() # Prepare initial joint positions for all environments arm_name = "left_arm" diff --git a/examples/sim/motion/solvers/srs_solver.py b/examples/sim/motion/solvers/srs_solver.py index 01ddaace1..691ac3286 100644 --- a/examples/sim/motion/solvers/srs_solver.py +++ b/examples/sim/motion/solvers/srs_solver.py @@ -39,11 +39,11 @@ def main(visualization: VisualizationCfg | None = None) -> None: torch.set_printoptions(precision=5, sci_mode=False) # Initialize simulation - sim_device = "cpu" + device = "cpu" sim = SimulationManager( SimulationManagerCfg( headless=False, - sim_device=sim_device, + device=device, width=2200, height=1200, visualization=visualization or VisualizationCfg(), @@ -51,6 +51,7 @@ def main(visualization: VisualizationCfg | None = None) -> None: ) robot: Robot = sim.add_robot(cfg=DexforceW1Cfg.from_dict({"uid": "dexforce_w1"})) + sim.prepare() arm_name = "left_arm" # Set initial joint positions for left arm qpos_fk_list = [ diff --git a/examples/sim/motion/workspace/analyze_cartesian_workspace.py b/examples/sim/motion/workspace/analyze_cartesian_workspace.py index 4d2afd3da..6786a5665 100644 --- a/examples/sim/motion/workspace/analyze_cartesian_workspace.py +++ b/examples/sim/motion/workspace/analyze_cartesian_workspace.py @@ -101,6 +101,7 @@ def main() -> None: } ) robot = sim.add_robot(cfg=cfg) + sim.prepare() print("DexforceW1 robot added to the simulation.") left_qpos = torch.tensor( diff --git a/examples/sim/motion/workspace/analyze_joint_workspace.py b/examples/sim/motion/workspace/analyze_joint_workspace.py index 6b499f22a..65c5247d7 100644 --- a/examples/sim/motion/workspace/analyze_joint_workspace.py +++ b/examples/sim/motion/workspace/analyze_joint_workspace.py @@ -98,6 +98,7 @@ def main() -> None: } ) robot = sim_manager.add_robot(cfg=cfg) + sim_manager.prepare() print("DexforceW1 robot added to the simulation.") analyzer = WorkspaceAnalyzer( diff --git a/examples/sim/motion/workspace/analyze_plane_workspace.py b/examples/sim/motion/workspace/analyze_plane_workspace.py index e8befba62..5718d18c1 100644 --- a/examples/sim/motion/workspace/analyze_plane_workspace.py +++ b/examples/sim/motion/workspace/analyze_plane_workspace.py @@ -101,6 +101,7 @@ def main() -> None: } ) robot = sim.add_robot(cfg=cfg) + sim.prepare() print("DexforceW1 robot added to the simulation.") left_qpos = torch.tensor( diff --git a/examples/sim/robot/dexforce_w1.py b/examples/sim/robot/dexforce_w1.py index 9a4e78383..37a71c5d8 100644 --- a/examples/sim/robot/dexforce_w1.py +++ b/examples/sim/robot/dexforce_w1.py @@ -60,7 +60,7 @@ def main(visualization: VisualizationCfg | None = None) -> None: }, ] }, - "drive_pros": { + "joint_drive_props": { "max_effort": { "left_eef": 10.0, "right_eef": 10.0, @@ -70,6 +70,7 @@ def main(visualization: VisualizationCfg | None = None) -> None: ) robot = sim.add_robot(cfg=cfg) + sim.prepare() sim.update(step=1) print("DexforceW1 with a user defined end-effector added to the simulation.") diff --git a/examples/sim/scene/scene_demo.py b/examples/sim/scene/scene_demo.py index aeb39a3a5..a260a2b6b 100644 --- a/examples/sim/scene/scene_demo.py +++ b/examples/sim/scene/scene_demo.py @@ -29,7 +29,8 @@ from embodichain.lab.visualization import visualization_cfg_from_args from embodichain.lab.sim.cfg import ( RenderCfg, - RigidBodyAttributesCfg, + physics_cfg_for_backend, + RigidBodyPhysicsCfg, LightCfg, RobotCfg, URDFCfg, @@ -77,9 +78,6 @@ def resolve_asset_path(scene_name: str) -> str: def run_simulation(sim: SimulationManager): """Run the simulation loop.""" - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - try: while True: time.sleep(0.01) @@ -119,8 +117,9 @@ def main(): height=1080, headless=True, physics_dt=1.0 / 100.0, - sim_device=args.device, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), num_envs=args.num_envs, arena_space=10.0, visualization=visualization_cfg_from_args(args), @@ -142,11 +141,15 @@ def main(): cfg = LightCfg(uid=uid, intensity=intensity, radius=600, init_pos=[x, y, z]) lights.append(sim.add_light(cfg)) - physics_attrs = RigidBodyAttributesCfg( - mass=10, - dynamic_friction=0.5, - static_friction=0.5, - restitution=0.1, + physics_attrs = RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 10}, + "material_props": { + "dynamic_friction": 0.5, + "static_friction": 0.5, + "restitution": 0.1, + }, + } ) try: @@ -179,6 +182,8 @@ def main(): logger.log_info(f"Failed to load scene asset: {e}") return + sim.prepare() + logger.log_info(f"Scene '{args.scene}' setup complete!") logger.log_info(f"Running simulation with {args.num_envs} environment(s)") logger.log_info("Press Ctrl+C to stop the simulation") diff --git a/examples/sim/sensors/batch_camera.py b/examples/sim/sensors/batch_camera.py index e52b180fc..0af567c7b 100644 --- a/examples/sim/sensors/batch_camera.py +++ b/examples/sim/sensors/batch_camera.py @@ -21,8 +21,13 @@ import matplotlib.pyplot as plt from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + RenderCfg, + physics_cfg_for_backend, + RigidObjectCfg, + LightCfg, +) from embodichain.lab.visualization import visualization_cfg_from_args -from embodichain.lab.sim.cfg import RenderCfg, RigidObjectCfg, LightCfg from embodichain.lab.sim.shapes import MeshCfg from embodichain.lab.sim.objects import RigidObject, Light from embodichain.lab.sim.sensors import ( @@ -38,10 +43,11 @@ def main(args): config = SimulationManagerCfg( headless=True, - sim_device=args.device, + device=args.device, num_envs=args.num_envs, arena_space=2, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), visualization=visualization_cfg_from_args(args), ) sim = SimulationManager(config) @@ -54,8 +60,7 @@ def main(args): ) ) - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + sim.prepare() if not args.headless: sim.open_window() @@ -117,6 +122,8 @@ def main(args): else: plt.show() + sim.destroy() + if __name__ == "__main__": import argparse diff --git a/examples/sim/sensors/create_contact_sensor.py b/examples/sim/sensors/create_contact_sensor.py index e2332d9d1..73f3f6394 100644 --- a/examples/sim/sensors/create_contact_sensor.py +++ b/examples/sim/sensors/create_contact_sensor.py @@ -28,8 +28,12 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.visualization import visualization_cfg_from_args from embodichain.lab.sim.cfg import ( + DefaultRigidBodyPropertiesCfg, + MassPropertiesCfg, RenderCfg, - RigidBodyAttributesCfg, + physics_cfg_for_backend, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, ) from embodichain.lab.sim.sensors import ( ContactSensorCfg, @@ -60,12 +64,14 @@ def create_cube( uid=uid, shape=CubeCfg(size=cube_size), body_type="dynamic", - attrs=RigidBodyAttributesCfg( - mass=0.1, - dynamic_friction=0.9, - static_friction=0.95, - restitution=0.01, - sleep_threshold=0.0, + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=0.1), + rigid_props=DefaultRigidBodyPropertiesCfg(sleep_threshold=0.0), + material_props=RigidBodyMaterialCfg( + dynamic_friction=0.9, + static_friction=0.95, + restitution=0.01, + ), ), init_pos=position, ) @@ -151,10 +157,10 @@ def create_robot( }, "init_pos": position, "init_qpos": [0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], - "drive_pros": { - "stiffness": {"JOINT[1-6]": 1e4, "FINGER[1-2]_JOINT": 1e2}, - "damping": {"JOINT[1-6]": 1e3, "FINGER[1-2]_JOINT": 1e1}, - "max_effort": {"JOINT[1-6]": 1e5, "FINGER[1-2]_JOINT": 1e3}, + "joint_drive_props": { + "stiffness": {"Joint[1-6]": 1e4, "finger[1-2]_joint": 1e2}, + "damping": {"Joint[1-6]": 1e3, "finger[1-2]_joint": 1e1}, + "max_effort": {"Joint[1-6]": 1e5, "finger[1-2]_joint": 1e3}, }, "solver_cfg": { "arm": { @@ -169,7 +175,7 @@ def create_robot( ], } }, - "control_parts": {"arm": ["JOINT[1-6]"], "hand": ["FINGER[1-2]_JOINT"]}, + "control_parts": {"arm": ["Joint[1-6]"], "hand": ["finger[1-2]_joint"]}, } robot: Robot = sim.add_robot(cfg=RobotCfg.from_dict(robot_cfg_dict)) return robot @@ -192,10 +198,11 @@ def main(): num_envs=args.num_envs, headless=True, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device=args.device, + device=args.device, render_cfg=RenderCfg( renderer=args.renderer ), # Enable ray tracing for better visuals + physics_cfg=physics_cfg_for_backend(args.physics), visualization=visualization_cfg_from_args(args), ) @@ -207,6 +214,7 @@ def main(): cube1 = create_cube(sim, "cube1", position=[0.0, 0.0, 0.06]) cube2 = create_cube(sim, "cube2", position=[0.0, 0.0, 0.09]) robot = create_robot(sim, "UR10_PGI", position=[0.5, 0.0, 0.0]) + sim.prepare() print("[INFO]: Scene setup complete!") print(f"[INFO]: Running simulation with {args.num_envs} environment(s)") @@ -228,10 +236,6 @@ def run_simulation(sim: SimulationManager): sim: The SimulationManager instance to run """ - # Initialize GPU physics if using CUDA - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - step_count = 0 # contact filter config contact_filter_cfg = ContactSensorCfg() @@ -262,13 +266,22 @@ def run_simulation(sim: SimulationManager): f"[INFO]: Fetch contact cost time: {average_cost_time * 1000:.2f} ms, num_envs: {sim.num_envs}" ) # filter contact report for a rigid object with a articulation link - cube2_user_ids = sim.get_rigid_object("cube2").get_user_ids() - finger1_user_ids = ( - sim.get_robot("UR10_PGI").get_user_ids("finger1_link").reshape(-1) + assert contact_sensor.item_user_ids is not None + filter_actor_ids = torch.as_tensor( + [ + actor_id + for actor_id in contact_sensor.item_user_ids.tolist() + if contact_sensor.get_actor_info(actor_id).path.endswith( + "/cube2" + ) + or contact_sensor.get_actor_info(actor_id).link_name + == "finger1_link" + ], + dtype=torch.int32, + device=sim.device, ) - filter_user_ids = torch.cat([cube2_user_ids, finger1_user_ids]) filter_contact_report = contact_sensor.filter_by_user_ids( - filter_user_ids + filter_actor_ids ) # print("filter_contact_report", filter_contact_report) # visualize contact points diff --git a/scripts/benchmark/atomic_action/common.py b/scripts/benchmark/atomic_action/common.py index a7b7e6db9..bfd726ca3 100644 --- a/scripts/benchmark/atomic_action/common.py +++ b/scripts/benchmark/atomic_action/common.py @@ -29,7 +29,7 @@ from dataclasses import dataclass from datetime import datetime from pathlib import Path -from typing import Callable +from typing import Callable, Literal try: import psutil @@ -82,7 +82,7 @@ class MeshObjectPreset: mesh_path: str = "" shape_type: str = "mesh" cube_size: tuple[float, float, float] | None = None - use_usd_properties: bool = False + asset_physics_mode: Literal["preserve", "overlay"] = "overlay" dynamic_friction: float = 0.97 static_friction: float = 0.99 restitution: float = 0.0 @@ -95,7 +95,10 @@ class MeshObjectPreset: min_velocity_iters: int = 1 max_linear_velocity: float = 100.0 max_angular_velocity: float = 100.0 - max_convex_hull_num: int = 16 + collision_approximation: Literal["convex_hull", "convex_decomposition"] = ( + "convex_decomposition" + ) + max_hulls: int | None = 16 enable_ccd: bool = False @@ -123,7 +126,7 @@ class MeshObjectPreset: body_scale=(0.8, 0.8, 0.8), mass=0.05, initial_z=0.05, - use_usd_properties=False, + asset_physics_mode="overlay", ), "coffee_cup": MeshObjectPreset( object_type="coffee_cup", @@ -134,7 +137,7 @@ class MeshObjectPreset: body_scale=(4.0, 4.0, 4.0), mass=0.01, initial_z=0.01, - use_usd_properties=False, + asset_physics_mode="overlay", ), "cube": MeshObjectPreset( object_type="cube", @@ -146,7 +149,7 @@ class MeshObjectPreset: body_scale=(1.0, 1.0, 1.0), mass=0.05, initial_z=0.05, - use_usd_properties=False, + asset_physics_mode="overlay", dynamic_friction=0.5, static_friction=0.5, contact_offset=0.003, @@ -154,7 +157,8 @@ class MeshObjectPreset: max_depenetration_velocity=10.0, min_position_iters=32, min_velocity_iters=8, - max_convex_hull_num=1, + collision_approximation="convex_hull", + max_hulls=None, ), "paper_cup": MeshObjectPreset( object_type="paper_cup", @@ -165,7 +169,7 @@ class MeshObjectPreset: body_scale=(0.75, 0.75, 1.0), mass=0.01, initial_z=0.05, - use_usd_properties=False, + asset_physics_mode="overlay", dynamic_friction=1.0, static_friction=1.0, contact_offset=0.003, @@ -177,7 +181,7 @@ class MeshObjectPreset: min_velocity_iters=8, max_linear_velocity=5.0, max_angular_velocity=10.0, - max_convex_hull_num=8, + max_hulls=8, ), "scanned_bottle": MeshObjectPreset( object_type="scanned_bottle", @@ -188,7 +192,7 @@ class MeshObjectPreset: body_scale=(1.0, 1.0, 1.0), mass=0.05, initial_z=0.05, - use_usd_properties=False, + asset_physics_mode="overlay", ), } COVERAGE_MESH_OBJECT_TYPES = ("sugar_box", "cube", "paper_cup") @@ -518,11 +522,17 @@ def create_benchmark_object( ): """Create one benchmark object at a selected initial position.""" from embodichain.data import get_data_path - from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RigidObjectCfg - from embodichain.lab.sim.shapes import CubeCfg, MeshCfg + from embodichain.lab.sim.cfg import RigidBodyPhysicsCfg, RigidObjectCfg + from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, MeshCollisionCfg if preset.shape_type == "mesh": - shape = MeshCfg(fpath=get_data_path(preset.mesh_path)) + shape = MeshCfg( + fpath=get_data_path(preset.mesh_path), + collision=MeshCollisionCfg( + approximation=preset.collision_approximation, + max_hulls=preset.max_hulls, + ), + ) elif preset.shape_type == "cube": if preset.cube_size is None: raise ValueError(f"Cube preset {preset.object_type!r} misses cube_size.") @@ -535,27 +545,34 @@ def create_benchmark_object( cfg = RigidObjectCfg( uid=f"benchmark_{preset.label}_{position_case.name}_{uid_suffix}", shape=shape, - attrs=RigidBodyAttributesCfg( - mass=preset.mass, - dynamic_friction=preset.dynamic_friction, - static_friction=preset.static_friction, - restitution=preset.restitution, - contact_offset=preset.contact_offset, - rest_offset=preset.rest_offset, - linear_damping=preset.linear_damping, - angular_damping=preset.angular_damping, - max_depenetration_velocity=preset.max_depenetration_velocity, - min_position_iters=preset.min_position_iters, - min_velocity_iters=preset.min_velocity_iters, - max_linear_velocity=preset.max_linear_velocity, - max_angular_velocity=preset.max_angular_velocity, - enable_ccd=preset.enable_ccd, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": preset.mass}, + "rigid_props": { + "linear_damping": preset.linear_damping, + "angular_damping": preset.angular_damping, + "max_depenetration_velocity": preset.max_depenetration_velocity, + "min_position_iters": preset.min_position_iters, + "min_velocity_iters": preset.min_velocity_iters, + "max_linear_velocity": preset.max_linear_velocity, + "max_angular_velocity": preset.max_angular_velocity, + "enable_ccd": preset.enable_ccd, + }, + "collision_props": { + "contact_offset": preset.contact_offset, + "rest_offset": preset.rest_offset, + }, + "material_props": { + "dynamic_friction": preset.dynamic_friction, + "static_friction": preset.static_friction, + "restitution": preset.restitution, + }, + } ), - max_convex_hull_num=preset.max_convex_hull_num, init_pos=[position_case.xy[0], position_case.xy[1], preset.initial_z], init_rot=preset.init_rot, body_scale=preset.body_scale, - use_usd_properties=preset.use_usd_properties, + asset_physics_mode=preset.asset_physics_mode, ) obj = sim.add_rigid_object(cfg=cfg) sim.update(step=10) diff --git a/scripts/benchmark/data_pipeline/benchmark_lerobot_save.py b/scripts/benchmark/data_pipeline/benchmark_lerobot_save.py index 576eb9c80..f309b2daf 100644 --- a/scripts/benchmark/data_pipeline/benchmark_lerobot_save.py +++ b/scripts/benchmark/data_pipeline/benchmark_lerobot_save.py @@ -143,7 +143,11 @@ def _run_child(args: argparse.Namespace) -> int: shutil.rmtree(save_root / args.variant, ignore_errors=True) parser = argparse.ArgumentParser() - add_env_launcher_args_to_parser(parser) + # This child consumes a Gym config, so let the file own the physics + # backend and its device default. The shared standalone-parser defaults + # (``physics=default``/``renderer=auto``) would otherwise be interpreted + # as explicit overrides before a Newton config is decoded. + add_env_launcher_args_to_parser(parser, require_gym_config=True) launcher_args = parser.parse_args(["--gym_config", args.gym_config, "--headless"]) env_cfg, gcfg, action_config = build_env_cfg_from_args( launcher_args, diff --git a/scripts/benchmark/rl/runtime.py b/scripts/benchmark/rl/runtime.py index 216d98d7c..20ea70b55 100644 --- a/scripts/benchmark/rl/runtime.py +++ b/scripts/benchmark/rl/runtime.py @@ -111,7 +111,7 @@ def _build_env_cfg( gym_env_cfg.seed = getattr(gym_env_cfg, "seed", None) gym_env_cfg.sim_cfg.headless = headless gym_env_cfg.sim_cfg.gpu_id = gpu_id - gym_env_cfg.sim_cfg.sim_device = device + gym_env_cfg.sim_cfg.device = device return gym_config_data, gym_env_cfg diff --git a/scripts/benchmark/task_program/demo_success.py b/scripts/benchmark/task_program/demo_success.py index 3e553cd31..ff9f0c541 100644 --- a/scripts/benchmark/task_program/demo_success.py +++ b/scripts/benchmark/task_program/demo_success.py @@ -1247,6 +1247,10 @@ def _build_parser() -> argparse.ArgumentParser: parser.set_defaults( num_envs=None, renderer=None, + # A live benchmark loads a file-owned Gym backend. Keep this unset so + # the shared launcher does not accidentally request ``default`` and + # conflict with a Newton configuration before the file is parsed. + physics=None, viser_image_fps=None, ) parser.add_argument( diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index c664c90fe..141f0982d 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -48,7 +48,7 @@ MotionPolicy, SceneEntityPose, ) -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.data import get_data_path from embodichain.lab.sim.objects import RigidObject, Robot from embodichain.lab.sim.shapes import CubeCfg, MeshCfg @@ -66,6 +66,7 @@ create_curobo_motion_generator, create_parallel_jaw_grasp_pose_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, draw_axis_marker, get_hand_open_close_qpos, @@ -108,6 +109,8 @@ ) HAND_CLOSE_QPOS = 0.026 +PICKUP_OBJECT_PART = "center" +"""Central body grasp keeps the horizontally staged can symmetric in the jaws.""" PICKUP_SAMPLE_INTERVAL = 80 PICKUP_HAND_INTERP_STEPS = 5 PICKUP_PRE_GRASP_DISTANCE = 0.08 @@ -170,8 +173,11 @@ def create_assemble_object(sim: SimulationManager) -> RigidObject: return sim.add_rigid_object( cfg=RigidObjectCfg( uid="assemble_object", - shape=MeshCfg(fpath=OBJECT_MESH_PATH, compute_uv=False), - attrs=RigidBodyAttributesCfg( + shape=MeshCfg( + fpath=OBJECT_MESH_PATH, + compute_uv=False, + ), + attrs=create_tutorial_rigid_body_physics( mass=0.01, dynamic_friction=0.97, static_friction=0.99, @@ -183,8 +189,8 @@ def create_assemble_object(sim: SimulationManager) -> RigidObject: min_position_iters=32, min_velocity_iters=8, max_depenetration_velocity=2.0, + newton_contact=sim.is_newton_backend, ), - max_convex_hull_num=1, init_pos=[ OBJECT_A_XY[0], OBJECT_A_XY[1], @@ -202,7 +208,7 @@ def create_base_object(sim: SimulationManager) -> RigidObject: cfg=RigidObjectCfg( uid="base_object", shape=CubeCfg(size=[CUBE_SIZE, CUBE_SIZE, CUBE_SIZE]), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=1.0, dynamic_friction=0.9, static_friction=0.95, @@ -222,15 +228,20 @@ def create_base_object(sim: SimulationManager) -> RigidObject: def compute_can_half_height(can: RigidObject) -> float: """Return half the soda-can extent along world Z when laid on its side.""" vertices = can.get_vertices(env_ids=[0], scale=True)[0].to(torch.float32) - rotated = vertices @ _CAN_INIT_ROTATION.T + rotation = _CAN_INIT_ROTATION.to(device=vertices.device, dtype=vertices.dtype) + rotated = vertices @ rotation.T extent_z = float(rotated[:, 2].max().item() - rotated[:, 2].min().item()) return 0.5 * extent_z -def make_assemble_to_base_pose(dz: float) -> torch.Tensor: +def make_assemble_to_base_pose( + dz: float, + *, + device: torch.device | str | None = None, +) -> torch.Tensor: """Build the can pose relative to the cube: above it, same orientation.""" - pose = torch.eye(4, dtype=torch.float32) - pose[:3, :3] = _CAN_INIT_ROTATION + pose = torch.eye(4, dtype=torch.float32, device=device) + pose[:3, :3] = _CAN_INIT_ROTATION.to(device=pose.device, dtype=pose.dtype) pose[2, 3] = dz return pose @@ -244,6 +255,7 @@ def run_assemble_demo( create_support_surface(sim) can = create_assemble_object(sim) cube = create_base_object(sim) + sim.prepare() settle_object(sim, can, step=0) clone_local_pose_from_first_env(can) @@ -257,16 +269,20 @@ def run_assemble_demo( can, label="soda_can", ) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + use_cuda_graph=args.physics != "newton", + ) left_open, left_close = get_hand_open_close_qpos( robot, hand_control_part="left_hand", close_qpos=HAND_CLOSE_QPOS ) + cube_pose = cube.get_local_pose(to_matrix=True) can_half_z = compute_can_half_height(can) assemble_to_base = make_assemble_to_base_pose( - 0.5 * CUBE_SIZE + can_half_z + ASSEMBLE_MARGIN + 0.5 * CUBE_SIZE + can_half_z + ASSEMBLE_MARGIN, + device=cube_pose.device, ) - cube_pose = cube.get_local_pose(to_matrix=True) assemble_object_target_pose = cube_pose[0] @ assemble_to_base num_envs = robot.get_qpos().shape[0] @@ -277,9 +293,9 @@ def run_assemble_demo( broadcast_pose_batch(assemble_object_target_pose, num_envs), ) - # Step 1 - the left arm picks the soda can up by its top part. + # Step 1 - the left arm picks the soda can up at its central body. pick_up_options = PickUpOptions( - pick_object_part="top", + pick_object_part=PICKUP_OBJECT_PART, pre_grasp_distance=PICKUP_PRE_GRASP_DISTANCE, lift_height=PICKUP_LIFT_HEIGHT, hand_interp_steps=PICKUP_HAND_INTERP_STEPS, diff --git a/scripts/tutorials/atomic_action/axis_align.py b/scripts/tutorials/atomic_action/axis_align.py index 8685f3f6c..523372905 100644 --- a/scripts/tutorials/atomic_action/axis_align.py +++ b/scripts/tutorials/atomic_action/axis_align.py @@ -37,7 +37,7 @@ MotionPolicy, ObjectSemantics, ) -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.lab.sim.objects import RigidObject from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger @@ -48,6 +48,7 @@ create_parallel_jaw_grasp_pose_generator, create_toppra_motion_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, draw_axis_marker, get_hand_open_close_qpos, @@ -105,12 +106,12 @@ def create_align_object( cfg=RigidObjectCfg( uid="cube", shape=CubeCfg(size=list(OBJECT_SIZE)), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=0.05, dynamic_friction=0.97, static_friction=0.99, + newton_contact=sim.is_newton_backend, ), - max_convex_hull_num=16, init_pos=init_pos, ) ) diff --git a/scripts/tutorials/atomic_action/control_dt.py b/scripts/tutorials/atomic_action/control_dt.py index 617567cdc..b1b33eb91 100644 --- a/scripts/tutorials/atomic_action/control_dt.py +++ b/scripts/tutorials/atomic_action/control_dt.py @@ -64,6 +64,7 @@ def main() -> None: args = parse_arguments() sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) + sim.prepare() engine = AtomicActionEngine(motion_generator=create_toppra_motion_generator(robot)) initial_qpos = robot.get_qpos().clone() diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index b3ea01d1f..7bb5446cc 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -43,12 +43,9 @@ CoordinatedPickmentOptions, MotionPolicy, ) -from embodichain.lab.sim.cfg import ( - RigidBodyAttributesCfg, - RigidObjectCfg, -) +from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.lab.sim.objects import RigidObject, Robot -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.utils import logger from embodichain.utils.math import matrix_from_euler from scripts.tutorials.atomic_action.scenario_utils import ( @@ -69,6 +66,7 @@ create_parallel_jaw_grasp_pose_generator, create_toppra_motion_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, draw_axis_marker, format_tensor, @@ -177,7 +175,6 @@ def parse_arguments() -> argparse.Namespace: "headless_play", "visualize_axes", ), - default_device="cpu", default_renderer="hybrid", ) parser.add_argument( @@ -222,9 +219,14 @@ def create_pickment_object( cfg=RigidObjectCfg( uid=preset.label, shape=MeshCfg( - fpath=resolve_cached_data_path(preset.mesh_path), compute_uv=False + fpath=resolve_cached_data_path(preset.mesh_path), + compute_uv=False, + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=16, + ), ), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=0.01, dynamic_friction=0.97, static_friction=0.99, @@ -236,13 +238,14 @@ def create_pickment_object( min_position_iters=32, min_velocity_iters=8, max_depenetration_velocity=2.0, + newton_contact=sim.is_newton_backend, ), - max_convex_hull_num=16, init_pos=[preset.init_xy[0], preset.init_xy[1], SUPPORT_SURFACE_Z], init_rot=list(preset.init_rot), body_scale=preset.body_scale, ) ) + sim.prepare() obj.cfg.init_pos = compute_supported_init_pos(obj, preset) obj.reset() return obj diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index e78034e5b..eab62b7c1 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -49,12 +49,9 @@ MotionPolicy, TaskState, ) -from embodichain.lab.sim.cfg import ( - RigidBodyAttributesCfg, - RigidObjectCfg, -) +from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.lab.sim.objects import RigidObject, Robot -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.scenario_utils import ( add_dual_tutorial_robot, @@ -76,6 +73,7 @@ clone_local_pose_from_first_env, create_toppra_motion_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, draw_axis_marker, format_tensor, @@ -231,7 +229,7 @@ def create_table(sim: SimulationManager) -> RigidObject: cfg=RigidObjectCfg( uid="table", shape=MeshCfg(fpath=resolve_cached_data_path(TABLE_MESH_PATH)), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=10.0, dynamic_friction=0.9, static_friction=0.95, @@ -250,9 +248,14 @@ def create_bread(sim: SimulationManager) -> RigidObject: cfg=RigidObjectCfg( uid="bread", shape=MeshCfg( - fpath=resolve_cached_data_path(BREAD_MESH_PATH), compute_uv=False + fpath=resolve_cached_data_path(BREAD_MESH_PATH), + compute_uv=False, + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=8, + ), ), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=0.01, contact_offset=0.003, rest_offset=0.001, @@ -260,9 +263,9 @@ def create_bread(sim: SimulationManager) -> RigidObject: min_position_iters=32, min_velocity_iters=8, max_depenetration_velocity=10.0, + newton_contact=sim.is_newton_backend, ), body_scale=(1.75, 1.75, 1.75), - max_convex_hull_num=8, init_pos=list(BREAD_INIT_POS), init_rot=list(BREAD_INIT_ROT), ) @@ -275,9 +278,14 @@ def create_pan(sim: SimulationManager) -> RigidObject: cfg=RigidObjectCfg( uid="pan", shape=MeshCfg( - fpath=resolve_cached_data_path(PAN_MESH_PATH), compute_uv=False + fpath=resolve_cached_data_path(PAN_MESH_PATH), + compute_uv=False, + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=16, + ), ), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=0.01, dynamic_friction=0.97, static_friction=0.99, @@ -289,9 +297,9 @@ def create_pan(sim: SimulationManager) -> RigidObject: min_position_iters=32, min_velocity_iters=8, max_depenetration_velocity=2.0, + newton_contact=sim.is_newton_backend, ), body_scale=(1.75, 1.75, 1.75), - max_convex_hull_num=16, init_pos=list(PAN_INIT_POS), init_rot=list(PAN_INIT_ROT), ) @@ -535,6 +543,7 @@ def run_coordinated_placement_demo( create_table(sim) bread = create_bread(sim) pan = create_pan(sim) + sim.prepare() settle_object(sim, bread, step=0) settle_object(sim, pan, step=0) bread_pose_batch = clone_local_pose_from_first_env(bread) diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index 3397e1aeb..ee9be41f0 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -49,7 +49,6 @@ TimedCommandSequence, TrackingPolicy, ) -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg, Robot from embodichain.lab.sim.motion.motion_generator import MotionGenCfg, MotionGenerator from embodichain.lab.sim.motion.planners.curobo.curobo_planner import ( @@ -418,12 +417,12 @@ def main() -> None: roughness=0.35, ), ), - attrs=RigidBodyAttributesCfg(), body_type="kinematic", init_pos=list(OBSTACLE_START_POSITION), init_rot=[0.0, 0.0, 0.0], ) ) + sim.prepare() # Initialize GPU physics before planning or recording so the first visible # frame and the initial planning context share the same settled state. sim.update(step=10) @@ -431,6 +430,8 @@ def main() -> None: MotionGenCfg( planner_cfg=CuroboPlannerCfg( robot_uid=robot.uid, + # Newton physics captures CUDA graphs on the same device. + use_cuda_graph=args.physics != "newton", # The coarse default voxel fit under-covers the hand and # fingertips. Keep the denser morphit fit, but no extra radius # padding: 5 mm makes this tutorial's initial pose infeasible. diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index f34d7116b..c91147989 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -36,10 +36,10 @@ HandOverOptions, MotionPolicy, ) -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.data import get_data_path from embodichain.lab.sim.objects import RigidObject, Robot -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.scenario_utils import ( add_dual_tutorial_robot, @@ -52,6 +52,7 @@ create_parallel_jaw_grasp_pose_generator, create_toppra_motion_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, get_hand_open_close_qpos, clone_local_pose_from_first_env, @@ -142,19 +143,28 @@ def create_support_surface(sim: SimulationManager) -> RigidObject: ) -def create_handover_object(sim: SimulationManager, args) -> RigidObject: +def create_handover_object( + sim: SimulationManager, + args: argparse.Namespace | None = None, +) -> RigidObject: """Create the mode-specific mesh object on the support surface.""" + is_horizontal = bool(getattr(args, "is_horizontal", False)) mesh_path = ( - HORIZONTAL_OBJECT_MESH_PATH if args.is_horizontal else VERTICAL_OBJECT_MESH_PATH - ) - body_scale = ( - HORIZONTAL_OBJECT_SCALE if args.is_horizontal else VERTICAL_OBJECT_SCALE + HORIZONTAL_OBJECT_MESH_PATH if is_horizontal else VERTICAL_OBJECT_MESH_PATH ) + body_scale = HORIZONTAL_OBJECT_SCALE if is_horizontal else VERTICAL_OBJECT_SCALE return sim.add_rigid_object( cfg=RigidObjectCfg( uid="handover_object", - shape=MeshCfg(fpath=mesh_path, compute_uv=False), - attrs=RigidBodyAttributesCfg( + shape=MeshCfg( + fpath=mesh_path, + compute_uv=False, + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=16, + ), + ), + attrs=create_tutorial_rigid_body_physics( mass=0.01, dynamic_friction=0.97, static_friction=0.99, @@ -166,12 +176,10 @@ def create_handover_object(sim: SimulationManager, args) -> RigidObject: min_position_iters=32, min_velocity_iters=8, max_depenetration_velocity=2.0, + newton_contact=sim.is_newton_backend, ), - max_convex_hull_num=16, init_pos=[OBJECT_INIT_XY[0], OBJECT_INIT_XY[1], SUPPORT_SURFACE_Z + 0.12], - init_rot=( - OBJECT_ROT_VERTICAL if not args.is_horizontal else OBJECT_ROT_HORIZONTAL - ), + init_rot=(OBJECT_ROT_HORIZONTAL if is_horizontal else OBJECT_ROT_VERTICAL), body_scale=body_scale, ) ) @@ -185,6 +193,7 @@ def run_handover_demo( """Plan and optionally execute one unified pick-up and handover.""" create_support_surface(sim) obj = create_handover_object(sim, args) + sim.prepare() settle_object(sim, obj, step=0) clone_local_pose_from_first_env(obj) obj.clear_dynamics() diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index 9993916e1..be696a34b 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -65,6 +65,7 @@ def main() -> None: args = parse_arguments() sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) + sim.prepare() motion_gen = create_curobo_motion_generator(robot) engine = AtomicActionEngine(motion_generator=motion_gen) diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index efc553ac1..4548b81e4 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -38,9 +38,9 @@ MotionPolicy, PickUpOptions, ) -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.lab.sim.objects import RigidObject -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( add_tutorial_robot, @@ -50,6 +50,7 @@ create_curobo_motion_generator, create_parallel_jaw_grasp_pose_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, draw_axis_marker, get_hand_open_close_qpos, @@ -62,6 +63,7 @@ OBJECT_MESH_PATH = "PaperCup/paper_cup.ply" OBJECT_XY = (-0.42, -0.08) +OBJECT_INITIAL_Z = 0.05 MOVE_SAMPLE_INTERVAL = 60 PICK_SAMPLE_INTERVAL = 120 MOVE_HELD_OBJECT_SAMPLE_INTERVAL = 120 @@ -83,17 +85,24 @@ def create_pick_object(sim) -> RigidObject: obj = sim.add_rigid_object( cfg=RigidObjectCfg( uid="paper_cup", - shape=MeshCfg(fpath=get_data_path(OBJECT_MESH_PATH)), - attrs=RigidBodyAttributesCfg( + shape=MeshCfg( + fpath=get_data_path(OBJECT_MESH_PATH), + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=16, + ), + ), + attrs=create_tutorial_rigid_body_physics( mass=0.01, dynamic_friction=0.97, static_friction=0.99, + newton_contact=sim.is_newton_backend, ), - max_convex_hull_num=16, - init_pos=[*OBJECT_XY, 0.0], + init_pos=[*OBJECT_XY, OBJECT_INITIAL_Z], body_scale=(0.75, 0.75, 1.0), ) ) + sim.prepare() sim.update(step=10) clone_local_pose_from_first_env(obj) obj.clear_dynamics() @@ -118,6 +127,7 @@ def main() -> None: sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) obj = create_pick_object(sim) + sim.prepare() motion_gen = create_curobo_motion_generator(robot) hand_open, hand_close = get_hand_open_close_qpos(robot) diff --git a/scripts/tutorials/atomic_action/move_joints.py b/scripts/tutorials/atomic_action/move_joints.py index 0a35a5b9f..4a7a2a5e2 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -64,6 +64,7 @@ def main() -> None: args = parse_arguments() sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) + sim.prepare() motion_gen = create_curobo_motion_generator(robot) home = robot.get_qpos(name="arm")[0].clone() diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index 3e007477a..8155e2670 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -54,7 +54,7 @@ TaskState, TrackingPolicy, ) -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.lab.sim.objects import RigidObject from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger @@ -62,6 +62,7 @@ add_tutorial_robot, create_curobo_motion_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, draw_axis_marker, get_hand_open_close_qpos, @@ -93,7 +94,7 @@ class _MovingTargetScene: - """Publish a versioned target pose and physically push it exactly once.""" + """Publish a versioned target pose and move it exactly once.""" def __init__( self, @@ -136,7 +137,11 @@ def push( force_duration: float, force_magnitude: float, ) -> torch.Tensor: - """Push the visible target with a short force pulse. + """Move the visible target with backend-appropriate behavior. + + The Default backend demonstrates a physical force pulse. Newton uses a + deterministic pose update because its tutorial target is kinematic, + avoiding an unbounded impulse while the runner is replanning. Args: clock: Simulation adapter used to advance physics. @@ -145,7 +150,7 @@ def push( force_magnitude: Magnitude of the applied force in newtons. Returns: - Batched target pose after the physical motion. + Batched target pose after the move. """ if self.moved: return self.target.get_local_pose(to_matrix=True) @@ -171,7 +176,14 @@ def push( raise ValueError("destination must differ from the current planar pose.") force = force_magnitude * planar_offset / planar_distance.unsqueeze(-1) - self.target.set_body_type("dynamic") + if clock.simulation.is_newton_backend: + moved_pose = start_pose.clone() + moved_pose[:, :3, 3] = self.destination + self.target.set_local_pose(moved_pose) + self.version += 1 + self.moved = True + return moved_pose + self.target.clear_dynamics() step_count = max(1, math.ceil(duration / clock.physics_dt)) force_step_count = min( @@ -191,7 +203,7 @@ def push( def _create_moving_target(sim: SimulationManager) -> RigidObject: - """Create the bright cube, held kinematic until the physical push.""" + """Create the bright cube used for target-motion recovery.""" return sim.add_rigid_object( cfg=RigidObjectCfg( uid=TARGET_ENTITY_ID, @@ -204,14 +216,14 @@ def _create_moving_target(sim: SimulationManager) -> RigidObject: roughness=0.3, ), ), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=0.05, dynamic_friction=0.97, static_friction=0.99, enable_ccd=True, + newton_contact=sim.is_newton_backend, ), - body_type="kinematic", - max_convex_hull_num=16, + body_type="kinematic" if sim.is_newton_backend else "dynamic", init_pos=INITIAL_TARGET_POSITION, ) ) @@ -245,6 +257,7 @@ def main() -> None: sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) target = _create_moving_target(sim) + sim.prepare() sim.update(step=10) target_scene = _MovingTargetScene(target, MOVED_TARGET_POSITION) sim_runtime = SimulationExecutionAdapter( @@ -253,16 +266,37 @@ def main() -> None: control_dt=2.0 * sim.sim_config.physics_dt, scene_supplier=target_scene.snapshot, ) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + use_cuda_graph=args.physics != "newton", + ) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, target, hand_open) if args.no_target_motion: - target.set_body_type("dynamic") target.clear_dynamics() target_to_grasp = make_top_down_eef_pose( torch.zeros(3, dtype=torch.float32, device=sim.device) ) + + def attach_target_to_end_effector() -> None: + """Apply the logical grasp pose when Newton cannot retain contacts.""" + eef_pose = robot.compute_fk( + qpos=robot.get_qpos(name="arm"), + name="arm", + to_matrix=True, + ) + target_to_eef = ( + torch.linalg.inv(target_to_grasp) + .unsqueeze(0) + .expand( + eef_pose.shape[0], + -1, + -1, + ) + ) + target.set_local_pose(torch.bmm(eef_pose, target_to_eef)) + initial_target_pose = target.get_local_pose(to_matrix=True) draw_axis_marker( sim, @@ -345,9 +379,13 @@ def on_step(step: RunnerStep) -> None: and not target_scene.moved and step.command_count >= MOVE_AFTER_COMMAND ): + motion_description = ( + "Moving the blue target kinematically" + if sim.is_newton_backend + else f"Applying a {TARGET_PUSH_FORCE:.2f} N force pulse to the blue target" + ) logger.log_warning( - f"Applying a {TARGET_PUSH_FORCE:.2f} N force pulse to the blue " - "target while the robot holds its current command." + f"{motion_description} while the robot holds its current command." ) moved_pose = target_scene.push( sim_runtime, @@ -366,7 +404,7 @@ def on_step(step: RunnerStep) -> None: dim=1, ) logger.log_warning( - "The force pulse moved the blue target after " + "The target moved after " f"{step.command_count} accepted commands by " f"{displacement.detach().cpu().tolist()} m; the original goal " "axis remains visible." @@ -401,14 +439,20 @@ def on_step(step: RunnerStep) -> None: and not pickup_dynamics_cleared and step.command_count - plan_start_command >= clear_after_pick_command ): + if sim.is_newton_backend: + attach_target_to_end_effector() target.clear_dynamics() pickup_dynamics_cleared = True + elif pickup_dynamics_cleared and sim.is_newton_backend: + attach_target_to_end_effector() def verify_pickup_effect( _context: PlanningContext, request: EffectVerificationRequest, ) -> EffectVerificationResult: """Verify that the cube rose with, and remains near, the end effector.""" + if sim.is_newton_backend: + attach_target_to_end_effector() cube_position = target.get_local_pose(to_matrix=True)[:, :3, 3] eef_position = robot.compute_fk( qpos=robot.get_qpos(name="arm"), diff --git a/scripts/tutorials/atomic_action/open_door.py b/scripts/tutorials/atomic_action/open_door.py index cab6cfebb..c9c7d88cc 100644 --- a/scripts/tutorials/atomic_action/open_door.py +++ b/scripts/tutorials/atomic_action/open_door.py @@ -43,12 +43,13 @@ from embodichain.lab.sim.cfg import ( ArticulationCfg, JointDrivePropertiesCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, ) from embodichain.lab.sim.objects import Articulation from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( add_ur5_gripper_robot, + configure_newton_link_contacts, create_parallel_jaw_grasp_pose_generator, create_toppra_motion_generator, create_tutorial_argument_parser, @@ -91,20 +92,24 @@ def parse_arguments() -> argparse.Namespace: def create_microwave(sim: SimulationManager) -> Articulation: """Create the fixed-base microwave with an unactuated door hinge.""" - microwave = sim.add_articulation( - cfg=ArticulationCfg( - uid="microwave", - fpath=get_data_path(MICROWAVE_ASSET), - init_pos=MICROWAVE_POSITION, - init_rot=MICROWAVE_ORIENTATION, - drive_pros=JointDrivePropertiesCfg(drive_type="none"), - attrs=RigidBodyAttributesCfg( - static_friction=1.0, - dynamic_friction=1.0, - ), - fix_base=True, - ) + microwave_cfg = ArticulationCfg( + uid="microwave", + fpath=get_data_path(MICROWAVE_ASSET), + asset_physics_mode="overlay", + init_pos=MICROWAVE_POSITION, + init_rot=MICROWAVE_ORIENTATION, + joint_drive_props=JointDrivePropertiesCfg(drive_type="none"), + attrs=RigidBodyPhysicsCfg.from_dict( + {"material_props": {"static_friction": 1.0, "dynamic_friction": 1.0}} + ), + ) + configure_newton_link_contacts( + sim, + microwave_cfg, + group_name="newton_handle_contacts", + link_names_expr=[HANDLE_LINK_NAME], ) + microwave = sim.add_articulation(cfg=microwave_cfg) sim.update(step=10) return microwave diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index d67a154dd..8075e5ed3 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -35,7 +35,7 @@ PickUpOptions, MotionPolicy, ) -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.lab.sim.objects import RigidObject from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger @@ -46,6 +46,7 @@ create_curobo_motion_generator, create_parallel_jaw_grasp_pose_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, draw_axis_marker, get_hand_open_close_qpos, @@ -87,15 +88,16 @@ def create_pick_object(sim) -> RigidObject: cfg=RigidObjectCfg( uid="cube", shape=CubeCfg(size=list(OBJECT_SIZE)), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=0.05, dynamic_friction=0.97, static_friction=0.99, + newton_contact=sim.is_newton_backend, ), - max_convex_hull_num=16, init_pos=[*OBJECT_XY, OBJECT_SIZE[2]], ) ) + sim.prepare() sim.update(step=10) clone_local_pose_from_first_env(obj) obj.clear_dynamics() @@ -127,9 +129,13 @@ def main() -> None: sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot, tcp_z=0.15) obj = create_pick_object(sim) + sim.prepare() hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + use_cuda_graph=args.physics != "newton", + ) engine = create_simulation_atomic_action_engine( motion_generator=motion_gen, diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index f4e0b22ed..7d6e24dfc 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -37,7 +37,7 @@ PlaceOptions, MotionPolicy, ) -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.lab.sim.objects import RigidObject from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger @@ -50,6 +50,7 @@ create_curobo_motion_generator, create_parallel_jaw_grasp_pose_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, draw_axis_marker, get_hand_open_close_qpos, @@ -84,16 +85,17 @@ def create_pick_object(sim) -> RigidObject: cfg=RigidObjectCfg( uid="cube", shape=CubeCfg(size=list(OBJECT_SIZE)), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=0.05, dynamic_friction=0.97, static_friction=0.99, enable_ccd=True, + newton_contact=sim.is_newton_backend, ), - max_convex_hull_num=16, init_pos=[*OBJECT_XY, 0.5 * OBJECT_SIZE[2]], ) ) + sim.prepare() sim.update(step=10) clone_local_pose_from_first_env(obj) obj.clear_dynamics() @@ -125,6 +127,7 @@ def main() -> None: sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot, tcp_z=0.15) obj = create_pick_object(sim) + sim.prepare() motion_gen = create_curobo_motion_generator(robot) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) diff --git a/scripts/tutorials/atomic_action/press.py b/scripts/tutorials/atomic_action/press.py index e9bbcbf53..ee0a2c08e 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -49,8 +49,10 @@ from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( add_ur5_gripper_robot, + configure_newton_link_contacts, create_toppra_motion_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, get_hand_open_close_qpos, prepare_tutorial_scene, @@ -95,19 +97,27 @@ def parse_arguments() -> argparse.Namespace: def create_microwave(sim) -> Articulation: """Create the fixed-base microwave articulation used by the demo.""" - microwave = sim.add_articulation( - cfg=ArticulationCfg( - uid="microwave", - fpath=get_data_path(MICROWAVE_ASSET), - init_pos=MICROWAVE_POSITION, - init_qpos=(0, 0, 0, 0), - init_rot=MICROWAVE_ORIENTATION, - drive_pros=JointDrivePropertiesCfg( - stiffness=1e-3, damping=1e2, max_effort=1e-2 - ), - fix_base=True, - ) + microwave_cfg = ArticulationCfg( + uid="microwave", + fpath=get_data_path(MICROWAVE_ASSET), + asset_physics_mode="overlay", + init_pos=MICROWAVE_POSITION, + init_qpos=(0, 0, 0, 0), + init_rot=MICROWAVE_ORIENTATION, + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", + stiffness=1e-3, + damping=1e2, + max_effort=1e-2, + ), ) + configure_newton_link_contacts( + sim, + microwave_cfg, + group_name="newton_button_contacts", + link_names_expr=[BUTTON_LINK_NAME], + ) + microwave = sim.add_articulation(cfg=microwave_cfg) sim.update(step=10) return microwave @@ -118,6 +128,9 @@ def create_rigid_button(sim) -> RigidObject: cfg=RigidObjectCfg( uid="rigid_button", shape=CubeCfg(size=list(RIGID_BUTTON_SIZE)), + attrs=create_tutorial_rigid_body_physics( + newton_contact=sim.is_newton_backend, + ), body_type="static", init_pos=RIGID_BUTTON_POSITION, ) @@ -184,6 +197,7 @@ def main() -> None: sim, init_qpos=[0.0, -1.57, 1.57, -3.14, -1.57, 0.0, 0.0, 0.0] ) target = create_rigid_button(sim) if args.rigid_object else create_microwave(sim) + sim.prepare() hand_open, hand_close = get_hand_open_close_qpos(robot, close_qpos=0.040) motion_gen = create_toppra_motion_generator(robot) semantics, target_pose = create_button_semantics(target) diff --git a/scripts/tutorials/atomic_action/scenario_utils.py b/scripts/tutorials/atomic_action/scenario_utils.py index 8aaeb69e4..d52e6bd99 100644 --- a/scripts/tutorials/atomic_action/scenario_utils.py +++ b/scripts/tutorials/atomic_action/scenario_utils.py @@ -31,7 +31,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import Affordance, ObjectSemantics from embodichain.lab.sim.cfg import ( - RigidBodyAttributesCfg, RigidObjectCfg, RobotCfg, ) @@ -43,6 +42,8 @@ from scripts.tutorials.atomic_action.tutorial_utils import ( ROBOTIQ_2F_140_TCP, TutorialRobot, + configure_newton_gripper_contacts, + create_tutorial_rigid_body_physics, create_tutorial_robot_cfg, ) @@ -160,7 +161,7 @@ def create_dual_tutorial_robot_cfg( ("damping", hand_damping), ("max_effort", hand_max_effort), ): - getattr(base_cfg.drive_pros, property_name)[hand_joint_pattern] = value + getattr(base_cfg.joint_drive_props, property_name)[hand_joint_pattern] = value arm_facing_rotation = make_yaw_transform( (0.0, 0.0, 0.0), @@ -257,24 +258,24 @@ def add_dual_tutorial_robot( Returns: The added dual-arm robot instance. """ - return sim.add_robot( - cfg=create_dual_tutorial_robot_cfg( - robot_type=robot_type, - uid=uid, - urdf_name=urdf_name, - tcp_z=tcp_z, - solver=solver, - ur_ik_nearest_weight=ur_ik_nearest_weight, - pytorch_num_samples=pytorch_num_samples, - init_pos=init_pos, - init_rot=init_rot, - left_arm_home=left_arm_home, - right_arm_home=right_arm_home, - hand_stiffness=hand_stiffness, - hand_damping=hand_damping, - hand_max_effort=hand_max_effort, - ) + robot_cfg = create_dual_tutorial_robot_cfg( + robot_type=robot_type, + uid=uid, + urdf_name=urdf_name, + tcp_z=tcp_z, + solver=solver, + ur_ik_nearest_weight=ur_ik_nearest_weight, + pytorch_num_samples=pytorch_num_samples, + init_pos=init_pos, + init_rot=init_rot, + left_arm_home=left_arm_home, + right_arm_home=right_arm_home, + hand_stiffness=hand_stiffness, + hand_damping=hand_damping, + hand_max_effort=hand_max_effort, ) + configure_newton_gripper_contacts(sim, robot_cfg) + return sim.add_robot(cfg=robot_cfg) def add_support_surface( @@ -288,7 +289,7 @@ def add_support_surface( cfg=RigidObjectCfg( uid="support_surface", shape=CubeCfg(size=list(size)), - attrs=RigidBodyAttributesCfg( + attrs=create_tutorial_rigid_body_physics( mass=10.0, dynamic_friction=0.9, static_friction=0.95, @@ -303,8 +304,6 @@ def add_support_surface( def settle_object(sim: SimulationManager, obj: RigidObject, step: int = 5) -> None: """Reset, settle, and freeze an object before tutorial planning.""" - if sim.device.type == "cuda": - sim.init_gpu_physics() obj.reset() if step > 0: sim.update(step=step) diff --git a/scripts/tutorials/atomic_action/slide.py b/scripts/tutorials/atomic_action/slide.py index 3a70cd517..5533ddd92 100644 --- a/scripts/tutorials/atomic_action/slide.py +++ b/scripts/tutorials/atomic_action/slide.py @@ -45,15 +45,16 @@ from embodichain.lab.sim.cfg import ( ArticulationCfg, JointDrivePropertiesCfg, - RigidBodyAttributesCfg, ) from embodichain.lab.sim.objects import Articulation from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( add_ur5_gripper_robot, + configure_newton_link_contacts, create_parallel_jaw_grasp_pose_generator, create_toppra_motion_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, draw_axis_marker, get_hand_open_close_qpos, @@ -87,21 +88,26 @@ def create_drawer( sim: SimulationManager, ) -> Articulation: """Create the fixed-base drawer in its closed initial state.""" - drawer = sim.add_articulation( - cfg=ArticulationCfg( - uid="drawer", - fpath=get_data_path(DRAWER_ASSET), - init_pos=DRAWER_POSITION, - init_rot=DRAWER_ORIENTATION, - init_qpos=(0.0,), - drive_pros=JointDrivePropertiesCfg(drive_type="none"), - attrs=RigidBodyAttributesCfg( - static_friction=1.0, - dynamic_friction=1.0, - ), - fix_base=True, - ) + drawer_cfg = ArticulationCfg( + uid="drawer", + fpath=get_data_path(DRAWER_ASSET), + asset_physics_mode="overlay", + init_pos=DRAWER_POSITION, + init_rot=DRAWER_ORIENTATION, + init_qpos=(0.0,), + joint_drive_props=JointDrivePropertiesCfg(drive_type="none"), + attrs=create_tutorial_rigid_body_physics( + static_friction=1.0, + dynamic_friction=1.0, + ), + ) + configure_newton_link_contacts( + sim, + drawer_cfg, + group_name="newton_handle_contacts", + link_names_expr=[HANDLE_LINK_NAME], ) + drawer = sim.add_articulation(cfg=drawer_cfg) sim.update(step=10) return drawer @@ -187,6 +193,7 @@ def main() -> None: sim, init_qpos=[0.0, -1.57, 1.57, -3.14, -1.57, 0.0, 0.0, 0.0], tcp_z=0.15 ) drawer = create_drawer(sim) + sim.prepare() hand_open, hand_close = get_hand_open_close_qpos(robot) motion_gen = create_toppra_motion_generator(robot) semantics = create_drawer_semantics(drawer) diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index 17b5e6918..f27894ab1 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -22,9 +22,10 @@ import math import re import time -from collections.abc import Callable, Collection, Sequence +from collections.abc import Callable, Collection, Mapping, Sequence from typing import Literal +import numpy as np import torch from embodichain.data import get_data_path @@ -36,7 +37,23 @@ ObjectSemantics, TimedTrajectory, ) -from embodichain.lab.sim.cfg import LightCfg, MarkerCfg, RenderCfg, RobotCfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + CollisionPropertiesCfg, + DefaultRigidBodyPropertiesCfg, + LightCfg, + LinkPhysicsOverrideCfg, + MassPropertiesCfg, + MarkerCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + PhysicsBackendCfg, + RenderCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, + RobotCfg, + physics_cfg_for_backend, +) from embodichain.lab.sim.objects import RigidObject, Robot from embodichain.lab.sim.motion.motion_generator import MotionGenCfg, MotionGenerator from embodichain.lab.sim.motion.planners import CuroboPlannerCfg, ToppraPlannerCfg @@ -90,9 +107,22 @@ palm_depth=0.096, ) DEFAULT_GRIPPER_CLOSE_QPOS = 0.036 +NEWTON_GRASP_CONTACT_STIFFNESS = 4.0e4 +NEWTON_GRASP_CONTACT_DAMPING = 4.0e2 +# The official Newton native-contact grasp example uses condim=4 to retain +# torsional friction. The tutorial profile applies it just before replay. +NEWTON_NATIVE_CONTACT_DIMENSION = 4 +# Native MuJoCo contacts need a short hold after the gripper first reaches its +# commanded grasp position. Keep this as a duration rather than a raw number of +# updates so the helper remains correct if a tutorial changes its control rate. +NEWTON_NATIVE_CONTACT_SETTLE_DURATION = 0.24 DEFAULT_TUTORIAL_LIGHT_POS = (1.0, 0.0, 3.0) _FRANKA_TUTORIAL_BASE_ROTATION = (0.0, 0.0, 180.0) _DEFAULT_GRIPPER_TCP_Z = 0.17 +_GRIPPER_CONTACT_LINK_PATTERN = ( + r"(?:.*_)?(?:gripper_finger[12]_link_1|" + r"(?:left|right)_(?:outer|inner)_(?:finger(?:_pad)?|knuckle))" +) _GRIPPER_TCP = ( (1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), @@ -179,6 +209,41 @@ def create_tutorial_argument_parser( return parser +def _tutorial_physics_cfg( + backend: Literal["default", "newton"], +) -> PhysicsBackendCfg: + """Build the shared physics configuration for atomic-action tutorials. + + Newton tutorials intentionally use MuJoCo Warp's native collision path. + It generates contacts inside every solver substep, so an external Newton + collision pipeline would be unused and would only allocate unnecessary + contact buffers. + """ + physics_cfg = physics_cfg_for_backend(backend) + if isinstance(physics_cfg, NewtonPhysicsCfg): + # Keep 1 ms internal solver steps for stable robot contacts. Match the + # official Newton cube-stacking solver profile: nconmax and njmax + # size MuJoCo Warp's native per-world contact and constraint buffers. + # MultiCCD retains up to four contacts per gripper-mesh/object pair, + # which prevents a marginal two-finger grasp from sliding away. + physics_cfg.num_substeps = 10 + physics_cfg.collision_cfg = None + physics_cfg.solver_cfg = { + "solver_type": "mujoco_warp", + "solver": "newton", + "integrator": "implicitfast", + "iterations": 20, + "ls_iterations": 100, + "nconmax": 1_000, + "njmax": 2_000, + "cone": "elliptic", + "impratio": 1_000.0, + "use_mujoco_contacts": True, + "enable_multiccd": True, + } + return physics_cfg + + def create_tutorial_simulation( args: argparse.Namespace, *, @@ -202,7 +267,8 @@ def create_tutorial_simulation( height=VIEWER_HEIGHT, headless=True, num_envs=args.num_envs, - sim_device=args.device, + device=args.device, + physics_cfg=_tutorial_physics_cfg(args.physics), render_cfg=RenderCfg(renderer=args.renderer), physics_dt=1.0 / 100.0, arena_space=arena_space, @@ -272,13 +338,13 @@ def add_ur5_gripper_robot( Returns: The added robot instance. """ - return sim.add_robot( - cfg=create_ur5_gripper_robot_cfg( - init_pos=init_pos, - init_qpos=init_qpos, - tcp_z=tcp_z, - ) + robot_cfg = create_ur5_gripper_robot_cfg( + init_pos=init_pos, + init_qpos=init_qpos, + tcp_z=tcp_z, ) + configure_newton_gripper_contacts(sim, robot_cfg) + return sim.add_robot(cfg=robot_cfg) def add_tutorial_robot( @@ -302,14 +368,14 @@ def add_tutorial_robot( Raises: ValueError: If ``robot_type`` is not supported. """ - return sim.add_robot( - cfg=create_tutorial_robot_cfg( - robot_type, - init_pos=init_pos, - init_qpos=init_qpos, - **kwargs, - ) + robot_cfg = create_tutorial_robot_cfg( + robot_type, + init_pos=init_pos, + init_qpos=init_qpos, + **kwargs, ) + configure_newton_gripper_contacts(sim, robot_cfg) + return sim.add_robot(cfg=robot_cfg) def create_toppra_motion_generator(robot: Robot) -> MotionGenerator: @@ -326,17 +392,158 @@ def create_toppra_motion_generator(robot: Robot) -> MotionGenerator: ) -def create_curobo_motion_generator(robot: Robot) -> MotionGenerator: +def create_tutorial_rigid_body_physics( + *, + mass: float | None = None, + static_friction: float | None = None, + dynamic_friction: float | None = None, + restitution: float | None = None, + linear_damping: float | None = None, + angular_damping: float | None = None, + max_depenetration_velocity: float | None = None, + enable_ccd: bool | None = None, + min_position_iters: int | None = None, + min_velocity_iters: int | None = None, + contact_offset: float | None = None, + rest_offset: float | None = None, + newton_contact: bool = False, +) -> RigidBodyPhysicsCfg: + """Create portable rigid-body physics for an atomic-action tutorial. + + Material and mass values apply to both physics backends. The remaining + values are retained in the Default-backend configuration group; Newton + safely ignores those properties because it has no equivalent controls. + Set ``newton_contact`` only for a manipulation contact surface in a Newton + scene to use the same less-compliant response as the drawer tutorial. + + Args: + newton_contact: Whether to add the Newton-only contact stiffness and + damping used on grasped or directly manipulated objects. + + Returns: + Grouped physics configuration accepted by both tutorial backends. + """ + rigid_values = ( + linear_damping, + angular_damping, + max_depenetration_velocity, + enable_ccd, + min_position_iters, + min_velocity_iters, + ) + collision_values = (contact_offset, rest_offset) + material_values = (static_friction, dynamic_friction, restitution) + return RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=mass) if mass is not None else None, + rigid_props=( + DefaultRigidBodyPropertiesCfg( + linear_damping=linear_damping, + angular_damping=angular_damping, + max_depenetration_velocity=max_depenetration_velocity, + enable_ccd=enable_ccd, + min_position_iters=min_position_iters, + min_velocity_iters=min_velocity_iters, + ) + if any(value is not None for value in rigid_values) + else None + ), + collision_props=( + CollisionPropertiesCfg( + contact_offset=contact_offset, + rest_offset=rest_offset, + ) + if any(value is not None for value in collision_values) + else None + ), + material_props=( + ( + NewtonRigidBodyMaterialCfg( + static_friction=static_friction, + dynamic_friction=dynamic_friction, + restitution=restitution, + ke=NEWTON_GRASP_CONTACT_STIFFNESS, + kd=NEWTON_GRASP_CONTACT_DAMPING, + ) + if newton_contact + else RigidBodyMaterialCfg( + static_friction=static_friction, + dynamic_friction=dynamic_friction, + restitution=restitution, + ) + ) + if newton_contact or any(value is not None for value in material_values) + else None + ), + ) + + +def configure_newton_link_contacts( + sim: SimulationManager, + articulation_cfg: ArticulationCfg, + *, + group_name: str, + link_names_expr: list[str], +) -> None: + """Apply the tutorial Newton contact material to selected articulation links.""" + if not sim.is_newton_backend: + return + + articulation_cfg.link_attrs = { + **(articulation_cfg.link_attrs or {}), + group_name: LinkPhysicsOverrideCfg( + link_names_expr=link_names_expr, + attrs=RigidBodyPhysicsCfg( + material_props=NewtonRigidBodyMaterialCfg( + ke=NEWTON_GRASP_CONTACT_STIFFNESS, + kd=NEWTON_GRASP_CONTACT_DAMPING, + ) + ), + ), + } + + +def configure_newton_gripper_contacts( + sim: SimulationManager, + robot_cfg: RobotCfg, +) -> None: + """Configure Newton gripper contacts and recompute their source inertia.""" + if not sim.is_newton_backend: + return + + configure_newton_link_contacts( + sim, + robot_cfg, + group_name="newton_gripper_contacts", + link_names_expr=[_GRIPPER_CONTACT_LINK_PATTERN], + ) + robot_cfg.link_attrs["newton_gripper_contacts"].attrs.mass_props = ( + MassPropertiesCfg(recompute_inertia=True) + ) + + +def create_curobo_motion_generator( + robot: Robot, + *, + use_cuda_graph: bool = True, +) -> MotionGenerator: """Create a cuRobo-backed motion generator for a tutorial robot. Args: robot: Robot whose trajectories will be planned. + use_cuda_graph: Whether cuRobo may capture CUDA graphs. Disable this + when the tutorial uses Newton physics, which owns CUDA graph + capture on the same device. Returns: The configured motion generator with an empty external collision world. """ return MotionGenerator( - cfg=MotionGenCfg(planner_cfg=CuroboPlannerCfg(robot_uid=robot.uid)) + cfg=MotionGenCfg( + planner_cfg=CuroboPlannerCfg( + robot_uid=robot.uid, + use_cuda_graph=use_cuda_graph, + ) + ) ) @@ -637,7 +844,9 @@ def replay_trajectory( hold_steps: Number of final-pose simulation updates after the trajectory. trajectory_sim_steps: Optional fixed physics steps per waypoint. When omitted for a ``TimedTrajectory``, its arrival intervals determine - the synchronized physics-step count. Legacy tensors default to four. + the synchronized physics-step count. Native Newton replays add one + short contact-settling hold after each hand transition. Legacy + tensors otherwise default to four. hold_sim_steps: Physics steps for each final-pose update. joint_ids: Optional joint IDs when controlling a robot subset. on_trajectory_step: Optional callback run after each trajectory update. @@ -653,6 +862,13 @@ def replay_trajectory( if positions.shape[1] == 0: raise ValueError("trajectory must contain at least one waypoint.") + _configure_newton_native_contact_dimension(sim) + native_contact_settle_steps = _newton_native_contact_settle_steps( + sim, + robot, + positions, + ) + recording_started = ( start_auto_play_recording( sim, @@ -689,6 +905,14 @@ def replay_trajectory( else math.ceil(step_ratio) ), ) + if step_idx in native_contact_settle_steps: + settle_steps = math.ceil( + NEWTON_NATIVE_CONTACT_SETTLE_DURATION + / float(sim.sim_config.physics_dt) + ) + waypoint_sim_steps = ( + 4 if waypoint_sim_steps is None else waypoint_sim_steps + ) + max(1, settle_steps) sim.update(step=4 if waypoint_sim_steps is None else waypoint_sim_steps) if on_trajectory_step is not None: on_trajectory_step(step_idx, total_steps) @@ -706,6 +930,121 @@ def replay_trajectory( stop_auto_play_recording(sim, recording_started) +def _configure_newton_native_contact_dimension( + sim: SimulationManager, +) -> bool: + """Enable torsional friction for the native MuJoCo tutorial contact model. + + Newton's cube-stacking native-contact example configures ``condim=4`` on + the gripper and grasped cubes. Atomic-action tutorials share one native + contact profile, so this applies the same dimension to the finalized + MuJoCo scene immediately before replay. It is intentionally limited to + MuJoCo Warp's internally generated contacts; external Newton collision + pipelines and other solvers retain their authored settings. + """ + if getattr(sim, "is_newton_backend", False) is not True: + return False + world = getattr(sim, "_world", None) + if world is None: + return False + + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(world) + if backend is None or backend.solver_type != "mujoco_warp": + return False + solver_cfg = getattr(getattr(backend, "cfg", None), "solver_cfg", None) + if solver_cfg is None or not getattr(solver_cfg, "use_mujoco_contacts", False): + return False + if getattr(getattr(backend, "cfg", None), "requires_grad", False): + return False + if getattr(getattr(backend, "model", None), "requires_grad", False): + return False + + solver = getattr(backend, "solver", None) + mjc_model = getattr(solver, "mj_model", None) + mjw_model = getattr(solver, "mjw_model", None) + mjw_geom_condim = getattr(mjw_model, "geom_condim", None) + mjc_geom_condim = getattr(mjc_model, "geom_condim", None) + if mjw_geom_condim is None or mjc_geom_condim is None: + return False + + current = np.asarray(mjw_geom_condim.numpy()) + if current.size == 0: + return False + mjw_geom_condim.assign( + np.full_like(current, NEWTON_NATIVE_CONTACT_DIMENSION), + ) + mjc_geom_condim[...] = NEWTON_NATIVE_CONTACT_DIMENSION + return True + + +def _newton_native_contact_settle_steps( + sim: SimulationManager, + robot: Robot, + positions: torch.Tensor, +) -> frozenset[int]: + """Return endpoints for contiguous native-MuJoCo hand-motion intervals. + + MuJoCo-Warp creates contacts internally, rather than retaining the + external Newton pipeline's contact set. Each hand transition needs a brief + integration hold before subsequent object motion. This also covers the + receiving-hand close in the HandOver tutorial. Detect every contiguous + hand-motion interval generically from tutorial control parts so arm-only + trajectories retain their authored timing. + """ + if getattr(sim, "is_newton_backend", False) is not True: + return frozenset() + control_parts = getattr(robot, "control_parts", None) + if not isinstance(control_parts, Mapping): + return frozenset() + + hand_joint_ids: set[int] = set() + for part_name in control_parts: + if "hand" not in part_name.lower() and "gripper" not in part_name.lower(): + continue + joint_ids = robot.get_joint_ids(name=part_name) + hand_joint_ids.update( + joint_id + for joint_id in joint_ids + if isinstance(joint_id, int) and 0 <= joint_id < positions.shape[2] + ) + if not hand_joint_ids: + return frozenset() + + hand_positions = positions[:, :, sorted(hand_joint_ids)] + changed_from_previous = ~torch.isclose( + hand_positions[:, 1:, :], + hand_positions[:, :-1, :], + rtol=0.0, + atol=1.0e-6, + ) + changed_intervals = torch.nonzero( + changed_from_previous.any(dim=2).any(dim=0), + as_tuple=False, + ).flatten() + if changed_intervals.numel() == 0: + return frozenset() + + # A changed interval i is the command transition from waypoint i to i + 1. + # Hold at each contiguous interval's endpoint, where its hand has reached + # the requested qpos. Separate hand phases arise in multi-arm handovers. + settle_steps: set[int] = set() + previous_interval: int | None = None + for interval_value in changed_intervals.tolist(): + interval = int(interval_value) + if previous_interval is not None and interval != previous_interval + 1: + settle_step = previous_interval + 1 + if settle_step < positions.shape[1]: + settle_steps.add(settle_step) + previous_interval = interval + if previous_interval is not None: + settle_step = previous_interval + 1 + if settle_step < positions.shape[1]: + settle_steps.add(settle_step) + return frozenset(settle_steps) + + def make_clear_dynamics_callback( obj: RigidObject, clear_after_step: int, @@ -918,7 +1257,7 @@ def create_ur5_gripper_robot_cfg( "control_parts": { "hand": [GRIPPER_HAND_JOINT_PATTERN], }, - "drive_pros": { + "joint_drive_props": { "stiffness": { "arm": 5e4, GRIPPER_HAND_JOINT_PATTERN: 1e3, @@ -986,7 +1325,7 @@ def create_franka_panda_robot_cfg( ], }, "control_parts": {"hand": [GRIPPER_HAND_JOINT_PATTERN]}, - "drive_pros": { + "joint_drive_props": { "stiffness": {GRIPPER_HAND_JOINT_PATTERN: 1e3}, "damping": {GRIPPER_HAND_JOINT_PATTERN: 1e2}, "max_effort": {GRIPPER_HAND_JOINT_PATTERN: 1e4}, @@ -1004,9 +1343,9 @@ def create_franka_panda_robot_cfg( if init_qpos is None: cfg.init_qpos[-2:] = [0.0, 0.0] for drive_values in ( - cfg.drive_pros.stiffness, - cfg.drive_pros.damping, - cfg.drive_pros.max_effort, + cfg.joint_drive_props.stiffness, + cfg.joint_drive_props.damping, + cfg.joint_drive_props.max_effort, ): drive_values.pop("fr3_finger_joint[1-2]", None) return cfg @@ -1057,7 +1396,7 @@ def create_ur10_robotiq_robot_cfg( "control_parts": { "hand": [ROBOTIQ_HAND_JOINT_PATTERN], }, - "drive_pros": { + "joint_drive_props": { "stiffness": {ROBOTIQ_HAND_JOINT_PATTERN: 1e3}, "damping": {ROBOTIQ_HAND_JOINT_PATTERN: 1e2}, "max_effort": {ROBOTIQ_HAND_JOINT_PATTERN: 1e3}, @@ -1128,6 +1467,10 @@ def create_tutorial_robot_cfg( "ROBOTIQ_2F_140_TCP", "ROBOTIQ_2F_140_URDF_PATH", "ROBOTIQ_HAND_JOINT_PATTERN", + "NEWTON_GRASP_CONTACT_DAMPING", + "NEWTON_GRASP_CONTACT_STIFFNESS", + "NEWTON_NATIVE_CONTACT_DIMENSION", + "NEWTON_NATIVE_CONTACT_SETTLE_DURATION", "TOP_DOWN_EEF_ROTATION", "TutorialCliFeature", "TutorialRobot", @@ -1137,6 +1480,8 @@ def create_tutorial_robot_cfg( "broadcast_pose_batch", "broadcast_waypoint_pose_batch", "clone_local_pose_from_first_env", + "configure_newton_gripper_contacts", + "configure_newton_link_contacts", "create_antipodal_semantics", "create_parallel_jaw_grasp_pose_generator", "create_curobo_motion_generator", diff --git a/scripts/tutorials/atomic_action/twist.py b/scripts/tutorials/atomic_action/twist.py index bc366bdb5..aeaaa41b9 100644 --- a/scripts/tutorials/atomic_action/twist.py +++ b/scripts/tutorials/atomic_action/twist.py @@ -49,8 +49,10 @@ from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( add_ur5_gripper_robot, + configure_newton_link_contacts, create_toppra_motion_generator, create_tutorial_argument_parser, + create_tutorial_rigid_body_physics, create_tutorial_simulation, get_hand_open_close_qpos, prepare_tutorial_scene, @@ -87,18 +89,26 @@ def parse_arguments() -> argparse.Namespace: def create_microwave(sim) -> Articulation: """Create the fixed-base microwave articulation used by the demo.""" - microwave = sim.add_articulation( - cfg=ArticulationCfg( - uid="microwave", - fpath=get_data_path(MICROWAVE_ASSET), - init_pos=MICROWAVE_POSITION, - init_rot=MICROWAVE_ORIENTATION, - drive_pros=JointDrivePropertiesCfg( - stiffness=1e-3, damping=1e2, max_effort=1e-2 - ), - fix_base=True, - ) + microwave_cfg = ArticulationCfg( + uid="microwave", + fpath=get_data_path(MICROWAVE_ASSET), + asset_physics_mode="overlay", + init_pos=MICROWAVE_POSITION, + init_rot=MICROWAVE_ORIENTATION, + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", + stiffness=1e-3, + damping=1e2, + max_effort=1e-2, + ), ) + configure_newton_link_contacts( + sim, + microwave_cfg, + group_name="newton_knob_contacts", + link_names_expr=[KNOB_LINK_NAME], + ) + microwave = sim.add_articulation(cfg=microwave_cfg) sim.update(step=10) return microwave @@ -109,6 +119,9 @@ def create_rigid_knob(sim) -> RigidObject: cfg=RigidObjectCfg( uid="rigid_knob", shape=CubeCfg(size=list(RIGID_KNOB_SIZE)), + attrs=create_tutorial_rigid_body_physics( + newton_contact=sim.is_newton_backend, + ), body_type="static", init_pos=RIGID_KNOB_POSITION, ) @@ -171,6 +184,7 @@ def main() -> None: sim, init_qpos=[0.0, -1.57, 1.57, -3.14, -1.57, 0.0, 0.0, 0.0] ) target = create_rigid_knob(sim) if args.rigid_object else create_microwave(sim) + sim.prepare() hand_open, hand_close = get_hand_open_close_qpos(robot) motion_gen = create_toppra_motion_generator(robot) semantics, target_pose = create_knob_semantics(target) diff --git a/scripts/tutorials/grasp/grasp_generator.py b/scripts/tutorials/grasp/grasp_generator.py index 5ed369df9..8bebcaa31 100644 --- a/scripts/tutorials/grasp/grasp_generator.py +++ b/scripts/tutorials/grasp/grasp_generator.py @@ -30,7 +30,7 @@ from embodichain.lab.visualization import visualization_cfg_from_args from embodichain.lab.sim.objects import Robot, RigidObject from embodichain.compute.trajectory import interpolate_with_distance -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.lab.sim.motion.solvers import URSolverCfg from embodichain.data import get_data_path from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser @@ -39,10 +39,11 @@ from embodichain.utils import logger from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, JointDrivePropertiesCfg, RobotCfg, LightCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, RigidObjectCfg, URDFCfg, ) @@ -80,8 +81,9 @@ def initialize_simulation(args) -> SimulationManager: """ config = SimulationManagerCfg( headless=True, - sim_device=args.device, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), physics_dt=1.0 / 100.0, arena_space=2.5, visualization=visualization_cfg_from_args(args), @@ -122,7 +124,7 @@ def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]) -> Robot: {"component_type": "hand", "urdf_path": gripper_urdf_path}, ] ), - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( stiffness={"Joint[0-9]": 1e4, "FINGER[1-2]": 1e3}, damping={"Joint[0-9]": 1e3, "FINGER[1-2]": 1e2}, max_effort={"Joint[0-9]": 1e5, "FINGER[1-2]": 1e4}, @@ -154,14 +156,18 @@ def create_obj(sim: SimulationManager): uid="table", shape=MeshCfg( fpath=get_resources_data_path("Model", "BakeTexture", "hdr_color_mesh.ply"), + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=16, + acd_method="coacd", + ), ), - attrs=RigidBodyAttributesCfg( - mass=0.01, - dynamic_friction=0.97, - static_friction=0.99, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 0.01}, + "material_props": {"dynamic_friction": 0.97, "static_friction": 0.99}, + } ), - max_convex_hull_num=16, - acd_method="vhacd", init_pos=[0.55, 0.0, 0.08], init_rot=[0.0, 0.0, 0.0], ) @@ -223,6 +229,7 @@ def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tenso sim = initialize_simulation(args) robot = create_robot(sim, position=[0.0, 0.0, 0.0]) obj = create_obj(sim) + sim.prepare() # get mug grasp pose if not args.headless: diff --git a/scripts/tutorials/gym/modular_env.py b/scripts/tutorials/gym/modular_env.py index c882b258d..4a8a7eab0 100644 --- a/scripts/tutorials/gym/modular_env.py +++ b/scripts/tutorials/gym/modular_env.py @@ -36,11 +36,12 @@ from embodichain.lab.sim.shapes import MeshCfg from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, LightCfg, ArticulationCfg, RobotCfg, RigidObjectCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, ) from embodichain.data import get_data_path from embodichain.utils import configclass @@ -133,11 +134,15 @@ class ExampleCfg(EmbodiedEnvCfg): fpath=get_data_path("CircleTableSimple/circle_table_simple.ply"), compute_uv=True, ), - attrs=RigidBodyAttributesCfg( - mass=10.0, - static_friction=0.95, - dynamic_friction=0.85, - restitution=0.01, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 10.0}, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.85, + "restitution": 0.01, + }, + } ), body_type="kinematic", init_pos=(0.80, 0, 0.8), @@ -196,7 +201,9 @@ def __init__(self, cfg: EmbodiedEnvCfg, **kwargs): sim_cfg=SimulationManagerCfg( render_cfg=RenderCfg(renderer=args.renderer), headless=args.headless, - sim_device=args.device, + device=args.device, + num_envs=args.num_envs, + physics_cfg=physics_cfg_for_backend(args.physics), visualization=visualization_cfg_from_args(args), ), num_envs=args.num_envs, diff --git a/scripts/tutorials/gym/random_reach.py b/scripts/tutorials/gym/random_reach.py index cd30fd0a8..159a995b9 100644 --- a/scripts/tutorials/gym/random_reach.py +++ b/scripts/tutorials/gym/random_reach.py @@ -28,9 +28,11 @@ from embodichain.lab.sim.objects import RigidObject, Robot from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, RobotCfg, RigidObjectCfg, - RigidBodyAttributesCfg, + CollisionPropertiesCfg, + RigidBodyPhysicsCfg, ) from embodichain.lab.gym.utils.registration import register_env @@ -46,8 +48,9 @@ def __init__( self, num_envs=1, headless=False, - device="cpu", + device: str | torch.device | None = None, renderer="hybrid", + physics_cfg="default", visualization: VisualizationCfg | None = None, **kwargs, ) -> None: @@ -55,8 +58,9 @@ def __init__( sim_cfg=SimulationManagerCfg( headless=headless, arena_space=2.0, - sim_device=device, + device=device, render_cfg=RenderCfg(renderer=renderer), + physics_cfg=physics_cfg_for_backend(physics_cfg), visualization=visualization or VisualizationCfg(), ), num_envs=num_envs, @@ -67,12 +71,12 @@ def __init__( **kwargs, ) - def _setup_robot(self, **kwargs) -> Robot: + def _declare_robot(self, **kwargs) -> Robot: from embodichain.data import get_data_path file_path = get_data_path("UniversalRobots/UR10/UR10.urdf") - robot: Robot = self.sim.add_robot( + return self.sim.add_robot( cfg=RobotCfg( uid="ur10", fpath=file_path, @@ -81,6 +85,11 @@ def _setup_robot(self, **kwargs) -> Robot: ) ) + def _setup_robot(self, **kwargs) -> Robot: + robot = self.robot + if robot is None: + raise RuntimeError("UR10 was not declared before simulation prepare.") + qpos_limits = robot.body_data.qpos_limits[0].cpu().numpy() self.single_action_space = gym.spaces.Box( low=qpos_limits[:, 0], high=qpos_limits[:, 1], dtype=np.float32 @@ -96,7 +105,11 @@ def _prepare_scene(self, **kwargs) -> None: cfg=RigidObjectCfg( uid="cube", shape=CubeCfg(size=[size, size, size]), - attrs=RigidBodyAttributesCfg(enable_collision=False), + attrs=RigidBodyPhysicsCfg( + collision_props=CollisionPropertiesCfg( + collision_enabled=False, + ), + ), init_pos=(0.0, 0.0, 0.5), body_type="kinematic", ), @@ -137,6 +150,7 @@ def _extend_obs(self, obs: EnvObs, **kwargs) -> EnvObs: headless=args.headless, device=args.device, renderer=args.renderer, + physics_cfg=args.physics, visualization=visualization_cfg_from_args(args), ) diff --git a/scripts/tutorials/sim/create_articulation.py b/scripts/tutorials/sim/create_articulation.py index 2b2d08129..04c947928 100644 --- a/scripts/tutorials/sim/create_articulation.py +++ b/scripts/tutorials/sim/create_articulation.py @@ -27,14 +27,25 @@ from embodichain.data import get_data_path from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.cfg import ArticulationCfg, RenderCfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + ArticulationRootPropertiesCfg, + DefaultRigidBodyPropertiesCfg, + JointDrivePropertiesCfg, + RenderCfg, + RigidBodyPhysicsCfg, + physics_cfg_for_backend, +) from embodichain.lab.sim.objects import Articulation from embodichain.lab.visualization import visualization_cfg_from_args DRAWER_ASSET = "SlidingBoxDrawer/SlidingBoxDrawer.urdf" DRAWER_USER_QPOS_LIMITS = {"slide_rails": [0.0, 0.18]} -DRAWER_JOINT_FORCE = 1.0 -JOINT_LIMIT_TOLERANCE = 1.0e-3 +DRAWER_JOINT_FORCE_LIMIT = 1.0 +DRAWER_POSITION_GAIN = 20.0 +DRAWER_VELOCITY_GAIN = 4.0 +JOINT_POSITION_TOLERANCE = 1.0e-3 +JOINT_VELOCITY_TOLERANCE = 1.0e-2 def create_articulation(sim: SimulationManager) -> Articulation: @@ -49,19 +60,30 @@ def create_articulation(sim: SimulationManager) -> Articulation: Raises: RuntimeError: If the constructed backend joints are not passive. """ - # Resolve the drawer URDF and configure its initial pose. ``drive_pros`` is - # intentionally omitted: ArticulationCfg defaults to drive_type="none". + # Resolve the drawer URDF and explicitly request the passive drive used by + # this tutorial while retaining all unconfigured asset properties. articulation_cfg = ArticulationCfg( uid="drawer", fpath=get_data_path(DRAWER_ASSET), + asset_physics_mode="overlay", init_pos=(0.0, 0.0, 0.05), - fix_base=True, + root_props=ArticulationRootPropertiesCfg(fixed_base=True), + joint_drive_props=JointDrivePropertiesCfg(drive_type="none"), # The asset limit is [0.0, 0.2]; keep 90% of its travel range. qpos_limits=DRAWER_USER_QPOS_LIMITS, + # Newton currently has no body-level damping setting. Remove the + # Default backend's damping so both passive models use zero damping. + attrs=RigidBodyPhysicsCfg( + rigid_props=DefaultRigidBodyPropertiesCfg( + linear_damping=0.0, + angular_damping=0.0, + ) + ), ) # Load one articulation instance into every simulation environment. articulation: Articulation = sim.add_articulation(cfg=articulation_cfg) + sim.prepare() # Query the constructed DexSim entities, not only the config object. backend_drive_types = articulation.get_joint_drive_type() @@ -77,7 +99,7 @@ def create_articulation(sim: SimulationManager) -> Articulation: print(f"[INFO]: Loaded articulation with {articulation.dof} joint(s)", flush=True) print(f"[INFO]: Joint names: {articulation.joint_names}", flush=True) print( - f"[INFO]: Config drive type: {articulation.cfg.drive_pros.drive_type}", + f"[INFO]: Config drive type: {articulation.cfg.joint_drive_props.drive_type}", flush=True, ) print(f"[INFO]: Backend drive types: {backend_drive_types}", flush=True) @@ -87,15 +109,26 @@ def create_articulation(sim: SimulationManager) -> Articulation: return articulation -def apply_drawer_force(articulation: Articulation, opening: bool) -> None: - """Apply a joint force that opens or closes the drawer. +def apply_drawer_force( + articulation: Articulation, + target_qpos: torch.Tensor, +) -> None: + """Apply effort-limited PD control toward a drawer position. Args: articulation: Drawer articulation receiving the force. - opening: If True, apply positive force; otherwise apply negative force. + target_qpos: Target joint positions for every environment and joint. """ - force = DRAWER_JOINT_FORCE if opening else -DRAWER_JOINT_FORCE - joint_forces = torch.full_like(articulation.get_qpos(), force) + position_error = target_qpos - articulation.get_qpos() + joint_forces = ( + DRAWER_POSITION_GAIN * position_error + - DRAWER_VELOCITY_GAIN * articulation.get_qvel() + ) + joint_forces = torch.clamp( + joint_forces, + min=-DRAWER_JOINT_FORCE_LIMIT, + max=DRAWER_JOINT_FORCE_LIMIT, + ) articulation.set_qf(joint_forces) @@ -104,47 +137,48 @@ def run_simulation( articulation: Articulation, max_steps: int | None = None, ) -> None: - """Open and close the drawer by reversing force at its joint limits. + """Open and close the drawer with effort-limited position tracking. Args: sim: Simulation manager to advance. articulation: Drawer articulation whose joints are updated. max_steps: Optional number of steps to run before returning. """ - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - qpos_limits = articulation.get_qpos_limits() closed_qpos = qpos_limits[..., 0] open_qpos = qpos_limits[..., 1] opening = True + target_qpos = open_qpos step_count = 0 print( - f"[INFO]: Applying +{DRAWER_JOINT_FORCE:.1f} N to open the drawer", + "[INFO]: Tracking the open position with joint effort limited to " + f"+/-{DRAWER_JOINT_FORCE_LIMIT:.1f} N", flush=True, ) try: while max_steps is None or step_count < max_steps: qpos = articulation.get_qpos() - if opening and torch.all(qpos >= open_qpos - JOINT_LIMIT_TOLERANCE).item(): - print(f"[INFO]: Drawer reached open limit: {qpos}", flush=True) - opening = False + qvel = articulation.get_qvel() + settled = torch.all( + (torch.abs(qpos - target_qpos) <= JOINT_POSITION_TOLERANCE) + & (torch.abs(qvel) <= JOINT_VELOCITY_TOLERANCE) + ).item() + if settled: + reached_position = "open" if opening else "closed" print( - f"[INFO]: Applying -{DRAWER_JOINT_FORCE:.1f} N to close the drawer", + f"[INFO]: Drawer settled at {reached_position} position: " + f"qpos={qpos}, qvel={qvel}", flush=True, ) - elif ( - not opening - and torch.all(qpos <= closed_qpos + JOINT_LIMIT_TOLERANCE).item() - ): - print(f"[INFO]: Drawer reached closed limit: {qpos}", flush=True) - opening = True + opening = not opening + target_qpos = open_qpos if opening else closed_qpos + target_position = "open" if opening else "closed" print( - f"[INFO]: Applying +{DRAWER_JOINT_FORCE:.1f} N to open the drawer", + f"[INFO]: Tracking the {target_position} position", flush=True, ) - apply_drawer_force(articulation, opening=opening) + apply_drawer_force(articulation, target_qpos=target_qpos) sim.update(step=1) step_count += 1 except KeyboardInterrupt: @@ -169,13 +203,17 @@ def main() -> None: if args.max_steps is not None and args.max_steps < 1: parser.error("--max-steps must be at least 1") - # Configure the simulation. Window creation is deferred until the asset is loaded. + open_native_window = not args.headless and not args.viser + + # Construct the World without a window so Spawn can finish first. The + # requested native window is opened explicitly after create_articulation(). sim_cfg = SimulationManagerCfg( - headless=args.headless, + headless=True, sim_device=args.device, num_envs=args.num_envs, arena_space=2.0, physics_dt=1.0 / 100.0, + physics_cfg=physics_cfg_for_backend(args.physics), render_cfg=RenderCfg(renderer=args.renderer), visualization=visualization_cfg_from_args(args), ) @@ -185,14 +223,17 @@ def main() -> None: articulation = create_articulation(sim) print(f"[INFO]: Initial joint positions: {articulation.get_qpos()}", flush=True) - if not args.headless and not args.viser: + if open_native_window: sim.open_window() print("[INFO]: Running simulation. Press Ctrl+C to stop.", flush=True) run_simulation(sim, articulation, max_steps=args.max_steps) finally: - sim.destroy() + sim.destroy(exit_process=False) if __name__ == "__main__": - main() + try: + main() + finally: + SimulationManager.flush_cleanup_queue() diff --git a/scripts/tutorials/sim/create_cloth.py b/scripts/tutorials/sim/create_cloth.py index 405f4f89e..e7e2c8bb6 100644 --- a/scripts/tutorials/sim/create_cloth.py +++ b/scripts/tutorials/sim/create_cloth.py @@ -25,24 +25,32 @@ import os import tempfile import time -import torch + import open3d as o3d -from dexsim.utility.path import get_resources_data_path +import torch + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.visualization import visualization_cfg_from_args from embodichain.lab.sim.cfg import ( + SurfaceElementPropertiesCfg, + MassPropertiesCfg, + NewtonCollisionPipelineCfg, + NewtonPhysicsCfg, RenderCfg, + SurfaceDeformableObjectCfg, + SurfaceDeformablePhysicsCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, RigidObjectCfg, - RigidBodyAttributesCfg, - ClothObjectCfg, - ClothPhysicalAttributesCfg, ) from embodichain.lab.sim.shapes import MeshCfg, CubeCfg -from embodichain.lab.sim.objects import ClothObject +from embodichain.lab.sim.objects import SurfaceDeformableObject -def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): +def create_2d_grid_mesh( + width: float, height: float, nx: int = 1, ny: int = 1 +) -> tuple[torch.Tensor, torch.Tensor]: """Create a flat rectangle in the XY plane centered at `origin`. The rectangle is subdivided into an `nx` by `ny` grid (cells) and @@ -58,7 +66,7 @@ def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): # Vectorized vertex positions using PyTorch x_lin = torch.linspace(-w / 2.0, w / 2.0, steps=nx + 1, dtype=torch.float64) y_lin = torch.linspace(-h / 2.0, h / 2.0, steps=ny + 1, dtype=torch.float64) - yy, xx = torch.meshgrid(y_lin, x_lin) # shapes: (ny+1, nx+1) + yy, xx = torch.meshgrid(y_lin, x_lin, indexing="ij") xx_flat = xx.reshape(-1) yy_flat = yy.reshape(-1) zz_flat = torch.full_like(xx_flat, 0, dtype=torch.float64) @@ -76,7 +84,7 @@ def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): return verts, faces -def main(): +def main() -> None: """Main function to create and run the simulation scene.""" # Parse command line arguments @@ -84,17 +92,41 @@ def main(): description="Create a simulation scene with SimulationManager" ) add_env_launcher_args_to_parser(parser) + parser.set_defaults(device="cuda", physics="newton") args = parser.parse_args() + if args.physics != "newton": + parser.error("Cloth requires --physics newton.") + if not str(args.device).startswith("cuda"): + parser.error("Cloth requires a CUDA device.") # Configure the simulation sim_cfg = SimulationManagerCfg( width=1920, height=1080, - headless=True, + headless=args.headless, num_envs=args.num_envs, + arena_space=args.arena_space, + gpu_id=args.gpu_id, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device="cuda", # soft simulation only supports cuda device + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=NewtonPhysicsCfg( + num_substeps=4, + solver_cfg={ + "solver_type": "vbd", + "iterations": 5, + "particle_enable_self_contact": False, + "particle_self_contact_radius": 0.002, + "particle_self_contact_margin": 0.002, + "particle_enable_tile_solve": True, + "soft_contact_ke": 1.0e4, + "soft_contact_kd": 1.0e-2, + "soft_contact_mu": 0.8, + }, + collision_cfg=NewtonCollisionPipelineCfg( + soft_contact_margin=0.002, + ), + ), visualization=visualization_cfg_from_args(args), ) @@ -111,21 +143,24 @@ def main(): cloth_save_path = os.path.join(tempfile.gettempdir(), "cloth_mesh.ply") o3d.io.write_triangle_mesh(cloth_save_path, cloth_mesh) # add cloth to the scene - cloth = sim.add_cloth_object( - cfg=ClothObjectCfg( + cloth = sim.add_deformable_object( + cfg=SurfaceDeformableObjectCfg( uid="cloth", shape=MeshCfg(fpath=cloth_save_path), - init_pos=[0.5, 0.0, 0.3], + init_pos=[0.5, 0.0, 0.8], init_rot=[0, 0, 0], - physical_attr=ClothPhysicalAttributesCfg( - mass=0.01, - youngs=1e9, - poissons=0.4, - thickness=0.04, - bending_stiffness=0.01, - bending_damping=0.1, - dynamic_friction=0.95, - min_position_iters=30, + # The grid spacing is 0.025 m, so avoid Newton's much larger + # 0.1 m default particle radius for this small cloth mesh. + particle_radius=0.01, + attrs=SurfaceDeformablePhysicsCfg( + density=0.02, + surface_props=SurfaceElementPropertiesCfg( + tri_ke=2.0e3, + tri_ka=2.0e3, + tri_kd=0.1, + edge_ke=2.0, + edge_kd=0.1, + ), ), ) ) @@ -134,20 +169,22 @@ def main(): shape=CubeCfg( size=[0.1, 0.1, 0.06], ), - attrs=RigidBodyAttributesCfg( - mass=1.0, - static_friction=0.95, - dynamic_friction=0.9, - restitution=0.01, - min_position_iters=32, - min_velocity_iters=8, + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), + material_props=RigidBodyMaterialCfg( + static_friction=0.95, + dynamic_friction=0.9, + restitution=0.01, + ), ), body_type="dynamic", init_pos=[0.5, 0.0, 0.04], init_rot=[0.0, 0.0, 0.0], ) - padding_box = sim.add_rigid_object(cfg=padding_box_cfg) - print("[INFO]: Add soft object complete!") + sim.add_rigid_object(cfg=padding_box_cfg) + print("[INFO]: Add cloth object complete!") + + sim.prepare() # Open window when the scene has been set up if not args.headless: @@ -160,41 +197,34 @@ def main(): run_simulation(sim, cloth) -def run_simulation(sim: SimulationManager, cloth: ClothObject) -> None: +def run_simulation(sim: SimulationManager, cloth: SurfaceDeformableObject) -> None: """Run the simulation loop. Args: sim: The SimulationManager instance to run - soft_obj: soft object + cloth: The cloth object to simulate. """ - # Initialize GPU physics - sim.init_gpu_physics() - - step_count = 0 - try: - last_time = time.time() + step_count = 0 + last_time = time.perf_counter() last_step = 0 while True: # Update physics simulation sim.update(step=1) step_count += 1 - # Print FPS every second if step_count % 100 == 0: - current_time = time.time() + current_time = time.perf_counter() elapsed = current_time - last_time fps = ( sim.num_envs * (step_count - last_step) / elapsed - if elapsed > 0 - else 0 + if elapsed > 0.0 + else 0.0 ) print(f"[INFO]: Simulation step: {step_count}, FPS: {fps:.2f}") last_time = current_time last_step = step_count - if step_count % 500 == 0: - cloth.reset() except KeyboardInterrupt: print("\n[INFO]: Stopping simulation...") diff --git a/scripts/tutorials/sim/create_rigid_constraint.py b/scripts/tutorials/sim/create_rigid_constraint.py index 6a7a629e9..990417dfe 100644 --- a/scripts/tutorials/sim/create_rigid_constraint.py +++ b/scripts/tutorials/sim/create_rigid_constraint.py @@ -30,8 +30,9 @@ from embodichain.lab.sim.cfg import ( RigidObjectCfg, RigidConstraintCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, RenderCfg, + physics_cfg_for_backend, ) from embodichain.lab.sim.shapes import CubeCfg @@ -64,21 +65,26 @@ def main(): height=1080, headless=args.headless, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device=args.device, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), num_envs=args.num_envs, arena_space=3.0, + physics_cfg=physics_cfg_for_backend(args.physics), visualization=visualization_cfg_from_args(args), ) sim = SimulationManager(sim_cfg) # Shared physics attributes for the two cubes. - physics_attrs = RigidBodyAttributesCfg( - mass=0.2, - dynamic_friction=0.5, - static_friction=0.5, - restitution=0.1, + physics_attrs = RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 0.2}, + "material_props": { + "dynamic_friction": 0.5, + "static_friction": 0.5, + "restitution": 0.1, + }, + } ) # Add two dynamic cubes to the scene. cube_a starts higher than cube_b so @@ -101,8 +107,7 @@ def main(): ) ) - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + sim.prepare() print("[INFO]: Scene setup complete with two cubes (cube_a, cube_b).") diff --git a/scripts/tutorials/sim/create_rigid_object_group.py b/scripts/tutorials/sim/create_rigid_object_group.py index 08f3aac73..ac8c6cd17 100644 --- a/scripts/tutorials/sim/create_rigid_object_group.py +++ b/scripts/tutorials/sim/create_rigid_object_group.py @@ -25,8 +25,12 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim.cfg import ( + RigidBodyPhysicsCfg, + RenderCfg, + physics_cfg_for_backend, +) from embodichain.lab.visualization import visualization_cfg_from_args -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RenderCfg from embodichain.lab.sim.shapes import CubeCfg from embodichain.lab.sim.objects import ( RigidObjectGroup, @@ -51,10 +55,11 @@ def main(): height=1080, headless=True, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device=args.device, + device=args.device, render_cfg=RenderCfg( renderer=args.renderer ), # Enable ray tracing for better visuals + physics_cfg=physics_cfg_for_backend(args.physics), num_envs=args.num_envs, arena_space=3.0, visualization=visualization_cfg_from_args(args), @@ -63,11 +68,15 @@ def main(): # Create the simulation instance sim = SimulationManager(sim_cfg) - physics_attrs = RigidBodyAttributesCfg( - mass=1.0, - dynamic_friction=0.5, - static_friction=0.5, - restitution=0.1, + physics_attrs = RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 1.0}, + "material_props": { + "dynamic_friction": 0.5, + "static_friction": 0.5, + "restitution": 0.1, + }, + } ) # Add objects to the scene @@ -102,6 +111,7 @@ def main(): print("[INFO]: Press Ctrl+C to stop the simulation") # Open window when the scene has been set up + sim.prepare() if not args.headless: sim.open_window() @@ -116,10 +126,6 @@ def run_simulation(sim: SimulationManager): sim: The SimulationManager instance to run """ - # Initialize GPU physics if using CUDA - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - step_count = 0 try: diff --git a/scripts/tutorials/sim/create_robot.py b/scripts/tutorials/sim/create_robot.py index e4dd591df..9106f3798 100644 --- a/scripts/tutorials/sim/create_robot.py +++ b/scripts/tutorials/sim/create_robot.py @@ -35,6 +35,7 @@ from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, JointDrivePropertiesCfg, RobotCfg, URDFCfg, @@ -54,15 +55,24 @@ def main(): description="Create and simulate a robot in SimulationManager" ) add_env_launcher_args_to_parser(parser) + parser.add_argument( + "--max-steps", + type=int, + default=None, + help="Stop after this many physics steps (default: run until interrupted).", + ) args = parser.parse_args() + if args.max_steps is not None and args.max_steps < 1: + parser.error("--max-steps must be at least 1") # Initialize simulation print("Creating simulation...") config = SimulationManagerCfg( headless=True, - sim_device=args.device, + device=args.device, arena_space=3.0, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), physics_dt=1.0 / 100.0, num_envs=args.num_envs, visualization=visualization_cfg_from_args(args), @@ -72,16 +82,16 @@ def main(): # Create robot configuration robot = create_robot(sim) - # Initialize GPU physics if using CUDA - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + # Materialize the declared scene before accessing robot metadata. + sim.prepare() + print(f"Robot created successfully with {robot.dof} joints") # Open visualization window if not headless if not args.headless: sim.open_window() # Run simulation loop - run_simulation(sim, robot) + run_simulation(sim, robot, max_steps=args.max_steps) def create_robot(sim): @@ -127,21 +137,45 @@ def create_robot(sim): ] ), control_parts=CONTROL_PARTS, - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", stiffness={"joint[1-6]": 1e4, "LEFT_.*": 1e3}, - damping={"joint[1-6]": 1e3, "LEFT_.*": 1e2}, + damping={"joint[1-6]": 1.5e3, "LEFT_.*": 1e2}, + max_effort={"joint[1-6]": 1e4, "LEFT_.*": 1e4}, ), ) # Add robot to simulation robot: Robot = sim.add_robot(cfg=cfg) - print(f"Robot created successfully with {robot.dof} joints") - return robot -def run_simulation(sim: SimulationManager, robot: Robot): +def _expand_mimic_targets( + robot: Robot, joint_ids: list[int], joint_targets: torch.Tensor +) -> torch.Tensor: + """Expand active-joint targets into mimic-consistent articulation targets.""" + + targets = robot.get_qpos(target=True).clone() + targets[:, joint_ids] = joint_targets + + for mimic_id, parent_id, multiplier, offset in zip( + robot.mimic_ids, + robot.mimic_parents, + robot.mimic_multipliers, + robot.mimic_offsets, + ): + if mimic_id is None or parent_id is None: + continue + targets[:, mimic_id] = offset + multiplier * targets[:, parent_id] + + limits = robot.body_data.qpos_limits + return targets.clamp(min=limits[..., 0], max=limits[..., 1]) + + +def run_simulation( + sim: SimulationManager, robot: Robot, max_steps: int | None = None +) -> None: """Run the simulation loop with robot control.""" print("Starting simulation...") @@ -170,14 +204,29 @@ def run_simulation(sim: SimulationManager, robot: Robot): # Get joint IDs for the hand. hand_joint_ids = robot.get_joint_ids("hand") - # Define hand open and close positions based on joint limits. - hand_position_open = robot.body_data.qpos_limits[:, hand_joint_ids, 1] - hand_position_close = robot.body_data.qpos_limits[:, hand_joint_ids, 0] + active_hand_joint_ids = robot.get_joint_ids("hand", remove_mimic=True) + # Drive mimic joints toward the pose implied by their active parent instead of + # sending each joint to its independent limit. Newton keeps drives on mimic + # joints, so inconsistent targets otherwise compete with the mimic constraints. + hand_position_open = _expand_mimic_targets( + robot, + active_hand_joint_ids, + robot.body_data.qpos_limits[:, active_hand_joint_ids, 1], + )[:, hand_joint_ids] + hand_position_close = _expand_mimic_targets( + robot, + active_hand_joint_ids, + robot.body_data.qpos_limits[:, active_hand_joint_ids, 0], + )[:, hand_joint_ids] + + # The reset pose is zero for every DOF, but this hand has non-zero mimic + # offsets. Start from a valid closed pose so the initial state and drive + # targets satisfy the same mimic equations. + robot.set_qpos(qpos=hand_position_close, joint_ids=hand_joint_ids, target=False) + robot.set_qpos(qpos=hand_position_close, joint_ids=hand_joint_ids) try: - while True: - # Update physics - sim.update(step=1) + while max_steps is None or step_count < max_steps: cycle_step = step_count % ACTION_CYCLE_STEPS if cycle_step == 0: @@ -196,6 +245,9 @@ def run_simulation(sim: SimulationManager, robot: Robot): robot.set_qpos(qpos=hand_position_open, joint_ids=hand_joint_ids) print(f"Opening hand") + # Apply commands before advancing physics so both backends observe the + # target change on the same simulation step. + sim.update(step=1) step_count += 1 except KeyboardInterrupt: diff --git a/scripts/tutorials/sim/create_scene.py b/scripts/tutorials/sim/create_scene.py index fa4d82ea5..f8b6fdc0f 100644 --- a/scripts/tutorials/sim/create_scene.py +++ b/scripts/tutorials/sim/create_scene.py @@ -25,8 +25,14 @@ import time from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RenderCfg -from embodichain.lab.sim.shapes import CubeCfg, MeshCfg +from embodichain.lab.sim.cfg import ( + MassPropertiesCfg, + RenderCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, + physics_cfg_for_backend, +) +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, MeshCollisionCfg from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.visualization import visualization_cfg_from_args @@ -42,7 +48,7 @@ def main() -> None: ) add_env_launcher_args_to_parser(parser) parser.add_argument( - "--record-steps", + "--max_steps", type=int, default=1000, help=( @@ -68,7 +74,8 @@ def main() -> None: height=1080, headless=True, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device=args.device, + device=args.device, + physics_cfg=physics_cfg_for_backend(args.physics), render_cfg=RenderCfg( renderer=args.renderer, ), @@ -86,11 +93,13 @@ def main() -> None: uid="cube", shape=CubeCfg(size=[0.1, 0.1, 0.1]), body_type="dynamic", - attrs=RigidBodyAttributesCfg( - mass=1.0, - dynamic_friction=0.5, - static_friction=0.5, - restitution=0.1, + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=0.1), + material_props=RigidBodyMaterialCfg( + dynamic_friction=0.5, + static_friction=0.5, + restitution=0.1, + ), ), init_pos=[0, 0.0, 1.0], ) @@ -101,17 +110,26 @@ def main() -> None: chair: RigidObject = sim.add_rigid_object( cfg=RigidObjectCfg( uid="chair", - shape=MeshCfg(fpath=path), + shape=MeshCfg( + fpath=path, + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=32, + ), + ), body_type="dynamic", - attrs=RigidBodyAttributesCfg( - mass=3.0, + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=10.0), ), body_scale=[0.5, 0.5, 0.5], - init_pos=[0.0, 0.0, 0.2], - init_rot=[90.0, 0.0, 0.0], + init_pos=[0.0, 0.0, 0.51], + init_rot=[0.0, 0.0, 0.0], ) ) + # Materialize the complete initial scene before exposing it to the viewer. + sim.prepare() + print("[INFO]: Scene setup complete!") print(f"[INFO]: Running simulation with {args.num_envs} environment(s)") print("[INFO]: Press Ctrl+C to stop the simulation") @@ -133,13 +151,10 @@ def main() -> None: print( "[INFO]: The output path is reported by `SimulationManager.start_window_record()`." ) - print(f"[INFO]: Running {args.record_steps} steps before exporting the video") + print(f"[INFO]: Running {args.max_steps} steps before exporting the video") # Run the simulation - run_simulation( - sim, - max_steps=args.record_steps if args.headless else None, - ) + run_simulation(sim, max_steps=args.max_steps) def run_simulation( @@ -153,10 +168,6 @@ def run_simulation( max_steps: Optional maximum number of simulation steps to execute. """ - # Initialize GPU physics if using CUDA - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - step_count = 0 try: @@ -167,6 +178,9 @@ def run_simulation( sim.update(step=1) step_count += 1 + if max_steps is not None and step_count >= max_steps: + break + # Print FPS every second if step_count % 100 == 0: current_time = time.time() diff --git a/scripts/tutorials/sim/create_sensor.py b/scripts/tutorials/sim/create_sensor.py index 60fa82c3e..42a0aa482 100644 --- a/scripts/tutorials/sim/create_sensor.py +++ b/scripts/tutorials/sim/create_sensor.py @@ -37,6 +37,7 @@ from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, JointDrivePropertiesCfg, RobotCfg, URDFCfg, @@ -98,9 +99,10 @@ def main() -> None: print("Creating simulation...") config = SimulationManagerCfg( headless=True, - sim_device=args.device, + device=args.device, arena_space=3.0, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), physics_dt=1.0 / 100.0, num_envs=args.num_envs, visualization=visualization_cfg_from_args(args), @@ -110,8 +112,6 @@ def main() -> None: # Create robot configuration robot = create_robot(sim) - sensor = create_sensor(sim, args) - # Add a cube to the scene cube_cfg = RigidObjectCfg( uid="cube", @@ -121,9 +121,12 @@ def main() -> None: ) sim.add_rigid_object(cfg=cube_cfg) - # Initialize GPU physics if using CUDA - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + # Materialize all physical assets before reading robot metadata or + # constructing render-only sensors. + sim.prepare() + print(f"Robot created successfully with {robot.dof} joints") + + sensor = create_sensor(sim, args) # Open visualization window if not headless if not args.headless: @@ -147,7 +150,8 @@ def create_sensor(sim: SimulationManager, args): # extrinsics params pos = [0.09, 0.05, 0.04] - quat = R.from_euler("xyz", [-35, 135, 0], degrees=True).as_quat().tolist() + # CameraCfg uses xyzw; this rotation preserves the intended wrist-camera view. + quat = R.from_euler("xyz", [180, -45, 35], degrees=True).as_quat().tolist() # If attach_sensor is True, attach to robot end-effector; otherwise, place it in the scene if args.attach_sensor: @@ -156,7 +160,6 @@ def create_sensor(sim: SimulationManager, args): parent = None pos = [1.2, -0.2, 1.5] quat = R.from_euler("xyz", [0, 180, 0], degrees=True).as_quat().tolist() - quat = [quat[3], quat[0], quat[1], quat[2]] # Convert to (w, x, y, z) # create camera sensor and attach to robot end-effector camera: Camera = sim.add_sensor( @@ -223,17 +226,17 @@ def create_robot(sim): ] ), control_parts=CONTROL_PARTS, - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", stiffness={"joint[1-6]": 1e4, "LEFT_.*": 1e3}, - damping={"joint[1-6]": 1e3, "LEFT_.*": 1e2}, + damping={"joint[1-6]": 1.5e3, "LEFT_.*": 1e2}, + max_effort={"joint[1-6]": 1e4, "LEFT_.*": 1e4}, ), ) # Add robot to simulation robot: Robot = sim.add_robot(cfg=cfg) - print(f"Robot created successfully with {robot.dof} joints") - return robot diff --git a/scripts/tutorials/sim/create_softbody.py b/scripts/tutorials/sim/create_softbody.py index 83b15b662..ba237c70e 100644 --- a/scripts/tutorials/sim/create_softbody.py +++ b/scripts/tutorials/sim/create_softbody.py @@ -23,23 +23,24 @@ import argparse import time + from dexsim.utility.path import get_resources_data_path from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.visualization import visualization_cfg_from_args from embodichain.lab.sim.cfg import ( + NewtonCollisionPipelineCfg, + NewtonPhysicsCfg, RenderCfg, - SoftbodyVoxelAttributesCfg, - SoftbodyPhysicalAttributesCfg, + VolumeDeformableObjectCfg, + VolumeDeformableMeshingCfg, + VolumeDeformablePhysicsCfg, ) from embodichain.lab.sim.shapes import MeshCfg -from embodichain.lab.sim.objects import ( - SoftObject, - SoftObjectCfg, -) +from embodichain.lab.sim.objects import VolumeDeformableObject -def main(): +def main() -> None: """Main function to create and run the simulation scene.""" # Parse command line arguments @@ -47,19 +48,44 @@ def main(): description="Create a simulation scene with SimulationManager" ) add_env_launcher_args_to_parser(parser) + parser.set_defaults(device="cuda", physics="newton") args = parser.parse_args() + if args.physics != "newton": + parser.error("Soft bodies require --physics newton.") + if not str(args.device).startswith("cuda"): + parser.error("Soft bodies require a CUDA device.") # Configure the simulation sim_cfg = SimulationManagerCfg( width=1920, height=1080, - headless=True, + headless=args.headless, num_envs=args.num_envs, + arena_space=args.arena_space, + gpu_id=args.gpu_id, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device="cuda", # soft simulation only supports cuda device + device=args.device, render_cfg=RenderCfg( renderer=args.renderer ), # Enable ray tracing for better visuals + physics_cfg=NewtonPhysicsCfg( + num_substeps=6, + solver_cfg={ + "solver_type": "vbd", + "iterations": 8, + "particle_enable_self_contact": False, + "particle_self_contact_radius": 0.001, + "particle_self_contact_margin": 0.001, + "particle_topological_contact_filter_threshold": 3, + "particle_enable_tile_solve": True, + "soft_contact_ke": 5.0e4, + "soft_contact_kd": 1.0e-3, + "soft_contact_mu": 1.5, + }, + collision_cfg=NewtonCollisionPipelineCfg( + soft_contact_margin=0.002, + ), + ), visualization=visualization_cfg_from_args(args), ) @@ -69,28 +95,32 @@ def main(): print("[INFO]: Scene setup complete!") # add softbody to the scene - cow: SoftObject = sim.add_soft_object( - cfg=SoftObjectCfg( + cow: VolumeDeformableObject = sim.add_deformable_object( + cfg=VolumeDeformableObjectCfg( uid="cow", shape=MeshCfg( fpath=get_resources_data_path("Model", "cow", "cow.obj"), ), - init_pos=[0.0, 0.0, 3.0], - voxel_attr=SoftbodyVoxelAttributesCfg( - simulation_mesh_resolution=8, - maximal_edge_length=0.5, + init_pos=[0.0, 5.0, 3.0], + particle_radius=0.01, + meshing=VolumeDeformableMeshingCfg( + triangle_remesh_resolution=24, + simulation_mesh_resolution=16, + voxel_num_relaxation_iters=5, ), - physical_attr=SoftbodyPhysicalAttributesCfg( - youngs=1e6, - poissons=0.45, - density=100, - dynamic_friction=0.1, - min_position_iters=30, + attrs=VolumeDeformablePhysicsCfg( + # Equivalent to the DexSim demo's k_mu=1e4 and k_lambda=5e4. + youngs=2.833333333e4, + poissons=5.0 / 12.0, + density=50.0, + elasticity_damping=2.0e-3, ), ), ) print("[INFO]: Add soft object complete!") + sim.prepare() + # Open window when the scene has been set up if not args.headless: sim.open_window() @@ -102,7 +132,7 @@ def main(): run_simulation(sim, cow) -def run_simulation(sim: SimulationManager, soft_obj: SoftObject) -> None: +def run_simulation(sim: SimulationManager, soft_obj: VolumeDeformableObject) -> None: """Run the simulation loop. Args: @@ -110,9 +140,6 @@ def run_simulation(sim: SimulationManager, soft_obj: SoftObject) -> None: soft_obj: soft object """ - # Initialize GPU physics - sim.init_gpu_physics() - step_count = 0 try: diff --git a/scripts/tutorials/sim/export_usd.py b/scripts/tutorials/sim/export_usd.py index a8c7de46b..a96f7aede 100644 --- a/scripts/tutorials/sim/export_usd.py +++ b/scripts/tutorials/sim/export_usd.py @@ -28,13 +28,14 @@ from embodichain.lab.sim.objects import Robot, RigidObject from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, LightCfg, JointDrivePropertiesCfg, RigidObjectCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, ArticulationCfg, ) -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.data import get_data_path from embodichain.utils import logger @@ -67,8 +68,9 @@ def initialize_simulation(args) -> SimulationManager: """ config = SimulationManagerCfg( headless=True, - sim_device=args.device, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), physics_dt=1.0 / 100.0, num_envs=1, arena_space=2.5, @@ -180,11 +182,12 @@ def create_table(sim: SimulationManager) -> RigidObject: uid="table", shape=MeshCfg( fpath=get_data_path("MultiW1Data/table_a.obj"), + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=8, + ), ), - attrs=RigidBodyAttributesCfg( - mass=0.5, - ), - max_convex_hull_num=8, + attrs=RigidBodyPhysicsCfg.from_dict({"mass_props": {"mass": 0.5}}), body_type="kinematic", init_pos=[1.1, -0.5, 0.08], init_rot=[0.0, 0.0, 0.0], @@ -206,12 +209,11 @@ def create_caffe(sim: SimulationManager) -> Robot: container_cfg = ArticulationCfg( uid="caffe", fpath=get_data_path("MultiW1Data/cafe/cafe.urdf"), + asset_physics_mode="overlay", init_pos=[1.05, -0.5, 0.79], init_rot=[0, 0, -30], - attrs=RigidBodyAttributesCfg( - mass=1.0, - ), - drive_pros=JointDrivePropertiesCfg( + attrs=RigidBodyPhysicsCfg.from_dict({"mass_props": {"mass": 1.0}}), + joint_drive_props=JointDrivePropertiesCfg( stiffness=1.0, damping=0.1, max_effort=100.0, drive_type="force" ), ) @@ -235,10 +237,7 @@ def create_cup(sim: SimulationManager) -> RigidObject: shape=MeshCfg( fpath=get_data_path("MultiW1Data/paper_cup_2.obj"), ), - attrs=RigidBodyAttributesCfg( - mass=0.3, - ), - max_convex_hull_num=1, + attrs=RigidBodyPhysicsCfg.from_dict({"mass_props": {"mass": 0.3}}), body_type="dynamic", init_pos=[0.86, -0.76, 0.841], init_rot=[0.0, 0.0, 0.0], @@ -261,7 +260,9 @@ def main(): caffe = create_caffe(sim) cup = create_cup(sim) - sim.export_usd("w1_coffee_scene.usda") + sim.prepare() + + sim.export_usd("w1_coffee_scene.usd") logger.log_info("Scene exported successfully.") diff --git a/scripts/tutorials/sim/gizmo_robot.py b/scripts/tutorials/sim/gizmo_robot.py index efa3c0f96..cf1fd6fd3 100644 --- a/scripts/tutorials/sim/gizmo_robot.py +++ b/scripts/tutorials/sim/gizmo_robot.py @@ -28,6 +28,7 @@ from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim.cfg import ( RenderCfg, + physics_cfg_for_backend, RobotCfg, URDFCfg, JointDrivePropertiesCfg, @@ -54,8 +55,9 @@ def main(): height=1080, headless=True, physics_dt=1.0 / 100.0, - sim_device=args.device, + device=args.device, render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), visualization=visualization_cfg_from_args(args), robot_ik_gizmo=GizmoCfg(ik_start_enabled=True), ) @@ -83,7 +85,8 @@ def main(): dt=0.1, ) }, - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", stiffness={"Joint[1-6]": 1e4}, damping={"Joint[1-6]": 1e3}, ), @@ -98,6 +101,8 @@ def main(): dtype=torch.float32, device=sim.device, ) + + sim.prepare() joint_ids = robot.get_joint_ids("arm") robot.set_qpos(qpos=initial_qpos, joint_ids=joint_ids, target=False) robot.set_qpos(qpos=initial_qpos, joint_ids=joint_ids) diff --git a/scripts/tutorials/sim/import_usd.py b/scripts/tutorials/sim/import_usd.py index b30639bb1..968840ff2 100644 --- a/scripts/tutorials/sim/import_usd.py +++ b/scripts/tutorials/sim/import_usd.py @@ -28,7 +28,13 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.visualization import visualization_cfg_from_args from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg, RenderCfg +from embodichain.lab.sim.cfg import ( + MassPropertiesCfg, + RenderCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, + physics_cfg_for_backend, +) from embodichain.lab.sim.shapes import CubeCfg, MeshCfg from embodichain.lab.sim.objects import ( RigidObject, @@ -55,10 +61,11 @@ def main(): height=1080, headless=True, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device=args.device, + device=args.device, render_cfg=RenderCfg( renderer=args.renderer, ), # Enable ray tracing for better visuals + physics_cfg=physics_cfg_for_backend(args.physics), num_envs=1, arena_space=3.0, visualization=visualization_cfg_from_args(args), @@ -72,11 +79,13 @@ def main(): uid="cube", shape=CubeCfg(size=[0.1, 0.1, 0.1]), body_type="dynamic", - attrs=RigidBodyAttributesCfg( - mass=1.0, - dynamic_friction=0.5, - static_friction=0.5, - restitution=0.1, + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), + material_props=RigidBodyMaterialCfg( + dynamic_friction=0.5, + static_friction=0.5, + restitution=0.1, + ), ), init_pos=[0.0, 0.0, 1.0], ) @@ -90,7 +99,7 @@ def main(): shape=MeshCfg(fpath=sugar_box_path), body_type="dynamic", init_pos=[0.2, 0.2, 1.0], - use_usd_properties=True, + asset_physics_mode="preserve", ) ) @@ -103,11 +112,12 @@ def main(): fpath=h1_path, build_pk_chain=False, init_pos=[-0.2, -0.2, 1.05], - use_usd_properties=False, + asset_physics_mode="overlay", ) ) # Open window when the scene has been set up + sim.prepare() if not args.headless: sim.open_window() @@ -125,10 +135,6 @@ def run_simulation(sim: SimulationManager): sim: The SimulationManager instance to run """ - # Initialize GPU physics if using CUDA - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - step_count = 0 try: diff --git a/scripts/tutorials/sim/motion_generator.py b/scripts/tutorials/sim/motion_generator.py index c04a2c431..a069482c8 100644 --- a/scripts/tutorials/sim/motion_generator.py +++ b/scripts/tutorials/sim/motion_generator.py @@ -25,8 +25,8 @@ from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import RenderCfg, physics_cfg_for_backend from embodichain.lab.visualization import visualization_cfg_from_args -from embodichain.lab.sim.cfg import RenderCfg from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.motion.motion_generator import ( MotionGenCfg, @@ -228,7 +228,8 @@ def main() -> None: height=RECORD_HEIGHT, headless=True, physics_dt=1.0 / 100.0, - sim_device=args.device, + device=args.device, + physics_cfg=physics_cfg_for_backend(args.physics), render_cfg=RenderCfg(renderer=args.renderer), num_envs=args.num_envs, arena_space=args.arena_space, @@ -239,8 +240,7 @@ def main() -> None: robot: Robot = sim.add_robot(cfg=CobotMagicCfg.from_dict({"uid": "CobotMagic"})) arm_name = "left_arm" - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + sim.prepare() if not args.headless: sim.open_window() diff --git a/scripts/tutorials/sim/open_drawer.py b/scripts/tutorials/sim/open_drawer.py index 2400cc7b5..76ad9dcb2 100644 --- a/scripts/tutorials/sim/open_drawer.py +++ b/scripts/tutorials/sim/open_drawer.py @@ -20,6 +20,7 @@ import argparse from collections.abc import Sequence +from typing import Literal import torch @@ -28,9 +29,15 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.cfg import ( ArticulationCfg, + ArticulationRootPropertiesCfg, JointDrivePropertiesCfg, + LinkPhysicsOverrideCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + PhysicsBackendCfg, RenderCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, + physics_cfg_for_backend, ) from embodichain.lab.sim.objects import Articulation, Robot from embodichain.lab.sim.motion.motion_generator import ( @@ -61,10 +68,15 @@ ARM_NAME = "arm" HAND_NAME = "hand" HANDLE_LINK_NAME = "handle_xpos" +DRAWER_CONTACT_LINK_NAME = "inner_box" +LEFT_FINGER_LINK_NAME = "fr3_leftfinger" +RIGHT_FINGER_LINK_NAME = "fr3_rightfinger" DRAWER_ASSET = "SlidingBoxDrawer/SlidingBoxDrawer.urdf" APPROACH_DISTANCE = 0.10 PULL_DISTANCE = 0.16 +NEWTON_GRASP_CONTACT_STIFFNESS = 4.0e4 +NEWTON_GRASP_CONTACT_DAMPING = 4.0e2 DRAWER_SUCCESS_THRESHOLD = 0.10 HALF_OPEN_FRACTION = 0.5 HALF_OPEN_TOLERANCE = 0.02 @@ -77,6 +89,21 @@ ) +def _newton_grasp_contact_override( + link_names_expr: str, +) -> LinkPhysicsOverrideCfg: + """Build the Newton contact material used at the drawer grasp.""" + return LinkPhysicsOverrideCfg( + link_names_expr=[link_names_expr], + attrs=RigidBodyPhysicsCfg( + material_props=NewtonRigidBodyMaterialCfg( + ke=NEWTON_GRASP_CONTACT_STIFFNESS, + kd=NEWTON_GRASP_CONTACT_DAMPING, + ) + ), + ) + + def create_scene(sim: SimulationManager) -> tuple[Robot, Articulation]: """Add a Franka Panda and a passive sliding drawer to the scene. @@ -96,31 +123,46 @@ def create_scene(sim: SimulationManager) -> tuple[Robot, Articulation]: "uid": "tutorial_franka", "robot_type": "panda", "attrs": { - "static_friction": 1.0, - "dynamic_friction": 1.0, + "material_props": { + "static_friction": 1.0, + "dynamic_friction": 1.0, + }, }, } ) + if sim.is_newton_backend: + robot_cfg.link_attrs = { + **(robot_cfg.link_attrs or {}), + "newton_gripper_contacts": _newton_grasp_contact_override( + f"(?:{LEFT_FINGER_LINK_NAME}|{RIGHT_FINGER_LINK_NAME})" + ), + } robot = sim.add_robot(cfg=robot_cfg) if robot is None: raise RuntimeError("Failed to add the Franka Panda robot.") # Keep the drawer base fixed while leaving its prismatic joint passive. The # 180-degree yaw makes the drawer's opening direction point toward Franka. - drawer = sim.add_articulation( - cfg=ArticulationCfg( - uid="drawer", - fpath=get_data_path(DRAWER_ASSET), - init_pos=(0.72, 0.0, 0.42), - init_rot=(0.0, 0.0, 180.0), - fix_base=True, - drive_pros=JointDrivePropertiesCfg(drive_type="none"), - attrs=RigidBodyAttributesCfg( - static_friction=1.0, - dynamic_friction=1.0, - ), - ) + drawer_cfg = ArticulationCfg( + uid="drawer", + fpath=get_data_path(DRAWER_ASSET), + asset_physics_mode="overlay", + init_pos=(0.72, 0.0, 0.42), + init_rot=(0.0, 0.0, 180.0), + root_props=ArticulationRootPropertiesCfg(fixed_base=True), + joint_drive_props=JointDrivePropertiesCfg(drive_type="none"), + attrs=RigidBodyPhysicsCfg.from_dict( + {"material_props": {"static_friction": 1.0, "dynamic_friction": 1.0}} + ), ) + if sim.is_newton_backend: + # The handle marker has no geometry; its collision belongs to inner_box. + drawer_cfg.link_attrs = { + "newton_handle_contacts": _newton_grasp_contact_override( + DRAWER_CONTACT_LINK_NAME + ) + } + drawer = sim.add_articulation(cfg=drawer_cfg) return robot, drawer @@ -230,6 +272,34 @@ def play_arm_trajectory( sim.update(step=physics_steps_per_waypoint) +def _move_arm_to_poses( + sim: SimulationManager, + robot: Robot, + motion_generator: MotionGenerator, + target_poses: Sequence[torch.Tensor], + *, + sample_count: int, + physics_steps_per_waypoint: int = 4, + wait_for_input: bool = False, +) -> None: + """Plan and execute arm motion through Cartesian target poses.""" + start_qpos = robot.get_qpos(name=ARM_NAME) + trajectory = generate_arm_trajectory( + motion_generator, + qpos_waypoints=solve_ik_waypoints(robot, target_poses, start_qpos), + start_qpos=start_qpos, + sample_count=sample_count, + ) + if wait_for_input: + input("[READY]: Trajectory planned. Press Enter to start execution...") + play_arm_trajectory( + sim, + robot, + trajectory, + physics_steps_per_waypoint=physics_steps_per_waypoint, + ) + + def move_gripper( sim: SimulationManager, robot: Robot, @@ -324,21 +394,14 @@ def open_drawer( approach_pose = grasp_pose.clone() approach_pose[:, :3, 3] -= grasp_pose[:, :3, 2] * APPROACH_DISTANCE - start_qpos = robot.get_qpos(name=ARM_NAME) - approach_waypoints = solve_ik_waypoints( + _move_arm_to_poses( + sim, robot, - target_poses=[approach_pose, grasp_pose], - start_qpos=start_qpos, - ) - approach_trajectory = generate_arm_trajectory( motion_generator, - qpos_waypoints=approach_waypoints, - start_qpos=start_qpos, + [approach_pose, grasp_pose], sample_count=60, + wait_for_input=wait_for_input, ) - if wait_for_input: - input("[READY]: Trajectory planned. Press Enter to start execution...") - play_arm_trajectory(sim, robot, approach_trajectory) # Close around the handle, then allow contacts to settle before pulling. move_gripper(sim, robot, hand_closed_qpos) @@ -350,22 +413,12 @@ def open_drawer( pull_pose = grasped_handle_pose.clone() pull_pose[:, :3, 3] -= grasped_handle_pose[:, :3, 2] * PULL_DISTANCE - pull_start_qpos = robot.get_qpos(name=ARM_NAME) - pull_waypoints = solve_ik_waypoints( + _move_arm_to_poses( + sim, robot, - target_poses=[pull_pose], - start_qpos=pull_start_qpos, - ) - pull_trajectory = generate_arm_trajectory( motion_generator, - qpos_waypoints=pull_waypoints, - start_qpos=pull_start_qpos, + [pull_pose], sample_count=80, - ) - play_arm_trajectory( - sim, - robot, - pull_trajectory, physics_steps_per_waypoint=5, ) sim.update(step=50) @@ -390,36 +443,27 @@ def open_drawer( push_pose = pushed_handle_pose.clone() push_pose[:, :3, 3] += pushed_handle_pose[:, :3, 2] * push_distance.unsqueeze(-1) - push_start_qpos = robot.get_qpos(name=ARM_NAME) - push_waypoints = solve_ik_waypoints( + _move_arm_to_poses( + sim, robot, - target_poses=[push_pose], - start_qpos=push_start_qpos, - ) - push_trajectory = generate_arm_trajectory( motion_generator, - qpos_waypoints=push_waypoints, - start_qpos=push_start_qpos, + [push_pose], sample_count=50, - ) - play_arm_trajectory( - sim, - robot, - push_trajectory, physics_steps_per_waypoint=5, ) sim.update(step=50) drawer_qpos = drawer.get_qpos() final_opening = drawer_qpos[:, 0] + half_open_error = torch.abs(final_opening - half_open_target) print( - "[INFO]: Drawer opening after half push (m): " - f"{final_opening.detach().cpu().tolist()}", + "[INFO]: Drawer opening after half push (m): final=" + f"{final_opening.detach().cpu().tolist()}, target=" + f"{half_open_target.detach().cpu().tolist()}, abs_error=" + f"{half_open_error.detach().cpu().tolist()}", flush=True, ) - if not torch.all( - torch.abs(final_opening - half_open_target) <= HALF_OPEN_TOLERANCE - ).item(): + if not torch.all(half_open_error <= HALF_OPEN_TOLERANCE).item(): raise RuntimeError( "The drawer did not return to half of its pulled opening. " f"Expected an error no greater than {HALF_OPEN_TOLERANCE:.2f} m." @@ -427,6 +471,24 @@ def open_drawer( return drawer_qpos +def _tutorial_physics_cfg( + backend: Literal["default", "newton"], +) -> PhysicsBackendCfg: + """Build the physics configuration used by this tutorial.""" + physics_cfg = physics_cfg_for_backend(backend) + if isinstance(physics_cfg, NewtonPhysicsCfg): + # Use a finer step and multi-point contacts for the Newton grasp. Leave + # per-world capacities auto-sized to avoid oversized CUDA Graph buffers + # when the scene is replicated across many environments. + physics_cfg.num_substeps = 10 + physics_cfg.solver_cfg = { + "solver_type": "mujoco_warp", + "cone": "elliptic", + "enable_multiccd": True, + } + return physics_cfg + + def main() -> None: """Run the Franka drawer-manipulation tutorial.""" parser = argparse.ArgumentParser( @@ -466,15 +528,23 @@ def main() -> None: if args.record_save_path is not None and not args.headless: parser.error("--record-save-path requires --headless") + open_native_window = not args.headless and not args.viser + + # PytorchSolver samples multiple IK seeds; make the tutorial trajectory + # reproducible across repeated runs of the same backend. + torch.manual_seed(0) + + # Construct the World without a window so Spawn can finish first. sim = SimulationManager( SimulationManagerCfg( width=RECORD_WIDTH, height=RECORD_HEIGHT, - headless=args.headless, + headless=True, sim_device=args.device, num_envs=args.num_envs, arena_space=args.arena_space, physics_dt=1.0 / 100.0, + physics_cfg=_tutorial_physics_cfg(args.physics), render_cfg=RenderCfg(renderer=args.renderer), visualization=visualization_cfg_from_args(args), ) @@ -483,9 +553,8 @@ def main() -> None: try: robot, drawer = create_scene(sim) - if sim.is_use_gpu_physics: - sim.init_gpu_physics() - if not args.headless and not args.viser: + sim.prepare() + if open_native_window: sim.open_window() sim.update(step=5) diff --git a/scripts/tutorials/sim/srs_solver.py b/scripts/tutorials/sim/srs_solver.py index 10afcb0f5..9cc678a16 100644 --- a/scripts/tutorials/sim/srs_solver.py +++ b/scripts/tutorials/sim/srs_solver.py @@ -73,7 +73,7 @@ def main( # Keep the native window closed while planning so renderer/window # lifecycle events cannot terminate or perturb timed CUDA IK calls. headless=True, - sim_device=device, + device=device, width=2200, height=1200, visualization=visualization or VisualizationCfg(), @@ -91,6 +91,7 @@ def main( [2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.0] ) robot: Robot = sim.add_robot(cfg=robot_cfg) + sim.prepare() joint_ids = robot.get_joint_ids(arm_name) qpos_seed = torch.tensor( [[np.pi / 6, 0.0, 0.0, -np.pi / 2, 0.0, 0.0, np.pi / 6]], diff --git a/scripts/tutorials/task_program/build_and_compile.py b/scripts/tutorials/task_program/build_and_compile.py index 54281b991..31d15c0a5 100644 --- a/scripts/tutorials/task_program/build_and_compile.py +++ b/scripts/tutorials/task_program/build_and_compile.py @@ -67,11 +67,11 @@ def build_program() -> TaskProgramCfg: values=( PoseCfg( position=(0.40, -0.20, 0.10), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), ), PoseCfg( position=(0.40, 0.20, 0.10), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), ), ) ) diff --git a/scripts/tutorials/visualization/README.md b/scripts/tutorials/visualization/README.md index 5533d7234..c7f593c7a 100644 --- a/scripts/tutorials/visualization/README.md +++ b/scripts/tutorials/visualization/README.md @@ -67,8 +67,8 @@ The matching object tutorials can be launched directly: ```bash python scripts/tutorials/sim/create_rigid_object_group.py --viser -python scripts/tutorials/sim/create_softbody.py --viser -python scripts/tutorials/sim/create_cloth.py --viser +python scripts/tutorials/sim/create_softbody.py --physics newton --viser +python scripts/tutorials/sim/create_cloth.py --physics newton --viser ``` The atomic-action tutorials receive the same options through @@ -81,10 +81,10 @@ Application launchers only need to check `--headless` before calling Viser is configured. It also rejects Viser startup while the native window is already open. -Cloth uses its welded physical surface topology. DexSim does not currently -expose the PhysX soft-body collision topology, so the soft-body preview uses -a convex-hull surface over the live collision vertices. It follows deformation -but intentionally omits concave render-mesh details. +Soft bodies and cloth use the live render surface exposed by their DexSim 0.5 +typed Newton particle-set handles. Volume deformables also retain their +tetrahedral collision-surface topology for physics consumers, while Viser +intentionally publishes the render topology. ## Remote access diff --git a/scripts/tutorials/visualization/viser_scene.py b/scripts/tutorials/visualization/viser_scene.py index a3d329932..9350391fc 100644 --- a/scripts/tutorials/visualization/viser_scene.py +++ b/scripts/tutorials/visualization/viser_scene.py @@ -159,8 +159,7 @@ def main() -> None: build_pk_chain=False, ) ) - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + sim.prepare() visualization_cfg = VisualizationCfg( backend="viser", diff --git a/tests/data_pipeline/test_online_data.py b/tests/data_pipeline/test_online_data.py index eca05afed..c2c9cf1a7 100644 --- a/tests/data_pipeline/test_online_data.py +++ b/tests/data_pipeline/test_online_data.py @@ -56,6 +56,7 @@ from embodichain.data_pipeline.engine.data import ( OnlineDataEngine, OnlineDataEngineCfg, + _apply_worker_simulation_overrides, ) # --------------------------------------------------------------------------- @@ -77,6 +78,39 @@ # --------------------------------------------------------------------------- +@pytest.mark.no_sim +def test_worker_overrides_preserve_newton_device_default() -> None: + """Worker rendering overrides must not replace Newton's typed config.""" + from embodichain.lab.sim import SimulationManagerCfg + from embodichain.lab.sim.cfg import NewtonPhysicsCfg + + sim_cfg = SimulationManagerCfg( + physics_cfg=NewtonPhysicsCfg(num_substeps=3), + ) + + _apply_worker_simulation_overrides( + sim_cfg, + {"headless": True, "renderer": "hybrid", "gpu_id": 0}, + ) + + assert isinstance(sim_cfg.physics_cfg, NewtonPhysicsCfg) + assert sim_cfg.device == "cuda:0" + assert sim_cfg.physics_cfg.num_substeps == 3 + + +@pytest.mark.no_sim +def test_worker_overrides_apply_explicit_device() -> None: + """An authored worker device remains an explicit runtime override.""" + from embodichain.lab.sim import SimulationManagerCfg + from embodichain.lab.sim.cfg import NewtonPhysicsCfg + + sim_cfg = SimulationManagerCfg(physics_cfg=NewtonPhysicsCfg()) + + _apply_worker_simulation_overrides(sim_cfg, {"device": "cpu"}) + + assert sim_cfg.device == "cpu" + + def _make_fake_engine( buffer_size: int = BUFFER_SIZE, max_episode_steps: int = MAX_EPISODE_STEPS, diff --git a/tests/docs/test_check_api_docs.py b/tests/docs/test_check_api_docs.py index d3eb209de..abaf1db7e 100644 --- a/tests/docs/test_check_api_docs.py +++ b/tests/docs/test_check_api_docs.py @@ -71,6 +71,7 @@ def test_discover_public_modules_uses_static_all(tmp_path: Path) -> None: _write(package_path / "feature" / "__init__.py", '__all__ = ["Feature"]\n') _write(package_path / "module.py", '__all__ = ["NotPackageLevel"]\n') _write(package_path / "_private" / "__init__.py", '__all__ = ["Hidden"]\n') + _write(package_path / ".generated" / "__init__.py", "__all__ = build_exports()\n") modules = discover_public_modules((PackageRoot("sample", package_path),)) diff --git a/tests/gen_sim/gradio_ui/test_app_articraft.py b/tests/gen_sim/gradio_ui/test_app_articraft.py index 2400484f0..5474dae04 100644 --- a/tests/gen_sim/gradio_ui/test_app_articraft.py +++ b/tests/gen_sim/gradio_ui/test_app_articraft.py @@ -356,7 +356,8 @@ def start_pipeline(command: list[str]): str(artifact.resolve()), "--asset_type", "articulation", - "--use_usd_properties", + "--asset-physics-mode", + "preserve", "--viser", "--viser-host", "0.0.0.0", diff --git a/tests/gen_sim/scene_engine/test_gravity_settler.py b/tests/gen_sim/scene_engine/test_gravity_settler.py index 5829fa39a..2dffebfba 100644 --- a/tests/gen_sim/scene_engine/test_gravity_settler.py +++ b/tests/gen_sim/scene_engine/test_gravity_settler.py @@ -19,7 +19,10 @@ import pytest -from embodichain.gen_sim.scene_engine.core.scene_object import SceneObject +from embodichain.gen_sim.scene_engine.core.scene_object import ( + ObjectPhysics, + SceneObject, +) from embodichain.gen_sim.scene_engine.pipeline.utils.gravity_settler import ( GravitySettleBody, GravitySettler, @@ -79,3 +82,27 @@ def test_gravity_settler_rejects_dynamic_assets_outside_participants() -> None: dynamic_asset_ids={_ASSET_ID}, static_asset_ids=set(), ).settle() + + +@pytest.mark.parametrize( + ("max_hulls", "expected_approximation", "expected_max_hulls"), + [ + (1, "convex_hull", None), + (8, "convex_decomposition", 8), + ], +) +def test_gravity_settler_normalizes_legacy_hull_budget( + max_hulls: int, + expected_approximation: str, + expected_max_hulls: int | None, +) -> None: + collision = GravitySettler._mesh_collision_cfg( + ObjectPhysics( + body_type="dynamic", + attrs={"mass_props": {"mass": 1.0}}, + max_convex_hull_num=max_hulls, + ) + ) + + assert collision.approximation == expected_approximation + assert collision.max_hulls == expected_max_hulls diff --git a/tests/gen_sim/scene_engine/test_scene_core_and_export.py b/tests/gen_sim/scene_engine/test_scene_core_and_export.py index 87d0043b3..06e1a70d9 100644 --- a/tests/gen_sim/scene_engine/test_scene_core_and_export.py +++ b/tests/gen_sim/scene_engine/test_scene_core_and_export.py @@ -66,7 +66,10 @@ def _scene_object( def _physics(body_type: str) -> ObjectPhysics: return ObjectPhysics( body_type=body_type, # type: ignore[arg-type] - attrs={"mass": 1.0, "static_friction": 0.8}, + attrs={ + "mass_props": {"mass": 1.0}, + "material_props": {"static_friction": 0.8}, + }, max_convex_hull_num=16, ) diff --git a/tests/gen_sim/scene_engine/test_scene_edit.py b/tests/gen_sim/scene_engine/test_scene_edit.py index 72ee061a3..a95b16172 100644 --- a/tests/gen_sim/scene_engine/test_scene_edit.py +++ b/tests/gen_sim/scene_engine/test_scene_edit.py @@ -50,7 +50,7 @@ def _write_scene_export( "shape_type": "Mesh", "fpath": "mesh_assets/table/table.glb", }, - "attrs": {"mass": 1.0}, + "attrs": {"mass_props": {"mass": 1.0}}, "body_type": "kinematic", "init_pos": [0.0, 0.0, 0.0], "init_rot": [0.0, 0.0, 0.0], @@ -68,7 +68,7 @@ def _write_scene_export( "shape_type": "Mesh", "fpath": "mesh_assets/cup/cup.glb", }, - "attrs": {"mass": 1.0}, + "attrs": {"mass_props": {"mass": 1.0}}, "body_type": "dynamic", "init_pos": [1.0, -3.0, 2.0], "init_rot": [0.0, 0.0, 0.0], diff --git a/tests/gym/envs/managers/test_action_manager.py b/tests/gym/envs/managers/test_action_manager.py index c617fee58..3a0cb16b3 100644 --- a/tests/gym/envs/managers/test_action_manager.py +++ b/tests/gym/envs/managers/test_action_manager.py @@ -160,11 +160,10 @@ def test_eef_pose_term_process_action_7d(): cfg = ActionTermCfg(func=EefPoseTerm, params={"scale": 1.0, "pose_dim": 7}) term = EefPoseTerm(cfg, env) - # 7D: position + quaternion (w,x,y,z) + # 7D: position + quaternion (x,y,z,w) action = torch.zeros(2, 7) action[:, :3] = 0.1 - action[:, 3] = 1.0 # quat w - action[:, 4:7] = 0.0 # quat x,y,z (identity) + action[:, 6] = 1.0 # xyzw identity result = term.process_action(action) assert "qpos" in result diff --git a/tests/gym/envs/managers/test_event_functors.py b/tests/gym/envs/managers/test_event_functors.py index 750fa2399..ac7443150 100644 --- a/tests/gym/envs/managers/test_event_functors.py +++ b/tests/gym/envs/managers/test_event_functors.py @@ -58,17 +58,20 @@ def __init__( self.cfg.shape = Mock() self.cfg.shape.fpath = "test.obj" self.cfg.attrs = Mock() - self.cfg.attrs.mass = 1.0 + self.cfg.attrs.mass_props = Mock(mass=1.0) # Default pose at origin self._pose = torch.eye(4).unsqueeze(0).repeat(num_envs, 1, 1) self._mass = torch.ones(num_envs) * 1.0 + self._inertia = torch.ones(num_envs, 3) self._com = torch.zeros(num_envs, 3) # Mock body_data self.body_data = Mock() + self.body_data.default_mass = self._mass.clone() + self.body_data.default_inertia = self._inertia.clone() self.body_data.default_com_pose = torch.zeros(num_envs, 7) - self.body_data.default_com_pose[:, 3] = 1.0 # quaternion w + self.body_data.default_com_pose[:, 6] = 1.0 # xyzw quaternion w self.body_data.lin_vel = torch.zeros(num_envs, 3) self.body_data.ang_vel = torch.zeros(num_envs, 3) @@ -92,6 +95,17 @@ def set_mass(self, mass, env_ids=None): else: self._mass = mass + def get_inertia(self, env_ids=None): + if env_ids is not None: + return self._inertia[env_ids] + return self._inertia + + def set_inertia(self, inertia, env_ids=None): + if env_ids is not None: + self._inertia[env_ids] = inertia + else: + self._inertia = inertia + class MockRigidObjectGroup: """Mock rigid object group for event functor tests.""" @@ -221,12 +235,17 @@ def __init__( # Default pose at origin (position + quaternion) # Format: (N, 7) - position (3) + quaternion (4) self._pose = torch.zeros(num_envs, 7) - self._pose[:, 3] = 1.0 # quaternion w = 1 (identity rotation) + self._pose[:, 6] = 1.0 # xyzw quaternion w = 1 (identity rotation) - self.default_link_masses = torch.ones( - (self.num_envs, len(self.link_names)), device=self.device + self._inertia = torch.ones( + (self.num_envs, len(self.link_names), 3), device=self.device ) self.body_data = Mock() + self.body_data.default_mass = torch.ones( + (self.num_envs, len(self.link_names)), device=self.device + ) + self.body_data.default_inertia = self._inertia.clone() + self.default_link_masses = self.body_data.default_mass self.body_data.body_link_vel = torch.zeros( self.num_envs, len(self.link_names), 6, device=self.device ) @@ -306,6 +325,30 @@ def set_mass(self, mass, link_names, env_ids=None): for j, name in enumerate(link_names): self._entities[env_idx]._link_masses[name] = mass[i, j].item() + def get_inertia(self, link_names=None, env_ids=None): + """Get link inertia diagonals, matching Articulation API.""" + env_index = torch.as_tensor( + list(range(self.num_envs)) if env_ids is None else env_ids, + dtype=torch.long, + ) + names = self.link_names if link_names is None else list(link_names) + link_index = torch.as_tensor( + [self.link_names.index(name) for name in names], dtype=torch.long + ) + return self._inertia[env_index[:, None], link_index[None, :]] + + def set_inertia(self, inertia, link_names=None, env_ids=None): + """Set link inertia diagonals, matching Articulation API.""" + env_index = torch.as_tensor( + list(range(self.num_envs)) if env_ids is None else env_ids, + dtype=torch.long, + ) + names = self.link_names if link_names is None else list(link_names) + link_index = torch.as_tensor( + [self.link_names.index(name) for name in names], dtype=torch.long + ) + self._inertia[env_index[:, None], link_index[None, :]] = inertia + class MockSim: """Mock simulation for event functor tests.""" @@ -533,6 +576,81 @@ def test_relative_mass_randomization(self): assert torch.all(masses >= 0.5) assert torch.all(masses <= 1.5) + def test_relative_mass_randomization_does_not_accumulate(self): + """Test repeated relative randomization uses the initial mass.""" + env = MockEnv(num_envs=4) + env_ids = torch.tensor([0, 1, 2, 3]) + # The backend-resolved mass is the baseline, not stale config metadata. + env.test_object.cfg.attrs.mass_props.mass = 10.0 + + for _ in range(2): + randomize_rigid_object_mass( + env, + env_ids, + entity_cfg=MagicMock(uid="cube"), + mass_range=(0.5, 0.5), + relative=True, + ) + + masses = env.test_object.get_mass().reshape(-1) + assert torch.allclose(masses, torch.full((4,), 1.5)) + + def test_mass_randomization_recomputes_inertia_from_defaults(self): + """Test inertia scaling uses the initial mass-property snapshot.""" + env = MockEnv(num_envs=4) + env_ids = torch.tensor([0, 1, 2, 3]) + env.test_object._inertia.fill_(9.0) + + randomize_rigid_object_mass( + env, + env_ids, + entity_cfg=MagicMock(uid="cube"), + mass_range=(2.0, 2.0), + ) + + assert torch.allclose(env.test_object.get_inertia(), torch.full((4, 3), 2.0)) + + def test_mass_randomization_enforces_positive_mass(self): + """Test relative offsets cannot produce a non-positive mass.""" + env = MockEnv(num_envs=4) + env_ids = torch.tensor([0, 1, 2, 3]) + + randomize_rigid_object_mass( + env, + env_ids, + entity_cfg=MagicMock(uid="cube"), + mass_range=(-2.0, -2.0), + relative=True, + min_mass=0.25, + ) + + assert torch.allclose(env.test_object.get_mass(), torch.full((4, 1), 0.25)) + + def test_sampling_uses_rigid_object_device(self, monkeypatch): + """Test samples are allocated on the rigid object's device.""" + env = MockEnv(num_envs=4) + env_ids = torch.tensor([0, 1, 2, 3]) + sampled_device = None + + def fake_sample_uniform(*, lower, upper, size, device): + nonlocal sampled_device + sampled_device = device + return torch.zeros(size, device=device) + + monkeypatch.setattr( + "embodichain.lab.gym.envs.managers.randomization.physics.sample_uniform", + fake_sample_uniform, + ) + + randomize_rigid_object_mass( + env, + env_ids, + entity_cfg=MagicMock(uid="cube"), + mass_range=(0.5, 2.0), + ) + + assert sampled_device == env.test_object.device + def test_handles_nonexistent_object(self): """Test that function handles non-existent object gracefully.""" env = MockEnv(num_envs=4) @@ -781,7 +899,7 @@ def test_sets_specific_link_with_list(self): assert torch.all(randomized <= 2.0) def test_relative_mass_randomization(self): - """Test relative mass randomization adds to current mass.""" + """Test relative mass randomization adds to the initial mass.""" env = MockEnv(num_envs=4) env_ids = torch.tensor([0, 1, 2, 3]) @@ -802,6 +920,90 @@ def test_relative_mass_randomization(self): assert torch.all(masses >= 0.5) assert torch.all(masses <= 1.5) + def test_relative_mass_randomization_does_not_accumulate(self): + """Repeated relative randomization uses initialization-time link mass.""" + env = MockEnv(num_envs=4) + env_ids = torch.tensor([0, 1, 2, 3]) + + for _ in range(2): + randomize_articulation_mass( + env, + env_ids, + entity_cfg=MagicMock(uid="articulation"), + mass_range=(0.5, 0.5), + link_names=["base_link"], + relative=True, + ) + + masses = env.test_articulation.get_mass( + link_names=["base_link"], env_ids=env_ids + ) + assert torch.allclose(masses, torch.full((4, 1), 1.5)) + + def test_mass_randomization_recomputes_link_inertia_from_defaults(self): + """Inertia scaling uses initialization snapshots rather than current values.""" + env = MockEnv(num_envs=4) + env_ids = torch.tensor([0, 1, 2, 3]) + env.test_articulation._inertia.fill_(9.0) + + randomize_articulation_mass( + env, + env_ids, + entity_cfg=MagicMock(uid="articulation"), + mass_range=(2.0, 2.0), + link_names=["base_link"], + ) + + inertia = env.test_articulation.get_inertia( + link_names=["base_link"], env_ids=env_ids + ) + assert torch.allclose(inertia, torch.full((4, 1, 3), 2.0)) + + def test_mass_randomization_enforces_positive_link_mass(self): + """Relative offsets cannot produce a non-positive link mass.""" + env = MockEnv(num_envs=4) + env_ids = torch.tensor([0, 1, 2, 3]) + + randomize_articulation_mass( + env, + env_ids, + entity_cfg=MagicMock(uid="articulation"), + mass_range=(-2.0, -2.0), + link_names=["base_link"], + relative=True, + min_mass=0.25, + ) + + masses = env.test_articulation.get_mass( + link_names=["base_link"], env_ids=env_ids + ) + assert torch.allclose(masses, torch.full((4, 1), 0.25)) + + def test_sampling_uses_articulation_device(self, monkeypatch): + """Test tuple-range samples use the articulation's device.""" + env = MockEnv(num_envs=4) + env_ids = torch.tensor([0, 1, 2, 3]) + sampled_device = None + + def fake_sample_uniform(*, lower, upper, size, device): + nonlocal sampled_device + sampled_device = device + return torch.zeros(size, device=device) + + monkeypatch.setattr( + "embodichain.lab.gym.envs.managers.randomization.physics.sample_uniform", + fake_sample_uniform, + ) + + randomize_articulation_mass( + env, + env_ids, + entity_cfg=MagicMock(uid="articulation"), + mass_range=(0.5, 2.0), + ) + + assert sampled_device == env.test_articulation.device + def test_handles_nonexistent_articulation(self): """Test that function handles non-existent articulation gracefully.""" env = MockEnv(num_envs=4) diff --git a/tests/gym/envs/managers/test_observation_functors.py b/tests/gym/envs/managers/test_observation_functors.py index ced6e1f7e..aae6f4c5c 100644 --- a/tests/gym/envs/managers/test_observation_functors.py +++ b/tests/gym/envs/managers/test_observation_functors.py @@ -104,7 +104,7 @@ def get_local_pose(self, to_matrix=True): pos = self._pose[:, :3, 3] # Simple quaternion from identity rotation quat = torch.zeros(self.num_envs, 4) - quat[:, 0] = 1.0 # w=1 (identity) + quat[:, 3] = 1.0 # xyzw identity return torch.cat([pos, quat], dim=-1) def get_mass(self): diff --git a/tests/gym/envs/managers/test_randomize_anchor_height.py b/tests/gym/envs/managers/test_randomize_anchor_height.py index 1c6acf17e..a3f4c9972 100644 --- a/tests/gym/envs/managers/test_randomize_anchor_height.py +++ b/tests/gym/envs/managers/test_randomize_anchor_height.py @@ -41,7 +41,7 @@ def __init__(self, uid: str, num_envs: int = 4): self.cfg = MagicMock() self.cfg.init_pos = [0.0, 0.0, 0.0] self._pose = torch.zeros(num_envs, 7) - self._pose[:, 3] = 1.0 # identity quaternion + self._pose[:, 6] = 1.0 # xyzw identity quaternion self._cleared = False self._cleared_env_ids = None diff --git a/tests/gym/envs/task_program/test_environment.py b/tests/gym/envs/task_program/test_environment.py index 7856cd25e..cd3004f34 100644 --- a/tests/gym/envs/task_program/test_environment.py +++ b/tests/gym/envs/task_program/test_environment.py @@ -498,7 +498,7 @@ def _program_with_later_segment_hooks( values=( PoseCfg( position=(0.4, 0.1, 0.2), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), ), ) ) @@ -648,7 +648,7 @@ def test_preflight_preserves_pick_target_lookahead_across_explicit_segments( values=( PoseCfg( position=(0.4, 0.1, 0.2), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), ), ) ) diff --git a/tests/gym/envs/task_program/test_simulation_environment.py b/tests/gym/envs/task_program/test_simulation_environment.py index d9924fd4b..2ee5ab3ad 100644 --- a/tests/gym/envs/task_program/test_simulation_environment.py +++ b/tests/gym/envs/task_program/test_simulation_environment.py @@ -148,7 +148,7 @@ _RELEASE_SEPARATION = 0.2 _DIRECT_PLACE_TARGET = SemanticPose( position=(0.0, 0.0, 0.0), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), ) @@ -565,7 +565,7 @@ def resolve( del call, context, bound pose = SemanticPose( position=(0.0, 0.0, 0.5), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), ) return HandOverPoseTargets( final=SemanticObjectTarget(pose=pose), @@ -1135,8 +1135,8 @@ def _pick_place_program_data() -> dict[str, object]: "values": [ { "position": _DIRECT_PLACE_TARGET.position.tolist(), - "quaternion_wxyz": ( - _DIRECT_PLACE_TARGET.quaternion_wxyz.tolist() + "quaternion_xyzw": ( + _DIRECT_PLACE_TARGET.quaternion_xyzw.tolist() ), } ], @@ -1702,7 +1702,7 @@ def without_in_flight_guards( object=SceneObjectRef("cube"), at=SemanticPose( position=(0.0, 0.0, 0.0), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), ), ), ), diff --git a/tests/gym/envs/task_program/test_simulation_handover.py b/tests/gym/envs/task_program/test_simulation_handover.py index 401c7ee03..7fab359d5 100644 --- a/tests/gym/envs/task_program/test_simulation_handover.py +++ b/tests/gym/envs/task_program/test_simulation_handover.py @@ -28,7 +28,7 @@ def _provider() -> ConfiguredHandOverPoseProvider: """Return one deterministic dual-arm transfer declaration.""" return ConfiguredHandOverPoseProvider( final_position=(0.0, -0.2, 0.7), - final_quaternion_wxyz=(1.0, 1.0, 0.0, 0.0), + final_quaternion_xyzw=(1.0, 0.0, 0.0, 1.0), ) @@ -59,7 +59,7 @@ def test_configured_handover_provider_normalizes_and_owns_targets() -> None: ("overrides", "error_type"), [ ({"final_position": (0.0, 0.0)}, TypeError), - ({"final_quaternion_wxyz": (0.0, 0.0, 0.0, 0.0)}, ValueError), + ({"final_quaternion_xyzw": (0.0, 0.0, 0.0, 0.0)}, ValueError), ], ) def test_configured_handover_provider_rejects_invalid_declarations( @@ -69,7 +69,7 @@ def test_configured_handover_provider_rejects_invalid_declarations( """Malformed provider declarations fail before simulation construction.""" values: dict[str, object] = { "final_position": (0.0, -0.2, 0.7), - "final_quaternion_wxyz": (1.0, 0.0, 0.0, 0.0), + "final_quaternion_xyzw": (0.0, 0.0, 0.0, 1.0), } values.update(overrides) diff --git a/tests/gym/envs/task_program/test_simulation_policies.py b/tests/gym/envs/task_program/test_simulation_policies.py index 67998b66e..18acbc1a1 100644 --- a/tests/gym/envs/task_program/test_simulation_policies.py +++ b/tests/gym/envs/task_program/test_simulation_policies.py @@ -150,7 +150,7 @@ def _compiled_segment(*, settle_preset: str = "fast"): "values": [ { "position": [0.0, 0.0, 0.0], - "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], } ], } diff --git a/tests/gym/envs/task_program/test_task_hand_over.py b/tests/gym/envs/task_program/test_task_hand_over.py index 4a80330d3..9957bd3a3 100644 --- a/tests/gym/envs/task_program/test_task_hand_over.py +++ b/tests/gym/envs/task_program/test_task_hand_over.py @@ -46,7 +46,7 @@ EntityState, HandOverOptions, ) -from embodichain.lab.sim.cfg import RobotCfg +from embodichain.lab.sim.cfg import DefaultPhysicsCfg, RobotCfg from embodichain.lab.task_program.semantics import ( BinaryEffectClause, BinaryEffectEvidenceQuery, @@ -166,12 +166,31 @@ def test_hand_over_gym_config_selects_packaged_program_without_contact_sensor() assert embodiment["embodiment_id"] == _PROFILE_ID assert "object_ids" not in evidence assert "task_program" not in environment + assert environment["physics"] == "default" + assert environment["physics_config"] == {"enable_ccd": True} assert environment["env"]["extensions"] == {} settle = environment["env"]["events"]["settle_can_on_reset"] assert settle["func"] == "wait_for_dynamic_objects_to_settle" assert settle["params"]["entity_cfgs"] == [{"uid": _CAN_SIMULATION_UID}] +def test_hand_over_gym_config_uses_declared_default_backend() -> None: + """The reusable physical environment owns its backend selection.""" + cfg = config_to_cfg(_gym_payload(), source_path=_gym_config_path()) + + assert isinstance(cfg.sim_cfg.physics_config, DefaultPhysicsCfg) + assert cfg.sim_cfg.physics_config.enable_ccd is True + + +def test_hand_over_gym_config_rejects_newton_override() -> None: + """The Default CCD environment cannot be relabeled as Newton.""" + payload = _gym_payload() + payload["physics"] = "newton" + + with pytest.raises(ValueError, match="owns physics and physics_config"): + config_to_cfg(payload, source_path=_gym_config_path()) + + def test_hand_over_gym_config_builds_dual_ur5_pgi_scene() -> None: """Config parsing preserves the tutorial robot, can, and support geometry.""" cfg = _configured_env_cfg() @@ -223,7 +242,7 @@ def test_hand_over_gym_config_builds_dual_ur5_pgi_scene() -> None: ) assert [item.uid for item in cfg.background] == [_SUPPORT_SURFACE_UID] assert [item.uid for item in cfg.rigid_object] == [_CAN_SIMULATION_UID] - assert cfg.rigid_object[0].max_convex_hull_num == 16 + assert cfg.rigid_object[0].shape.collision.max_hulls == 16 assert cfg.task_program is not None assert cfg.task_program.program_id == "dual_ur5_hand_over" @@ -232,8 +251,8 @@ def test_hand_over_config_owns_tuned_can_and_pgi_physics() -> None: """The sole config source retains the tuned object and gripper dynamics.""" cfg = _configured_env_cfg() - assert cfg.rigid_object[0].attrs.mass == pytest.approx(0.33) - drive = cfg.robot.drive_pros + assert cfg.rigid_object[0].attrs.mass_props.mass == pytest.approx(0.33) + drive = cfg.robot.joint_drive_props expected_values = { "stiffness": 1e3, "damping": 1e2, @@ -247,8 +266,8 @@ def test_hand_over_config_owns_tuned_can_and_pgi_physics() -> None: ) assert values[f"{side}_gripper_finger2_joint_1"] == pytest.approx(0.0) finger_attrs = cfg.robot.link_attrs["gripper_fingers"].attrs - assert finger_attrs.dynamic_friction == pytest.approx(2.0) - assert finger_attrs.static_friction == pytest.approx(2.0) + assert finger_attrs.material_props.dynamic_friction == pytest.approx(2.0) + assert finger_attrs.material_props.static_friction == pytest.approx(2.0) def test_hand_over_composition_owns_scene_pose_and_evidence_services() -> None: diff --git a/tests/gym/envs/task_program/test_task_vertical_slices.py b/tests/gym/envs/task_program/test_task_vertical_slices.py index 88891736d..e70d8360a 100644 --- a/tests/gym/envs/task_program/test_task_vertical_slices.py +++ b/tests/gym/envs/task_program/test_task_vertical_slices.py @@ -505,11 +505,11 @@ def test_cube_variant_extends_by_data_without_motion_generation_code() -> None: ( { "position": [-0.25, -0.20, 0.10], - "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], }, { "position": [-0.25, 0.20, 0.10], - "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], }, ) ) @@ -979,6 +979,7 @@ def test_example_gym_configs_omit_auxiliary_environment_mechanisms( assert "task_program_runtime" not in payload assert environment["env"]["events"] == {} assert environment["env"]["dataset"] == {} + assert environment["physics"] == "default" assert "physics_config" not in environment diff --git a/tests/gym/envs/test_base_env.py b/tests/gym/envs/test_base_env.py index b156c9ec5..7ea53dc77 100644 --- a/tests/gym/envs/test_base_env.py +++ b/tests/gym/envs/test_base_env.py @@ -29,7 +29,7 @@ RobotCfg, JointDrivePropertiesCfg, RigidObjectCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, ) from embodichain.lab.gym.utils.registration import register_env from embodichain.lab.sim import SimulationManager, SimulationManagerCfg @@ -58,7 +58,7 @@ def __init__( env_cfg = EnvCfg( sim_cfg=SimulationManagerCfg( - headless=headless, arena_space=2.0, sim_device=device + headless=headless, arena_space=2.0, device=device ), num_envs=NUM_ENVS, ) @@ -68,19 +68,24 @@ def __init__( **kwargs, ) - def _setup_robot(self, **kwargs): + def _declare_robot(self, **kwargs) -> Robot: file_path = get_data_path("UniversalRobots/UR10/UR10.urdf") - robot: Robot = self.sim.add_robot( + return self.sim.add_robot( cfg=RobotCfg( uid="UR10", fpath=file_path, init_pos=(0, 0, 1), init_qpos=self.robot_init_qpos, - drive_pros=JointDrivePropertiesCfg(drive_type=self.drive_type), + joint_drive_props=JointDrivePropertiesCfg(drive_type=self.drive_type), ) ) + def _setup_robot(self, **kwargs) -> Robot: + robot = self.robot + if robot is None: + raise RuntimeError("UR10 was not declared before simulation prepare.") + qpos_limits = robot.body_data.qpos_limits[0].cpu().numpy() self.single_action_space = gym.spaces.Box( low=qpos_limits[:, 0], high=qpos_limits[:, 1], dtype=np.float32 @@ -96,7 +101,9 @@ def _prepare_scene(self, **kwargs): cfg=RigidObjectCfg( uid="cube", shape=CubeCfg(size=[size, size, size]), - attrs=RigidBodyAttributesCfg(enable_collision=False), + attrs=RigidBodyPhysicsCfg.from_dict( + {"collision_props": {"collision_enabled": False}} + ), init_pos=(0.0, 0.0, 0.5), body_type="kinematic", ), @@ -121,14 +128,14 @@ class BaseEnvTest: """Shared test logic for CPU and CUDA.""" @classmethod - def setup_simulation_hook(cls, sim_device): + def setup_simulation_hook(cls, device): if hasattr(cls, "env"): return cls.env = gym.make( "RandomReach-v1", num_envs=NUM_ENVS, headless=True, - device=sim_device, + device=device, ) cls.device = cls.env.get_wrapper_attr("device") cls.num_envs = cls.env.get_wrapper_attr("num_envs") @@ -221,12 +228,12 @@ def setup_class(cls): import sys -def new_setup_simulation(cls, sim_device): +def new_setup_simulation(cls, device): print(">>> ENTERING setup_simulation", file=sys.stderr) if hasattr(cls, "env"): return cls.env = gym.make( - "RandomReach-v1", num_envs=NUM_ENVS, headless=True, device=sim_device + "RandomReach-v1", num_envs=NUM_ENVS, headless=True, device=device ) cls.device = cls.env.get_wrapper_attr("device") cls.num_envs = cls.env.get_wrapper_attr("num_envs") diff --git a/tests/gym/envs/test_differentiable_embodied_env.py b/tests/gym/envs/test_differentiable_embodied_env.py new file mode 100644 index 000000000..1c932b625 --- /dev/null +++ b/tests/gym/envs/test_differentiable_embodied_env.py @@ -0,0 +1,540 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Tests for the Newton-only kinematic :class:`DifferentiableEnv`.""" + +from __future__ import annotations + +from types import SimpleNamespace +from typing import Any + +import numpy as np +import pytest +import torch +import warp as wp + +import embodichain.lab.gym.envs.differentiable_env as differentiable_env_module +from embodichain.lab.gym.envs.differentiable_env import DifferentiableEnv +from embodichain.lab.gym.envs.embodied_env import EmbodiedEnvCfg +from embodichain.lab.sim.cfg import DefaultPhysicsCfg, NewtonPhysicsCfg +from embodichain.lab.sim.diff import NewtonStepFunc, tape_context +from embodichain.lab.sim.diff.runtime import NewtonDifferentiableRuntime +from embodichain.lab.sim.sim_manager import SimulationManagerCfg + + +@wp.kernel +def _scale_action_kernel( + action: wp.array(dtype=wp.float32), + state: wp.array(dtype=wp.float32), +) -> None: + """Map one action to a task-owned kinematic state.""" + state[0] = 2.0 * action[0] + + +@wp.kernel +def _square_reward_kernel( + state: wp.array(dtype=wp.float32), + reward: wp.array(dtype=wp.float32), +) -> None: + """Compute a differentiable scalar reward from kinematic state.""" + reward[0] = state[0] * state[0] + + +def _bridge_state(*, is_newton_backend: bool = True) -> dict[str, Any]: + """Build a one-dimensional kinematics bridge input on CPU.""" + state_wp = wp.zeros(1, dtype=wp.float32, device="cpu", requires_grad=True) + reward_wp = wp.zeros(1, dtype=wp.float32, device="cpu", requires_grad=True) + manager = SimpleNamespace(is_newton_backend=is_newton_backend) + + def _apply_action(action_wp: Any, tape: Any) -> None: + del tape + wp.launch( + _scale_action_kernel, + dim=1, + inputs=[action_wp, state_wp], + device="cpu", + ) + + def _read_outputs(final_state: Any) -> dict[str, Any]: + assert final_state is state_wp + wp.launch( + _square_reward_kernel, + dim=1, + inputs=[state_wp, reward_wp], + device="cpu", + ) + return { + "obs": wp.to_torch(state_wp), + "reward": wp.to_torch(reward_wp), + "terminated": torch.zeros(1, dtype=torch.bool), + "truncated": torch.zeros(1, dtype=torch.bool), + "_order": ("obs", "reward", "terminated", "truncated"), + "_grad_track": { + "obs": None, + "reward": reward_wp, + "terminated": None, + "truncated": None, + }, + } + + return { + "manager": manager, + "action_kernel": _apply_action, + "kernel_args": (), + "step_fn": lambda: state_wp, + "obs_reward_fn": _read_outputs, + "last_info": {}, + } + + +def _bare_env() -> DifferentiableEnv: + """Build an uninitialized environment with only its hook dependencies.""" + env = object.__new__(DifferentiableEnv) + env.sim = SimpleNamespace(is_newton_backend=True) + env._apply_action_kernel = lambda _action, tape: None + env._make_kinematic_step_fn = lambda: (lambda: object()) + env._read_outputs = lambda _state: { + "obs": torch.zeros(1, 1), + "reward": torch.zeros(1), + "terminated": torch.zeros(1, dtype=torch.bool), + "truncated": torch.zeros(1, dtype=torch.bool), + "_order": ("obs", "reward", "terminated", "truncated"), + "_grad_track": {}, + } + return env + + +def _diff_env_cfg( + requires_grad: bool = True, + backend: str = "newton", +) -> EmbodiedEnvCfg: + """Build the minimum config required for constructor validation.""" + physics_cfg = ( + NewtonPhysicsCfg( + requires_grad=requires_grad, + solver_cfg={"solver_type": "semi_implicit"}, + use_cuda_graph=False, + ) + if backend == "newton" + else DefaultPhysicsCfg() + ) + return EmbodiedEnvCfg( + sim_cfg=SimulationManagerCfg( + physics_cfg=physics_cfg, + num_envs=2, + headless=True, + ) + ) + + +def test_public_class_is_renamed_without_legacy_alias() -> None: + """The public module exports only the concise environment name.""" + assert differentiable_env_module.__all__ == ["DifferentiableEnv"] + assert not hasattr(differentiable_env_module, "DifferentiableEmbodiedEnv") + + +def test_environment_builds_only_a_kinematic_bridge_state() -> None: + """The base environment exposes no solver mode, substeps, or control hook.""" + env = _bare_env() + expected_state = object() + env._make_kinematic_step_fn = lambda: (lambda: expected_state) + + sim_state = env._build_sim_state_dict(torch.zeros(1)) + + assert sim_state["step_fn"]() is expected_state + assert "step_mode" not in sim_state + assert "substeps" not in sim_state + assert "action_to_control_kernel" not in sim_state + assert "_apply_dynamics_action_kernel" not in DifferentiableEnv.__dict__ + assert "differentiable_step_mode" not in DifferentiableEnv.__dict__ + + +def test_environment_action_hook_receives_only_action_and_tape() -> None: + """The bridge adapter never supplies a Newton control buffer.""" + env = _bare_env() + action_wp = object() + tape = object() + calls: list[tuple[object, object]] = [] + + def _apply_action(action: object, tape: object) -> None: + calls.append((action, tape)) + + env._apply_action_kernel = _apply_action + + sim_state = env._build_sim_state_dict(torch.zeros(1)) + sim_state["action_kernel"](action_wp, tape) + + assert calls == [(action_wp, tape)] + + +def test_kinematic_bridge_propagates_reward_gradient_to_action() -> None: + """Warp reverse mode is bridged back to the original torch action.""" + action = torch.tensor([0.5], dtype=torch.float32, requires_grad=True) + + _, reward, _, _ = NewtonStepFunc.apply(action, _bridge_state()) + reward.sum().backward() + + assert action.grad is not None + assert torch.allclose(action.grad, torch.tensor([4.0])) + + +def test_kinematic_bridge_no_grad_call_releases_tape_synchronously() -> None: + """Inference output does not retain a custom autograd node.""" + action = torch.tensor([0.5], dtype=torch.float32, requires_grad=True) + + with torch.no_grad(): + _, reward, _, _ = NewtonStepFunc.apply(action, _bridge_state()) + + assert not reward.requires_grad + + +def test_kinematic_bridge_rejects_default_backend_before_opening_tape() -> None: + """Direct bridge callers receive the same Newton-only contract.""" + action = torch.tensor([0.5], dtype=torch.float32, requires_grad=True) + + with pytest.raises(RuntimeError, match="Newton backend"): + NewtonStepFunc.apply( + action, + _bridge_state(is_newton_backend=False), + ) + + +def test_kinematic_bridge_requires_a_named_step_callback() -> None: + """An arbitrary dynamics fallback cannot replace the kinematics hook.""" + sim_state = _bridge_state() + sim_state["step_fn"] = None + + with pytest.raises(TypeError, match="callable step_fn"): + NewtonStepFunc.apply(torch.zeros(1), sim_state) + + +def test_tape_context_rejects_default_backend() -> None: + """Expert tape composition remains Newton-only.""" + manager = SimpleNamespace(is_newton_backend=False) + + with pytest.raises(RuntimeError, match="Newton backend"): + with tape_context(manager): + pass + + +def test_kinematic_runtime_does_not_validate_or_expose_a_solver() -> None: + """FK access needs a grad model and live state, not dynamics resources.""" + model = object() + current_state = object() + backend = SimpleNamespace( + model=model, + cfg=SimpleNamespace( + requires_grad=True, + solver_cfg=SimpleNamespace(solver_type="mujoco_warp"), + ), + _runtime=SimpleNamespace(current_state=current_state), + state_0=object(), + state_1=object(), + ) + runtime = NewtonDifferentiableRuntime(lambda: backend) + + assert runtime.model is model + assert runtime.current_state is current_state + assert runtime.live_states == (backend.state_0, backend.state_1) + assert not hasattr(runtime, "control") + assert not hasattr(runtime, "create_differentiable_trajectory") + + +def test_construct_without_requires_grad_raises() -> None: + """Newton kinematic models must opt into Warp gradients.""" + with pytest.raises(RuntimeError, match="requires_grad"): + DifferentiableEnv(_diff_env_cfg(requires_grad=False)) + + +def test_construct_on_default_backend_raises() -> None: + """The Default backend is rejected before environment initialization.""" + with pytest.raises( + RuntimeError, + match="DifferentiableEnv requires NewtonPhysicsCfg", + ): + DifferentiableEnv(_diff_env_cfg(backend="default")) + + +@pytest.mark.parametrize("grad_enabled", (True, False)) +def test_terminal_reset_respects_tape_lifetime( + monkeypatch: pytest.MonkeyPatch, + grad_enabled: bool, +) -> None: + """Tracked terminal steps defer reset; inference resets synchronously.""" + env = _bare_env() + reset_calls: list[torch.Tensor] = [] + sim_state = {"last_info": {}} + env._build_sim_state_dict = lambda _action: sim_state + + outputs = ( + torch.full((1, 1), 7.0), + torch.full((1,), 3.0), + torch.ones(1, dtype=torch.bool), + torch.zeros(1, dtype=torch.bool), + ) + monkeypatch.setattr( + NewtonStepFunc, + "apply", + staticmethod(lambda _action, _state: outputs), + ) + + def _reset(*, options: dict[str, Any]): + reset_ids = torch.as_tensor(options["reset_ids"]).clone() + reset_calls.append(reset_ids) + return torch.full((1, 1), -1.0), {} + + env.reset = _reset + action = torch.zeros(1, requires_grad=True) + context = torch.enable_grad() if grad_enabled else torch.no_grad() + with context: + obs, _, _, _, info = env.step(action) + + if grad_enabled: + assert torch.equal(obs, torch.full((1, 1), 7.0)) + assert reset_calls == [] + assert info["requires_reset_after_backward"] is True + assert torch.equal(info["deferred_reset_ids"], torch.tensor([0])) + else: + assert torch.equal(obs, torch.full((1, 1), -1.0)) + assert len(reset_calls) == 1 + assert "requires_reset_after_backward" not in info + + +def _import_franka_env(): + """Import the Franka APG environment after resolving task packages.""" + from embodichain_tasks.special.franka_reach_apg import FrankaReachApgEnv + + return FrankaReachApgEnv + + +def test_franka_kinematics_build_snapshots_live_primal_before_bridge( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Franka detaches taped FK inputs before the parent opens a tape.""" + from embodichain_tasks.special import franka_reach_apg + + env = object.__new__(franka_reach_apg.FrankaReachApgEnv) + live_joint_q = object() + snapshot_joint_q = object() + fresh_fk_state = object() + events: list[str] = [] + env.sim = SimpleNamespace( + differentiable_runtime=SimpleNamespace( + current_state=SimpleNamespace(joint_q=live_joint_q), + model=SimpleNamespace( + state=lambda: (events.append("state"), fresh_fk_state)[1] + ), + ) + ) + + def _clone(array: object) -> object: + assert array is live_joint_q + events.append("clone") + return snapshot_joint_q + + def _parent_build(_self: object, _action: torch.Tensor) -> dict[str, Any]: + events.append("parent") + assert env._current_joint_q_snapshot is snapshot_joint_q + assert env._fk_state is fresh_fk_state + return {"prepared": True} + + monkeypatch.setattr(franka_reach_apg.wp, "clone", _clone) + monkeypatch.setattr(DifferentiableEnv, "_build_sim_state_dict", _parent_build) + + result = env._build_sim_state_dict(torch.zeros(1, 7)) + + assert result == {"prepared": True} + assert events == ["clone", "state", "parent"] + + +def test_franka_action_kernel_reads_snapshot_instead_of_live_state( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """The recorded action kernel never captures mutable manager state.""" + from embodichain_tasks.special import franka_reach_apg + + env = object.__new__(franka_reach_apg.FrankaReachApgEnv) + snapshot_joint_q = object() + target_joint_q = object() + action_wp = object() + launch_inputs: list[object] = [] + env.sim = SimpleNamespace(num_envs=1) + env._current_joint_q_snapshot = snapshot_joint_q + env._n_joints_per_env = 9 + env._wp_device = "cpu" + env._limit_lo_wp = object() + env._limit_hi_wp = object() + env._action_scale = 0.2 + + monkeypatch.setattr( + franka_reach_apg.wp, + "zeros", + lambda *_args, **_kwargs: target_joint_q, + ) + + def _launch(*_args: Any, inputs: list[object], **_kwargs: Any) -> None: + launch_inputs.extend(inputs) + + monkeypatch.setattr(franka_reach_apg.wp, "launch", _launch) + + env._apply_action_kernel(action_wp, tape=object()) + + assert launch_inputs[0] is action_wp + assert launch_inputs[1] is snapshot_joint_q + assert launch_inputs[2] is target_joint_q + + +def test_franka_snapshot_keeps_gradient_after_live_state_mutation_and_matches_fd( + monkeypatch: pytest.MonkeyPatch, + tmp_path, +) -> None: + """Detached FK input survives live writes before backward under strict mode.""" + from embodichain_tasks.special import franka_reach_apg + + env = object.__new__(franka_reach_apg.FrankaReachApgEnv) + device = "cpu" + live_joint_q = wp.zeros(7, dtype=wp.float32, device=device) + env.sim = SimpleNamespace( + num_envs=1, + differentiable_runtime=SimpleNamespace( + current_state=SimpleNamespace(joint_q=live_joint_q), + model=SimpleNamespace(state=lambda: object()), + ), + ) + env._wp_device = device + env._n_joints_per_env = 7 + env._limit_lo_wp = wp.array( + np.full(7, -10.0, dtype=np.float32), + dtype=wp.float32, + device=device, + ) + env._limit_hi_wp = wp.array( + np.full(7, 10.0, dtype=np.float32), + dtype=wp.float32, + device=device, + ) + env._action_scale = 0.2 + monkeypatch.setattr( + DifferentiableEnv, + "_build_sim_state_dict", + lambda _self, _action: {}, + ) + env._build_sim_state_dict(torch.zeros(1, 7)) + + previous_verify_access = wp.config.verify_autograd_array_access + previous_kernel_cache_dir = wp.config.kernel_cache_dir + wp.config.verify_autograd_array_access = True + wp.config.kernel_cache_dir = str(tmp_path / "warp_cache") + tape = wp.Tape() + try: + action_wp = wp.array( + np.zeros(7, dtype=np.float32), + dtype=wp.float32, + device=device, + requires_grad=True, + ) + with tape: + env._apply_action_kernel(action_wp, tape=tape) + analytic_output = env._new_joint_q + + wp.copy( + live_joint_q, + wp.array( + np.full(7, 5.0, dtype=np.float32), + dtype=wp.float32, + device=device, + ), + ) + tape.backward(grads={analytic_output: wp.ones_like(analytic_output)}) + analytic_gradient = action_wp.grad.numpy().copy() + + assert np.isfinite(analytic_gradient).all() + assert np.all(np.abs(analytic_gradient) > 0.0) + + def _loss(action_value: float) -> float: + values = np.zeros(7, dtype=np.float32) + values[0] = action_value + finite_difference_action = wp.array( + values, + dtype=wp.float32, + device=device, + ) + env._apply_action_kernel(finite_difference_action, tape=object()) + return float(env._new_joint_q.numpy().sum()) + + epsilon = 1.0e-3 + finite_difference_gradient = (_loss(epsilon) - _loss(-epsilon)) / ( + 2.0 * epsilon + ) + assert np.isclose( + analytic_gradient[0], + finite_difference_gradient, + rtol=1.0e-4, + atol=1.0e-5, + ) + finally: + tape.reset() + wp.config.verify_autograd_array_access = previous_verify_access + wp.config.kernel_cache_dir = previous_kernel_cache_dir + + +@pytest.mark.requires_sim +@pytest.mark.gpu +def test_franka_apg_smoke_backward() -> None: + """Reward remains tracked and produces a finite action gradient.""" + try: + FrankaReachApgEnv = _import_franka_env() + except FileNotFoundError as exc: + pytest.skip(f"Franka URDF not available: {exc}") + + env = FrankaReachApgEnv(num_envs=2) + try: + env.reset(seed=0) + action = torch.zeros(2, 7, requires_grad=True, device=env.device) + _, reward, _, _, _ = env.step(action) + assert reward.requires_grad + reward.sum().backward() + assert action.grad is not None + assert torch.isfinite(action.grad).all() + finally: + env.close() + + +@pytest.mark.requires_sim +@pytest.mark.gpu +def test_franka_apg_one_iter_loss_reduces() -> None: + """A short action optimization reduces the kinematic reach loss.""" + try: + FrankaReachApgEnv = _import_franka_env() + except FileNotFoundError as exc: + pytest.skip(f"Franka URDF not available: {exc}") + + env = FrankaReachApgEnv(num_envs=2) + try: + env.reset(seed=0) + action = torch.zeros(2, 7, requires_grad=True, device=env.device) + optimizer = torch.optim.SGD([action], lr=0.01) + losses: list[float] = [] + for _ in range(3): + env.reset(seed=0) + optimizer.zero_grad() + _, reward, _, _, _ = env.step(action) + loss = (-reward).sum() + loss.backward() + optimizer.step() + losses.append(loss.detach().item()) + assert losses[-1] < losses[0], f"APG did not reduce loss: {losses}" + finally: + env.close() diff --git a/tests/gym/envs/test_embodied_env.py b/tests/gym/envs/test_embodied_env.py index 2ecacc0a9..b5b8f0b4d 100644 --- a/tests/gym/envs/test_embodied_env.py +++ b/tests/gym/envs/test_embodied_env.py @@ -44,6 +44,7 @@ urdf_path = get_data_path("UniversalRobots/UR5/UR5.urdf") METADATA = { "id": "EmbodiedEnv-v1", + "physics": "default", "max_episodes": 1, "env": { "events": { @@ -75,7 +76,7 @@ ], "robot": { "fpath": urdf_path, - "drive_pros": {"stiffness": {"joint[1-6]": 200.0}}, + "joint_drive_props": {"stiffness": {"joint[1-6]": 200.0}}, "solver_cfg": { "class_type": "PytorchSolver", "end_link_name": "ee_link", @@ -101,9 +102,12 @@ "shape": { "shape_type": "Mesh", "fpath": "ShopTableSimple/shop_table_simple.ply", + "collision": { + "approximation": "convex_decomposition", + "max_hulls": 2, + }, }, - "max_convex_hull_num": 2, - "attrs": {"mass": 10.0}, + "attrs": {"mass_props": {"mass": 10.0}}, "body_scale": (2, 1.6, 1), } ], @@ -145,14 +149,14 @@ def test_visual_randomization_filter_keeps_deterministic_material_events(): class EmbodiedEnvTest: """Shared test logic for CPU and CUDA.""" - def setup_simulation(self, sim_device): + def setup_simulation(self, device): cfg: EmbodiedEnvCfg = config_to_cfg( METADATA, manager_modules=DEFAULT_MANAGER_MODULES ) cfg.num_envs = NUM_ENVS cfg.sim_cfg = SimulationManagerCfg( headless=True, - sim_device=sim_device, + device=device, ) self.env = gym.make(id=METADATA["id"], cfg=cfg) diff --git a/tests/gym/envs/test_initialization_summary.py b/tests/gym/envs/test_initialization_summary.py index 341c49b75..9acf6ed51 100644 --- a/tests/gym/envs/test_initialization_summary.py +++ b/tests/gym/envs/test_initialization_summary.py @@ -19,12 +19,14 @@ from __future__ import annotations from types import SimpleNamespace +from functools import partial import pytest import torch import embodichain.lab.gym.envs.base_env as base_env_module -from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.gym.envs import BaseEnv, EmbodiedEnv +from embodichain.lab.sim import SimulationManagerCfg pytestmark = pytest.mark.no_sim @@ -52,6 +54,25 @@ class _ManagerStub: def __init__(self, active_functors: dict[str, list[str]]) -> None: self.active_functors = active_functors + self.configs = { + name: SimpleNamespace(func=_summary_functor, params={}, mode=mode) + for mode, names in active_functors.items() + for name in names + } + self.save_failed_episodes = False + + def get_functor_cfg(self, name: str) -> object: + return self.configs[name] + + +def _summary_functor(*args, **kwargs): + raise AssertionError("Formatting must never execute a functor") + + +class _ActionTermStub: + input_key = "action" + action_dim = 7 + cfg = SimpleNamespace(params={}) class _ActionManagerStub: @@ -61,18 +82,54 @@ class _ActionManagerStub: def get_terms_by_mode(self, mode: str) -> list[tuple[str, object]]: terms = { - "pre": [("delta_qpos", object())], - "post": [("smooth_action", object())], + "pre": [("delta_qpos", _ActionTermStub())], + "post": [("smooth_action", _ActionTermStub())], } return terms[mode] + def get_term(self, name: str) -> _ActionTermStub: + return _ActionTermStub() + + +@pytest.fixture(autouse=True) +def _no_gpu_summary(monkeypatch: pytest.MonkeyPatch) -> None: + """Summary tests must never probe CUDA hardware or emit terminal escapes.""" + monkeypatch.setattr(torch.cuda, "is_available", lambda: False) + monkeypatch.setenv("NO_COLOR", "1") + monkeypatch.setenv("COLUMNS", "120") + -def _make_summary_env() -> _SummaryEnv: +def _make_summary_env(mode: str = "full") -> _SummaryEnv: """Create a fully populated environment shell without starting simulation.""" env = object.__new__(_SummaryEnv) env.cfg = _SummaryCfg() - env.sim_cfg = SimpleNamespace(physics_dt=0.005, headless=True) - env.sim = SimpleNamespace(device=torch.device("cuda:0")) + env.sim_cfg = SimulationManagerCfg( + physics_dt=0.005, + headless=True, + device="cpu", + num_envs=8, + startup_summary=mode, + ) + env.sim = SimpleNamespace( + device=torch.device("cpu"), + sim_config=env.sim_cfg, + num_envs=8, + physics=SimpleNamespace(name="default", solver_type="TGS"), + _requested_renderer="hybrid", + _requested_solver="TGS", + is_window_opened=False, + spawn_result=SimpleNamespace(topology_revision=1, needs_rebuild=False), + _ready_spawn_topology_revision=1, + _robots={"test_arm": object()}, + _articulations={}, + _rigid_objects={"cube": object()}, + _rigid_object_groups={}, + _deformable_objects={}, + _sensors={}, + _default_plane=object(), + _startup_summary_logged=False, + _scene_summary_logged=False, + ) env._num_envs = 8 env.robot = _RobotStub() env.sensors = {"front_camera": object(), "wrist_camera": object()} @@ -99,40 +156,99 @@ def _make_summary_env() -> _SummaryEnv: return env -def test_summary_includes_runtime_metadata_and_every_manager_functor() -> None: - """The summary exposes key runtime facts and every configured functor.""" - lines = _make_summary_env()._initialization_summary_lines() - rendered = "\n".join(lines) - normalized_lines = {" ".join(line.split()) for line in lines} - - assert rendered.startswith("╭─ Environment initialized: _SummaryEnv") - assert "├─ Runtime" in rendered - assert "Config _SummaryCfg" in rendered - assert "Device cuda:0" in rendered - assert "Parallel environments 8" in rendered - assert "Robot _RobotStub (uid=test_arm)" in rendered - assert "Sensors 2 (front_camera, wrist_camera)" in rendered - assert "├─ Timing" in rendered - assert "Physics 0.005 s (200 Hz)" in rendered - assert "Control 0.02 s (50 Hz, 4 physics steps)" in rendered - assert "├─ Metadata" in rendered - assert "dataset 2 keys (instruction, robot_meta)" in rendered +def _column_text(rendered: str, column: int) -> str: + """Reassemble a wrapped details-table column without neighboring cells.""" + return "".join( + cells[column].strip() + for line in rendered.splitlines() + if len(cells := line.split("│")) == 6 + ) + + +def test_summary_includes_shared_runtime_scene_and_task_metadata() -> None: + """Gym must include the shared rendering/physics/scene facts in one table.""" + rendered = "\n".join(_make_summary_env()._initialization_summary_lines()) + + assert rendered.count("Environment initialized: _SummaryEnv") == 1 + for label in ( + "Section", + "Setting", + "Value", + "Renderer", + "Graphics API", + "Backend", + "Solver", + "Gravity", + "Robots", + "Rigid objects", + ): + assert label in rendered + for value in ( + "Default", + "TGS", + "_SummaryCfg", + "cpu", + "_RobotStub", + "test_arm", + "0.02 s", + "50 Hz", + "300 control steps", + "dataset", + "instruction", + "robot_meta", + "READY", + ): + assert value in rendered assert "render_fps" not in rendered assert "Pick up the red cube" not in rendered - assert "├─ Managers (4/5 active, 8 functors)" in rendered - assert "EventManager 3 functors" in rendered - assert "│ startup load_scene" in normalized_lines - assert "│ reset reset_robot, randomize_objects" in normalized_lines - assert "ObservationManager 2 functors" in rendered - assert "│ modify normalize_rgb" in normalized_lines - assert "│ add task_state" in normalized_lines - assert "RewardManager disabled" in rendered - assert "ActionManager 2 functors" in rendered - assert "│ pre delta_qpos" in normalized_lines - assert "│ post smooth_action" in normalized_lines - assert "DatasetManager 1 functor" in rendered - assert "│ save record_episode" in normalized_lines - assert rendered.endswith("╰─ Ready") + assert "PhysX" not in rendered + assert "Engine threads" not in rendered + assert "Stepping" not in rendered + + +def test_full_summary_preserves_every_manager_count_and_functor() -> None: + """Full output must retain names, modes, disabled managers, and totals.""" + rendered = "\n".join(_make_summary_env()._initialization_summary_lines()) + normalized = " ".join(rendered.split()) + + assert "4/5 active, 8 functors" in normalized + for manager, count in ( + ("EventManager", "3 functors"), + ("ObservationManager", "2 functors"), + ("RewardManager", "disabled"), + ("ActionManager", "2 functors"), + ("DatasetManager", "1 functor"), + ): + assert any(manager in line and count in line for line in rendered.splitlines()) + for mode, name in ( + ("startup", "load_scene"), + ("reset", "reset_robot"), + ("reset", "randomize_objects"), + ("modify", "normalize_rgb"), + ("add", "task_state"), + ("pre", "delta_qpos"), + ("post", "smooth_action"), + ("save", "record_episode"), + ): + assert any(mode in line and name in line for line in rendered.splitlines()) + + +def test_compact_summary_separates_counts_from_functor_details() -> None: + """Default output lists every functor in its own table after the main table.""" + rendered = "\n".join(_make_summary_env("compact")._initialization_summary_lines()) + + assert "4/5 active, 8 functors" in rendered + assert "EventManager" in rendered + assert "3 functors" in rendered + main, details = rendered.split("EmbodiChain · Functor Details", 1) + assert "load_scene" not in main + assert "randomize_objects" not in main + assert "load_scene" in details + assert "randomize_objects" in details + assert "_summary_functor" in _column_text(details, 2) + assert "input=action" in details + assert "dim=7" in details + assert "save failed episodes=OFF" in details def test_summary_omits_metadata_section_for_render_fps_only() -> None: @@ -142,23 +258,143 @@ def test_summary_omits_metadata_section_for_render_fps_only() -> None: rendered = "\n".join(env._initialization_summary_lines()) - assert "├─ Metadata" not in rendered + assert "Metadata" not in rendered -def test_summary_logs_tree_as_one_record_without_prefix( +@pytest.mark.parametrize( + "mode, expected_records", [("compact", 1), ("full", 1), ("off", 0)] +) +def test_summary_logs_once_and_consumes_simulation_summaries( monkeypatch: pytest.MonkeyPatch, + mode: str, + expected_records: int, ) -> None: - """The complete tree is emitted once without standard log columns.""" - env = _make_summary_env() + """The ready Gym table must suppress duplicate core, scene, and Gym tables.""" + env = _make_summary_env(mode) calls: list[tuple[object, dict[str, object]]] = [] def capture(message: object, **kwargs: object) -> None: calls.append((message, kwargs)) monkeypatch.setattr(base_env_module.logger, "log_info", capture) - env._log_initialization_summary() + env._log_initialization_summary() + + assert len(calls) == expected_records + if expected_records: + assert calls[0][1] == {"prefix": False} + assert "Environment initialized: _SummaryEnv" in str(calls[0][0]) + assert str(calls[0][0]).count("EmbodiChain · Functor Details") == 1 + assert env.sim._startup_summary_logged + assert env.sim._scene_summary_logged - assert calls == [ - ("\n".join(env._initialization_summary_lines()), {"prefix": False}) - ] + +def test_scene_setup_defers_simulation_summary_until_gym_is_ready( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Scene declaration must not print the standalone manager table early.""" + env = _make_summary_env() + emitted: list[str] = [] + + def create_sim(cfg: SimulationManagerCfg, *, defer_startup_summary: bool = False): + if not defer_startup_summary: + emitted.append("standalone startup") + return SimpleNamespace() + + monkeypatch.setattr(base_env_module, "SimulationManager", create_sim) + monkeypatch.setattr(env, "_declare_robot", lambda **kwargs: None) + monkeypatch.setattr(env, "_prepare_scene", lambda **kwargs: None) + BaseEnv._setup_scene(env) + + assert emitted == [] + + +@pytest.mark.parametrize("mode", ["compact", "full"]) +def test_functor_details_show_type_specific_settings(mode): + env = _make_summary_env(mode) + env.event_manager = _ManagerStub({"interval": ["record"]}) + env.event_manager.configs["record"].interval_step = 10 + env.observation_manager.configs["task_state"].name = "robot/eef_pose" + env.reward_manager = _ManagerStub({"add": ["reach"]}) + env.reward_manager.configs["reach"].weight = -0.25 + env.dataset_manager.save_failed_episodes = True + rendered = " ".join("\n".join(env._initialization_summary_lines()).split()) + for expected in ( + "every 10 control steps", + "output=robot/eef_pose", + "weight=-0.25", + "save failed episodes=ON", + ): + assert expected in rendered + + +def test_full_details_expand_parameters_without_evaluating_values(): + from embodichain.lab.gym.envs.managers.cfg import SceneEntityCfg + + class Opaque: + def __repr__(self): + raise AssertionError("Do not evaluate arbitrary repr") + + env = _make_summary_env("full") + cfg = env.event_manager.configs["load_scene"] + cfg.func = partial(_summary_functor, scale=2.0) + cfg.params = { + "target": SceneEntityCfg(uid="cube", body_ids=[1, 3]), + "large": list(range(1000)), + "tensor": torch.zeros(100, 3), + "opaque": Opaque(), + } + rendered = "\n".join(env._initialization_summary_lines()) + flattened = _column_text(rendered, 4).replace(" ", "") + assert "test_initialization_summary._summary_functor" in _column_text(rendered, 2) + assert 'uid="cube"' in flattened or "uid='cube'" in flattened + assert "body_ids=[1,3]" in flattened + assert "list(len=1000)" in flattened + assert "shape=(100,3)" in flattened + assert "Opaque" in flattened + assert "scale=2.0" in flattened + assert "0,1,2,3,4,5,6,7,8,9" not in flattened + env.sim_cfg.startup_summary = "compact" + compact = "\n".join(env._initialization_summary_lines()) + assert "Params" not in compact + assert "Opaque" not in compact + + +def test_off_omits_both_tables_and_empty_managers_have_no_details(): + assert _make_summary_env("off")._initialization_summary_lines() == [] + env = _make_summary_env() + for _, attribute in env._manager_summary_fields: + setattr(env, attribute, None) + rendered = "\n".join(env._initialization_summary_lines()) + assert "0/5 active, 0 functors" in rendered + assert "Functor Details" not in rendered + + +@pytest.mark.parametrize("width", [64, 80, 120]) +def test_details_table_width_and_color_do_not_change_content(width): + import re + from wcwidth import wcswidth + from embodichain.lab.gym.envs._startup_summary import format_functor_summary + + env = _make_summary_env() + manager = env.event_manager + groups = [("EventManager", manager, [("reset", ["reset_robot"])])] + plain = format_functor_summary(groups, full=True, color=False, width=width) + styled = format_functor_summary(groups, full=True, color=True, width=width) + assert re.sub(r"\x1b\[[0-9;]*m", "", styled) == plain + assert max(wcswidth(line) for line in plain.splitlines()) <= width + assert "\x1b[1;36m" in styled + assert "\x1b[1;32m" in styled + assert "\x1b[1;33m" in styled + + +def test_no_color_overrides_terminal_detection(monkeypatch): + import sys + from embodichain.lab.gym.envs._startup_summary import format_functor_summary + + env = _make_summary_env() + monkeypatch.setattr(sys.stderr, "isatty", lambda: True) + rendered = format_functor_summary( + [("EventManager", env.event_manager, [("reset", ["reset_robot"])])], full=False + ) + assert "\x1b[" not in rendered diff --git a/tests/gym/envs/test_official_task_layout.py b/tests/gym/envs/test_official_task_layout.py index afd5135d0..525d8146b 100644 --- a/tests/gym/envs/test_official_task_layout.py +++ b/tests/gym/envs/test_official_task_layout.py @@ -26,6 +26,7 @@ REGISTERED_ENVS, discover_task_packages, ) +from embodichain.lab.sim.cfg import physics_cfg_for_backend from embodichain.learning.rl.env import get_registered_learning_env_names from embodichain.utils.utility import load_config from embodichain_tasks.classic_control.point_mass import PointMassEnv @@ -61,6 +62,33 @@ ) +def test_official_gym_configs_own_one_explicit_physics_backend() -> None: + """Every runnable config resolves one backend-aligned physics mapping.""" + for config_path in sorted(TASK_CONFIG_ROOT.rglob("*")): + if config_path.suffix.lower() not in {".json", ".yaml", ".yml"}: + continue + config = load_config(config_path) + if type(config) is not dict or not config.get("id"): + continue + + environment = config.get("environment") + if type(environment) is dict and "component" in environment: + assert "physics" not in config, config_path + assert "physics_config" not in config, config_path + owner_path = config_path.parent / environment["component"] + owner = load_config(owner_path) + else: + owner_path = config_path + owner = config + + backend = owner.get("physics") + assert backend in {"default", "newton"}, owner_path + physics_config = owner.get("physics_config", {}) + assert type(physics_config) is dict, owner_path + config_type = type(physics_cfg_for_backend(backend)) + config_type(**physics_config) + + def test_import_registered_gym_ids_resolve_to_flat_task_modules() -> None: """Import-registered Gym IDs resolve to task-named Python modules.""" discover_task_packages() @@ -129,6 +157,7 @@ def test_config_defined_task_programs_do_not_need_python_task_modules() -> None: environment = load_config(environment_path) task = load_config(task_path) assert environment["environment_id"] == task_name + assert environment["physics"] in {"default", "newton"} assert "task_program" not in environment assert task["environment"] == {"component": "env.yaml"} assert (config_root / "task_program/program.yaml").is_file() diff --git a/tests/gym/envs/test_replay.py b/tests/gym/envs/test_replay.py index c2e3a7830..91a985cc6 100644 --- a/tests/gym/envs/test_replay.py +++ b/tests/gym/envs/test_replay.py @@ -56,7 +56,7 @@ def __init__( uid="UR10", fpath=get_data_path("UniversalRobots/UR10/UR10.urdf"), init_pos=(0.0, 0.0, 1.0), - drive_pros=JointDrivePropertiesCfg(drive_type="force"), + joint_drive_props=JointDrivePropertiesCfg(drive_type="force"), ) cfg.rigid_object = [ RigidObjectCfg( @@ -469,7 +469,7 @@ def __init__( uid="UR10", fpath=get_data_path("UniversalRobots/UR10/UR10.urdf"), init_pos=(0.0, 0.0, 1.0), - drive_pros=JointDrivePropertiesCfg(drive_type="force"), + joint_drive_props=JointDrivePropertiesCfg(drive_type="force"), ) cfg.rigid_object = [ RigidObjectCfg( diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index 6fc8d18e8..ad309cbac 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -42,6 +42,7 @@ init_rollout_buffer_from_config, ) from embodichain.lab.sim.robots import URRobotCfg +from embodichain.lab.sim.cfg import DefaultPhysicsCfg, NewtonPhysicsCfg from embodichain.lab.sim.sensors import CameraCfg from embodichain.utils.utility import load_config, save_config @@ -63,6 +64,178 @@ CUBE_SCENE_REGISTRY_ID = "task_program_repeated_pick_place" +def test_env_launcher_args_include_physics(): + """Test that launcher args expose the physics backend config selector.""" + parser = argparse.ArgumentParser() + add_env_launcher_args_to_parser(parser) + + default_args = parser.parse_args([]) + assert default_args.physics == "default" + assert default_args.device is None + + newton_args = parser.parse_args(["--physics", "newton"]) + assert newton_args.physics == "newton" + assert newton_args.device is None + + +def test_required_gym_launcher_preserves_device_when_omitted() -> None: + """A config-backed launcher does not manufacture a CPU override.""" + parser = argparse.ArgumentParser() + add_env_launcher_args_to_parser(parser, require_gym_config=True) + args = parser.parse_args(["--gym_config", "newton.yaml"]) + gym_config = {"physics": "newton", "device": "cuda:1"} + + merged_config = merge_args_with_gym_config(args, gym_config) + + assert args.device is None + assert merged_config["device"] == "cuda:1" + + +def test_required_gym_launcher_applies_explicit_cpu_device() -> None: + """An explicit CPU selection overrides a Newton config uniformly.""" + parser = argparse.ArgumentParser() + add_env_launcher_args_to_parser(parser, require_gym_config=True) + args = parser.parse_args(["--gym_config", "newton.yaml", "--device", "cpu"]) + + merged_config = merge_args_with_gym_config(args, {"physics": "newton"}) + + assert merged_config["device"] == "cpu" + + +def test_merge_args_with_gym_config_rejects_physics_override(): + """A launcher cannot change the backend owned by a Gym config file.""" + parser = argparse.ArgumentParser() + add_env_launcher_args_to_parser(parser, require_gym_config=True) + args = parser.parse_args(["--gym_config", "default.yaml", "--physics", "newton"]) + + with pytest.raises(ValueError, match="cannot override a file-owned backend"): + merge_args_with_gym_config(args, {"physics": "default"}) + + +def test_merge_args_with_gym_config_accepts_declared_physics(): + """The optional launcher value may confirm the file-owned backend.""" + parser = argparse.ArgumentParser() + add_env_launcher_args_to_parser(parser, require_gym_config=True) + args = parser.parse_args(["--gym_config", "newton.yaml", "--physics", "newton"]) + + merged_config = merge_args_with_gym_config(args, {"physics": "newton"}) + + assert merged_config["physics"] == "newton" + + +def test_config_to_cfg_requires_explicit_physics_backend(): + """Inline Gym configs cannot rely on an implicit Default backend.""" + config = {"id": "EmbodiedEnv-v1", "env": {}, "robot": {"uid": "robot"}} + + with pytest.raises(ValueError, match="explicitly declare physics"): + config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + +@pytest.mark.parametrize("backend", (None, True, "physx", " newton")) +def test_config_to_cfg_rejects_invalid_physics_backend(backend: object) -> None: + """Only the two exact public backend names are accepted.""" + config = { + "id": "EmbodiedEnv-v1", + "physics": backend, + "env": {}, + "robot": {"uid": "robot"}, + } + + with pytest.raises(ValueError, match="exactly 'default' or 'newton'"): + config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + +@pytest.mark.parametrize( + ("backend", "physics_config", "field_name"), + ( + ("default", {"num_substeps": 2}, "num_substeps"), + ("newton", {"enable_ccd": True}, "enable_ccd"), + ), +) +def test_config_to_cfg_rejects_other_backend_physics_fields( + backend: str, + physics_config: dict[str, object], + field_name: str, +) -> None: + """Backend-native world fields must match the declared backend.""" + config = { + "id": "EmbodiedEnv-v1", + "physics": backend, + "physics_config": physics_config, + "env": {}, + "robot": {"uid": "robot"}, + } + + with pytest.raises( + ValueError, + match=rf"physics_config does not match.*{backend}.*{field_name}", + ): + config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + +@pytest.mark.parametrize( + ("backend", "physics_config", "config_type"), + ( + ("default", {"enable_ccd": True}, DefaultPhysicsCfg), + ("newton", {"num_substeps": 2}, NewtonPhysicsCfg), + ), +) +def test_config_to_cfg_builds_declared_physics_config( + backend: str, + physics_config: dict[str, object], + config_type: type[DefaultPhysicsCfg] | type[NewtonPhysicsCfg], +) -> None: + """Each file constructs only the physics config type it declares.""" + config = { + "id": "EmbodiedEnv-v1", + "physics": backend, + "physics_config": physics_config, + "env": {}, + "robot": {"uid": "robot"}, + } + + cfg = config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + assert type(cfg.sim_cfg.physics_cfg) is config_type + + +@pytest.mark.parametrize( + ("backend", "expected_device"), + (("default", "cpu"), ("newton", "cuda:0")), + ids=("default", "newton"), +) +def test_config_to_cfg_preserves_backend_device_default( + backend: str, + expected_device: str, +) -> None: + """Omitting device keeps the selected backend's own default.""" + config = { + "id": "EmbodiedEnv-v1", + "physics": backend, + "env": {}, + "robot": {"uid": "robot"}, + } + + cfg = config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + assert cfg.sim_cfg.device == expected_device + + +def test_config_to_cfg_applies_explicit_cpu_to_newton() -> None: + """The shared device field explicitly selects Newton CPU execution.""" + config = { + "id": "EmbodiedEnv-v1", + "physics": "newton", + "device": "cpu", + "env": {}, + "robot": {"uid": "robot"}, + } + + cfg = config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + assert cfg.sim_cfg.device == "cpu" + + class TestInitRolloutBufferFromConfig: """Tests for init_rollout_buffer_from_config function.""" @@ -297,11 +470,12 @@ def test_merge_args_with_gym_config_overrides_max_episodes(): device="cpu", headless=False, renderer="auto", + physics="default", gpu_id=0, arena_space=5.0, max_episodes=12, ) - gym_config = {"max_episodes": 3, "id": "Dummy-v0"} + gym_config = {"max_episodes": 3, "id": "Dummy-v0", "physics": "default"} merged_config = merge_args_with_gym_config(args, gym_config) @@ -316,11 +490,12 @@ def test_merge_args_with_gym_config_keeps_default_max_episodes(): device="cpu", headless=False, renderer="auto", + physics="default", gpu_id=0, arena_space=5.0, max_episodes=None, ) - gym_config = {"max_episodes": 3, "id": "Dummy-v0"} + gym_config = {"max_episodes": 3, "id": "Dummy-v0", "physics": "default"} merged_config = merge_args_with_gym_config(args, gym_config) @@ -346,7 +521,9 @@ def test_merge_args_with_gym_config_enables_headless_viser(): viser_env_ids=[1, 3], ) - merged_config = merge_args_with_gym_config(args, {"id": "Dummy-v0"}) + merged_config = merge_args_with_gym_config( + args, {"id": "Dummy-v0", "physics": "default"} + ) assert merged_config["headless"] is True assert merged_config["visualization"] == { @@ -379,7 +556,9 @@ def test_merge_args_with_gym_config_accepts_all_viser_environments(): viser_env_ids=["all"], ) - merged_config = merge_args_with_gym_config(args, {"id": "Dummy-v0"}) + merged_config = merge_args_with_gym_config( + args, {"id": "Dummy-v0", "physics": "default"} + ) assert merged_config["visualization"]["env_ids"] is None @@ -390,7 +569,11 @@ def test_launcher_preserves_gym_renderer_when_cli_omits_override(): add_env_launcher_args_to_parser(parser, require_gym_config=True) args = parser.parse_args(["--gym_config", "gym_config.yaml"]) - gym_config = {"id": "Dummy-v0", "render_cfg": {"renderer": "rt"}} + gym_config = { + "id": "Dummy-v0", + "physics": "default", + "render_cfg": {"renderer": "rt"}, + } merged_config = merge_args_with_gym_config(args, gym_config) assert args.renderer is None @@ -459,7 +642,9 @@ def test_viser_launcher_flag_implies_headless_viser_commands(): add_env_launcher_args_to_parser(parser) args = parser.parse_args(["--viser"]) - merged_config = merge_args_with_gym_config(args, {"id": "Dummy-v0"}) + merged_config = merge_args_with_gym_config( + args, {"id": "Dummy-v0", "physics": "default"} + ) assert merged_config["headless"] is True assert merged_config["visualization"]["backend"] == "viser" @@ -589,6 +774,7 @@ def _environment_component_payload() -> dict[str, object]: simulation = load_config(_CUBE_ENVIRONMENT_PATH)["simulation"] return { "environment_id": "configured_pick", + "physics": "default", "max_episodes": 5, "max_episode_steps": 1200, "simulation": simulation, @@ -639,6 +825,7 @@ def _write_deployment( def test_robot_class_type_preserves_ur_variant(self): config = { "id": "EmbodiedEnv-v1", + "physics": "default", "env": {}, "robot": { "class_type": "URRobot", @@ -678,6 +865,7 @@ def test_handwritten_config_composes_embodiment_without_task_program( ) config = { "id": "EmbodiedEnv-v1", + "physics": "default", "env": {}, "embodiment": { "component": "embodiment.yaml", @@ -750,6 +938,7 @@ def test_handwritten_config_rejects_mixed_embodiment_ownership( ) config = { "id": "EmbodiedEnv-v1", + "physics": "default", "env": {}, "embodiment": {"component": "embodiment.yaml"}, field_name: {} if field_name == "robot" else [], @@ -776,6 +965,7 @@ def test_handwritten_config_composes_scene_without_task_program( ) config = { "id": "EmbodiedEnv-v1", + "physics": "default", "env": {}, "robot": {"uid": "TestRobot"}, "scene": {"component": "scene.yaml"}, @@ -894,6 +1084,64 @@ def test_environment_component_rejects_separate_scene(self, tmp_path) -> None: source_path=tmp_path / "task.ur5.yaml", ) + def test_environment_component_requires_physics_backend(self, tmp_path) -> None: + """A reusable physical environment owns one explicit backend.""" + self._write_deployment(tmp_path) + environment_path = tmp_path / "env.yaml" + environment = load_config(environment_path) + environment.pop("physics") + save_config(environment_path, environment) + + with pytest.raises(ValueError, match="missing required fields.*physics"): + config_to_cfg( + self._environment_component_gym_config(), + manager_modules=DEFAULT_MANAGER_MODULES, + source_path=tmp_path / "task.ur5.yaml", + ) + + def test_environment_component_builds_declared_newton_config( + self, + tmp_path, + ) -> None: + """A Newton environment component owns its matching world settings.""" + self._write_deployment(tmp_path) + environment_path = tmp_path / "env.yaml" + environment = load_config(environment_path) + environment["physics"] = "newton" + environment["physics_config"] = {"num_substeps": 2} + save_config(environment_path, environment) + + cfg = config_to_cfg( + self._environment_component_gym_config(), + manager_modules=DEFAULT_MANAGER_MODULES, + source_path=tmp_path / "task.ur5.yaml", + ) + + assert isinstance(cfg.sim_cfg.physics_cfg, NewtonPhysicsCfg) + assert cfg.sim_cfg.physics_cfg.num_substeps == 2 + + @pytest.mark.parametrize( + ("field_name", "value"), + (("physics", "newton"), ("physics_config", {"num_substeps": 2})), + ) + def test_environment_component_owns_physics_configuration( + self, + tmp_path, + field_name: str, + value: object, + ) -> None: + """A deployment cannot override its environment's backend or settings.""" + self._write_deployment(tmp_path) + config = self._environment_component_gym_config() + config[field_name] = value + + with pytest.raises(ValueError, match="owns physics and physics_config"): + config_to_cfg( + config, + manager_modules=DEFAULT_MANAGER_MODULES, + source_path=tmp_path / "task.ur5.yaml", + ) + def test_scene_binding_rejects_unknown_physical_entity(self, tmp_path) -> None: """Semantic roots must bind an entity declared by the physical scene.""" self._write_deployment(tmp_path) @@ -1407,6 +1655,7 @@ def test_gym_config_rejects_wrongly_typed_dlss_scalars( """Malformed file values fail at DLSS decoding with the offending field name.""" config = { "id": "EmbodiedEnv-v1", + "physics": "default", "env": {}, "robot": { "class_type": "URRobot", @@ -1477,6 +1726,7 @@ def test_gym_config_parses_automatic_robot_gizmo_settings( def test_yaml_gym_config_parses_to_cfg(self, tmp_path): config = { "id": "EmbodiedEnv-v1", + "physics": "default", "seed": 2026, "max_episode_steps": 100, "physics_config": { @@ -1581,6 +1831,7 @@ def test_yaml_gym_config_parses_to_cfg(self, tmp_path): def test_json_dataset_save_failed_episodes_parses_from_top_level(self, tmp_path): config = { "id": "EmbodiedEnv-v1", + "physics": "default", "env": { "events": {}, "observations": {}, @@ -1622,6 +1873,7 @@ def test_json_dataset_save_failed_episodes_parses_from_top_level(self, tmp_path) def test_build_env_cfg_applies_modifier_before_parsing(self, tmp_path): config = { "id": "EmbodiedEnv-v1", + "physics": "default", "max_episode_steps": 100, "physics_config": { "gravity": [0.0, 0.0, -3.71], @@ -1689,6 +1941,7 @@ def test_control_replay_disables_dataset_saving( ): config = { "id": "EmbodiedEnv-v1", + "physics": "default", "max_episode_steps": 100, "env": { "events": {}, diff --git a/tests/lab/scripts/test_preview_asset.py b/tests/lab/scripts/test_preview_asset.py index 2c17c900e..e5e926ec5 100644 --- a/tests/lab/scripts/test_preview_asset.py +++ b/tests/lab/scripts/test_preview_asset.py @@ -69,6 +69,21 @@ def test_joint_control_is_enabled_by_default_and_can_be_disabled() -> None: assert disabled.joint_control is False +def test_asset_physics_mode_accepts_cli_spelling_variants() -> None: + parser = _create_parser() + default = parser.parse_args(["--asset_path", ASSET_PATH]) + hyphenated = parser.parse_args( + ["--asset_path", ASSET_PATH, "--asset-physics-mode", "preserve"] + ) + underscored = parser.parse_args( + ["--asset_path", ASSET_PATH, "--asset_physics_mode", "preserve"] + ) + + assert default.asset_physics_mode == "overlay" + assert hyphenated.asset_physics_mode == "preserve" + assert underscored.asset_physics_mode == "preserve" + + def test_loaded_assets_are_published_immediately_in_viser() -> None: """Assets added after manager construction should be captured before waiting.""" sim = Mock() diff --git a/tests/lab/scripts/test_run_env.py b/tests/lab/scripts/test_run_env.py index 6c8d64fe5..742d41a97 100644 --- a/tests/lab/scripts/test_run_env.py +++ b/tests/lab/scripts/test_run_env.py @@ -159,7 +159,7 @@ def test_run_env_syncs_viser_images_each_step_by_default() -> None: """Run-env uses step-synchronized camera images when no FPS is supplied.""" args = _create_parser().parse_args(["--gym_config", GYM_CONFIG_PATH, "--viser"]) - merged = merge_args_with_gym_config(args, {"id": GYM_ID}) + merged = merge_args_with_gym_config(args, {"id": GYM_ID, "physics": "default"}) assert merged["visualization"]["sensor_image_fps"] is None @@ -177,7 +177,7 @@ def test_run_env_accepts_explicit_viser_image_fps() -> None: ] ) - merged = merge_args_with_gym_config(args, {"id": GYM_ID}) + merged = merge_args_with_gym_config(args, {"id": GYM_ID, "physics": "default"}) assert merged["visualization"]["sensor_image_fps"] == expected_fps @@ -191,6 +191,7 @@ def test_run_env_preserves_configured_viser_image_fps() -> None: args, { "id": GYM_ID, + "physics": "default", "visualization": {"sensor_image_fps": configured_fps}, }, ) diff --git a/tests/lab/task_program/semantics/test_calls.py b/tests/lab/task_program/semantics/test_calls.py index 9b56551ef..92e01ebfe 100644 --- a/tests/lab/task_program/semantics/test_calls.py +++ b/tests/lab/task_program/semantics/test_calls.py @@ -50,7 +50,7 @@ def _identity_pose() -> SemanticPose: - return SemanticPose((0.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0)) + return SemanticPose((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) def _call_descriptor( @@ -71,35 +71,35 @@ def _call_descriptor( def test_semantic_pose_owns_inputs_and_returns_independent_tensors() -> None: position = torch.tensor([1.0, 2.0, 3.0]) - quaternion = torch.tensor([1.0, 0.0, 0.0, 0.0]) + quaternion = torch.tensor([0.0, 0.0, 0.0, 1.0]) pose = SemanticPose(position, quaternion) position.zero_() quaternion.zero_() returned_position = pose.position - returned_quaternion = pose.quaternion_wxyz + returned_quaternion = pose.quaternion_xyzw returned_position.fill_(9.0) returned_quaternion.fill_(9.0) torch.testing.assert_close(pose.position, torch.tensor([1.0, 2.0, 3.0])) torch.testing.assert_close( - pose.quaternion_wxyz, - torch.tensor([1.0, 0.0, 0.0, 0.0]), + pose.quaternion_xyzw, + torch.tensor([0.0, 0.0, 0.0, 1.0]), ) -def test_semantic_pose_normalizes_wxyz_quaternion() -> None: - pose = SemanticPose((0.0, 0.0, 0.0), (2.0, 0.0, 0.0, 2.0)) +def test_semantic_pose_normalizes_xyzw_quaternion() -> None: + pose = SemanticPose((0.0, 0.0, 0.0), (0.0, 0.0, 2.0, 2.0)) expected = torch.tensor( - [math.sqrt(0.5), 0.0, 0.0, math.sqrt(0.5)], + [0.0, 0.0, math.sqrt(0.5), math.sqrt(0.5)], dtype=torch.float32, ) - torch.testing.assert_close(pose.quaternion_wxyz, expected) + torch.testing.assert_close(pose.quaternion_xyzw, expected) def test_semantic_pose_converts_to_homogeneous_matrix() -> None: - pose = SemanticPose((1.0, 2.0, 3.0), (2.0, 0.0, 0.0, 2.0)) + pose = SemanticPose((1.0, 2.0, 3.0), (0.0, 0.0, 2.0, 2.0)) expected = torch.tensor( [ @@ -115,7 +115,7 @@ def test_semantic_pose_converts_to_homogeneous_matrix() -> None: def test_semantic_call_metadata_is_deterministic_and_json_safe() -> None: call = Place( object=SceneObjectRef("cube"), - at=SemanticPose((1.0, 2.0, 3.0), (1.0, 0.0, 0.0, 0.0)), + at=SemanticPose((1.0, 2.0, 3.0), (0.0, 0.0, 0.0, 1.0)), resources={"primary": "left_arm"}, ) diff --git a/tests/lab/task_program/test_decoder.py b/tests/lab/task_program/test_decoder.py index 04a5f75fb..cdeffad40 100644 --- a/tests/lab/task_program/test_decoder.py +++ b/tests/lab/task_program/test_decoder.py @@ -62,15 +62,15 @@ def _program_data() -> dict[str, object]: "values": [ { "position": [0.45, -0.20, 0.20], - "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], }, { "position": [0.45, 0.00, 0.20], - "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], }, { "position": [0.45, 0.20, 0.20], - "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], }, ], } diff --git a/tests/lab/task_program/test_loader.py b/tests/lab/task_program/test_loader.py index d5139c6d7..f1ee762d6 100644 --- a/tests/lab/task_program/test_loader.py +++ b/tests/lab/task_program/test_loader.py @@ -258,7 +258,7 @@ def test_loads_task_program_json_normalizes_oversized_integer() -> None: "values": [ { "position": [10**400, 0, 0], - "quaternion_wxyz": [1, 0, 0, 0], + "quaternion_xyzw": [0, 0, 0, 1], } ], } diff --git a/tests/lab/task_program/test_program_compiler.py b/tests/lab/task_program/test_program_compiler.py index fed4343da..a79a0414c 100644 --- a/tests/lab/task_program/test_program_compiler.py +++ b/tests/lab/task_program/test_program_compiler.py @@ -131,10 +131,10 @@ def _integration() -> TaskProgramIntegrationCfg: def _pose(x: float, y: float = 0.0, z: float = 0.2) -> PoseCfg: - """Build one target pose with an identity WXYZ quaternion.""" + """Build one target pose with an identity XYZW quaternion.""" return PoseCfg( position=(x, y, z), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), ) @@ -156,7 +156,7 @@ def _program( def _assert_pose_equal(actual: SemanticPose, expected: SemanticPose) -> None: """Compare owned pose tensor values.""" assert torch.allclose(actual.position, expected.position) - assert torch.allclose(actual.quaternion_wxyz, expected.quaternion_wxyz) + assert torch.allclose(actual.quaternion_xyzw, expected.quaternion_xyzw) def _assert_semantic_call_equal( @@ -241,7 +241,7 @@ def test_compiler_matches_direct_python_semantic_calls_and_sequence_order() -> N ), HandOver( object=SceneObjectRef("cube"), - final_target=SemanticPose(target.position, target.quaternion_wxyz), + final_target=SemanticPose(target.position, target.quaternion_xyzw), resources={"destination": "right_actor"}, ), RegisteredSemanticCall( @@ -312,7 +312,7 @@ def test_repeat_expands_independent_segments_with_cyclic_targets() -> None: assert place.call.at is not None _assert_pose_equal( place.call.at, - SemanticPose(pose.position, pose.quaternion_wxyz), + SemanticPose(pose.position, pose.quaternion_xyzw), ) assert place.target_selections[0].value_index == index validator = segment.validators[0] diff --git a/tests/lab/task_program/test_semantic_compiler.py b/tests/lab/task_program/test_semantic_compiler.py index e16a60be2..c3e38424e 100644 --- a/tests/lab/task_program/test_semantic_compiler.py +++ b/tests/lab/task_program/test_semantic_compiler.py @@ -720,7 +720,7 @@ def test_curated_analysis_selects_monitors_per_semantic_call() -> None: Pick(object=SceneObjectRef("cube")), Place( object=SceneObjectRef("cube"), - at=SemanticPose((0.5, 0.0, 0.3), (1.0, 0.0, 0.0, 0.0)), + at=SemanticPose((0.5, 0.0, 0.3), (0.0, 0.0, 0.0, 1.0)), ), ) ) @@ -915,7 +915,7 @@ def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: object=SceneObjectRef("cube"), at=SemanticPose( (0.5, -0.2, 0.4), - (1.0, 0.0, 0.0, 0.0), + (0.0, 0.0, 0.0, 1.0), ), ), ) @@ -1166,7 +1166,7 @@ def test_analysis_is_provider_free_and_propagates_object_target() -> None: preset=_preset("safe", action_option_templates=templates), ), ) - drop = SemanticPose((0.4, 0.2, 0.3), (1.0, 0.0, 0.0, 0.0)) + drop = SemanticPose((0.4, 0.2, 0.3), (0.0, 0.0, 0.0, 1.0)) workflow = compiler.analyze( ( @@ -1214,7 +1214,7 @@ def test_pick_lookahead_uses_downstream_place_orientation_policy() -> None: preset=_preset("safe", action_option_templates=templates), ), ) - drop = SemanticPose((0.4, 0.2, 0.3), (1.0, 0.0, 0.0, 0.0)) + drop = SemanticPose((0.4, 0.2, 0.3), (0.0, 0.0, 0.0, 1.0)) workflow = compiler.analyze( ( Pick(object=SceneObjectRef("cube")), @@ -1450,7 +1450,7 @@ def test_place_uses_verified_object_to_eef_transform() -> None: preset=_preset("safe", action_option_templates=templates), ), ) - drop = SemanticPose((0.5, -0.2, 0.4), (1.0, 0.0, 0.0, 0.0)) + drop = SemanticPose((0.5, -0.2, 0.4), (0.0, 0.0, 0.0, 1.0)) workflow = compiler.analyze((Place(object=SceneObjectRef("cube"), at=drop),)) pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) semantics = compiler.ground( @@ -1498,7 +1498,7 @@ def test_place_can_keep_observed_object_orientation_at_target() -> None: object_to_eef = torch.eye(4).repeat(2, 1, 1) object_to_eef[:, 2, 3] = 0.12 context = _held_context(registry, semantics, object_to_eef) - drop = SemanticPose((0.5, -0.2, 0.4), (1.0, 0.0, 0.0, 0.0)) + drop = SemanticPose((0.5, -0.2, 0.4), (0.0, 0.0, 0.0, 1.0)) workflow = compiler.analyze((Place(object=SceneObjectRef("cube"), at=drop),)) grounded = compiler.ground(workflow, 0, context) @@ -1548,7 +1548,7 @@ def test_place_rejects_wrong_or_inactive_verified_holder() -> None: ( Place( object=SceneObjectRef("cube"), - at=SemanticPose((0.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0)), + at=SemanticPose((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)), ), ) ) @@ -1618,7 +1618,7 @@ def test_registered_lowerer_is_explicit_and_opaque_to_lookahead() -> None: registered, Place( object=SceneObjectRef("cube"), - at=SemanticPose((0.3, 0.0, 0.2), (1.0, 0.0, 0.0, 0.0)), + at=SemanticPose((0.3, 0.0, 0.2), (0.0, 0.0, 0.0, 1.0)), ), ) ) @@ -1635,11 +1635,11 @@ def test_registered_lowerer_can_certify_retained_object_lookahead() -> None: registry, _ = _scene_registry() registered_target = SemanticPose( (0.25, 0.1, 0.4), - (1.0, 0.0, 0.0, 0.0), + (0.0, 0.0, 0.0, 1.0), ) place_target = SemanticPose( (0.3, 0.0, 0.2), - (1.0, 0.0, 0.0, 0.0), + (0.0, 0.0, 0.0, 1.0), ) compiler, _ = _compiler( registry, diff --git a/tests/lab/task_program/test_semantic_executor_curobo_gpu.py b/tests/lab/task_program/test_semantic_executor_curobo_gpu.py index 0d76b7b9f..8805244d6 100644 --- a/tests/lab/task_program/test_semantic_executor_curobo_gpu.py +++ b/tests/lab/task_program/test_semantic_executor_curobo_gpu.py @@ -48,7 +48,7 @@ SkillDescriptor, ) from embodichain.lab.sim.atomic_actions.tracking import TrackingPolicy # noqa: E402 -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg # noqa: E402 +from embodichain.lab.sim.cfg import RigidBodyPhysicsCfg # noqa: E402 from embodichain.lab.sim.objects import RigidObjectCfg # noqa: E402 from embodichain.lab.sim.motion.motion_generator import MotionGenCfg, MotionGenerator from embodichain.lab.sim.motion.planners.curobo.curobo_planner import ( # noqa: E402 @@ -249,7 +249,7 @@ def test_semantic_runtime_replans_after_dynamic_curobo_world_change() -> None: cfg=RigidObjectCfg( uid=OBSTACLE_UID, shape=CubeCfg(size=OBSTACLE_SIZE), - attrs=RigidBodyAttributesCfg(), + attrs=RigidBodyPhysicsCfg(), body_type="kinematic", init_pos=OBSTACLE_START_POSITION, init_rot=[0.0, 0.0, 0.0], diff --git a/tests/lab/task_program/test_task_program_cfg.py b/tests/lab/task_program/test_task_program_cfg.py index 00aebc74f..5cd312805 100644 --- a/tests/lab/task_program/test_task_program_cfg.py +++ b/tests/lab/task_program/test_task_program_cfg.py @@ -183,7 +183,7 @@ def test_pose_rejects_zero_quaternion() -> None: with pytest.raises(ValueError, match="non-zero magnitude"): PoseCfg( position=(0.0, 0.0, 0.0), - quaternion_wxyz=(0.0, 0.0, 0.0, 0.0), + quaternion_xyzw=(0.0, 0.0, 0.0, 0.0), ) diff --git a/tests/learning/test_apg.py b/tests/learning/test_apg.py index 32a7348fc..ea5e6ca14 100644 --- a/tests/learning/test_apg.py +++ b/tests/learning/test_apg.py @@ -30,9 +30,14 @@ APG, APGCfg, build_algo, + complete_discounted_return, get_registered_algo_names, segmented_discounted_return, ) +from embodichain.learning.rl.gradients import ( + BatchedGradientNormStats, + clip_batched_gradient_norm, +) from embodichain.learning.rl.collector import ( DifferentiableCollector, DifferentiableRollout, @@ -149,6 +154,110 @@ def test_segmented_discounted_return_restarts_discount_after_done() -> None: assert torch.equal(returns, torch.tensor([5.0])) +def test_complete_discounted_return_stops_after_first_done() -> None: + observation = torch.zeros((1, 1)) + transitions = [] + for reward, done in ((1.0, False), (2.0, True), (100.0, False)): + transitions.append( + DifferentiableTransition( + observation=observation, + policy_output=TensorDict( + {"action": torch.zeros((1, 1))}, + batch_size=[1], + ), + reward=torch.tensor([reward]), + terminated=torch.tensor([done]), + truncated=torch.tensor([False]), + next_observation=observation, + info={}, + ) + ) + + returns = complete_discounted_return( + DifferentiableRollout(observation, tuple(transitions)), + gamma=0.5, + ) + + assert torch.equal(returns, torch.tensor([2.0])) + + +def test_action_adjoint_clip_is_per_environment_and_overflow_safe() -> None: + gradient = torch.tensor( + [[3.0, 4.0], [0.3, 0.4], [2.0e30, 2.0e30], [float("inf"), 0.0]] + ) + stats = BatchedGradientNormStats("cpu") + + clipped = clip_batched_gradient_norm(gradient, 1.0, stats) + + torch.testing.assert_close(clipped[0], torch.tensor([0.6, 0.8])) + torch.testing.assert_close(clipped[1], gradient[1]) + torch.testing.assert_close( + clipped[2], + torch.full((2,), 2.0**-0.5), + rtol=1.0e-5, + atol=1.0e-6, + ) + torch.testing.assert_close(clipped[3], torch.zeros(2)) + assert torch.isfinite(clipped).all() + assert stats.rows == 4 + assert stats.finite_rows == 3 + assert stats.clipped_rows == 2 + assert stats.nonfinite_rows == 1 + + +def test_complete_rollout_objective_scale_equalizes_sequence_lengths() -> None: + policy = _make_policy() + algorithm = APG(APGCfg(device="cpu", max_grad_norm=100.0), policy) + observation = torch.zeros((4, 1)) + waypoint_counts = torch.tensor([1.0, 2.0, 4.0, 8.0]) + transition = DifferentiableTransition( + observation=observation, + policy_output=TensorDict( + {"action": policy.actor.weight.sum() * torch.ones((4, 1))}, + batch_size=[4], + ), + reward=30.0 * waypoint_counts + policy.actor.weight.sum() * 0.0, + terminated=torch.zeros(4, dtype=torch.bool), + truncated=torch.zeros(4, dtype=torch.bool), + next_observation=observation, + info={}, + ) + rollout = DifferentiableRollout(observation, (transition,)) + + algorithm.begin_update() + algorithm.accumulate_complete_rollout( + rollout, + objective_scale=waypoint_counts.reciprocal(), + ) + metrics = algorithm.finish_update() + + assert metrics["objective"] == pytest.approx(30.0) + + +def test_apg_skips_gradient_above_preclip_safety_limit() -> None: + policy = _make_policy() + algorithm = APG( + APGCfg( + device="cpu", + max_grad_norm=1.0, + max_grad_norm_before_clip=0.01, + ), + policy, + ) + initial_weight = policy.actor.weight.detach().clone() + rollout = DifferentiableCollector( + _LinearDifferentiableEnv(), + policy, + torch.device("cpu"), + ).collect(num_steps=2, deterministic=True) + + metrics = algorithm.update(rollout) + + assert metrics["skipped_update"] == 1.0 + assert metrics["skipped_excessive_gradient"] == 1.0 + assert torch.equal(policy.actor.weight, initial_weight) + + def test_apg_entropy_uses_reward_discount_and_done_reset_semantics() -> None: policy = _make_policy() observation = torch.zeros((1, 1)) diff --git a/tests/learning/test_differentiable_trainer.py b/tests/learning/test_differentiable_trainer.py index a36023eed..ea9e5d95e 100644 --- a/tests/learning/test_differentiable_trainer.py +++ b/tests/learning/test_differentiable_trainer.py @@ -27,8 +27,10 @@ from gymnasium.spaces import Box from embodichain.learning.rl import ( + DifferentiableRolloutSpec, DifferentiableTrainer, DifferentiableTrainerCfg, + stratified_rollout_value, ) from embodichain.learning.rl.algo import APG, APGCfg from embodichain.learning.rl.models import ActorOnly @@ -74,6 +76,35 @@ def close(self) -> None: return None +class _ScheduledCompleteRolloutEnv(_QuadraticActionEnv): + def __init__(self, num_envs: int = 2) -> None: + super().__init__(num_envs) + self.prepared_indices: list[int] = [] + self.reset_calls = 0 + self.current_waypoint_count = 1 + + def prepare_differentiable_rollout( + self, + rollout_index: int, + ) -> DifferentiableRolloutSpec: + self.prepared_indices.append(rollout_index) + self.current_waypoint_count = stratified_rollout_value(rollout_index, 1, 3) + return DifferentiableRolloutSpec( + num_steps=2 * self.current_waypoint_count, + objective_scale=1.0 / self.current_waypoint_count, + metadata={"waypoint_count": float(self.current_waypoint_count)}, + ) + + def reset( + self, + *, + seed: int | None = None, + options: Mapping[str, Any] | None = None, + ) -> tuple[torch.Tensor, dict[str, Any]]: + self.reset_calls += 1 + return super().reset(seed=seed, options=options) + + def _make_components( ent_coef: float = 0.0, ) -> tuple[_QuadraticActionEnv, ActorOnly, APG]: @@ -116,6 +147,76 @@ def test_trainer_updates_policy_and_detaches_each_segment() -> None: assert env.detach_calls == 2 +def test_complete_rollout_mode_resets_each_scheduled_microbatch() -> None: + env = _ScheduledCompleteRolloutEnv() + actor = nn.Linear(1, 1, bias=False) + nn.init.constant_(actor.weight, 0.5) + policy = ActorOnly(1, 1, env.device, actor=actor) + algorithm = APG( + APGCfg( + device="cpu", + optimizer=OptimizerCfg(learning_rate=0.05), + max_grad_norm=10.0, + ), + policy, + ) + trainer = DifferentiableTrainer( + DifferentiableTrainerCfg( + rollout_mode="complete", + update_horizon=6, + gradient_accumulation_steps=3, + deterministic_actions=True, + clip_actions_to_space=True, + rollout_seed=17, + ), + env, + policy, + algorithm, + ) + + summary = trainer.train(total_timesteps=24) + + assert summary["num_updates"] == 1 + assert summary["global_step"] == 24 + assert env.prepared_indices == [0, 1, 2] + assert env.reset_calls == 3 + assert env.detach_calls == 3 + assert summary["last_train_metrics"][ + "train/rollout_waypoint_count_mean" + ] == pytest.approx(2.0) + + +def test_complete_rollout_mode_honors_exact_optimizer_update_budget() -> None: + env = _ScheduledCompleteRolloutEnv() + actor = nn.Linear(1, 1, bias=False) + policy = ActorOnly(1, 1, env.device, actor=actor) + algorithm = APG(APGCfg(device="cpu", max_grad_norm=10.0), policy) + trainer = DifferentiableTrainer( + DifferentiableTrainerCfg( + rollout_mode="complete", + update_horizon=6, + deterministic_actions=True, + ), + env, + policy, + algorithm, + ) + + summary = trainer.train(total_updates=2) + + assert summary["num_updates"] == 2 + assert env.prepared_indices == [0, 1] + assert summary["global_step"] == 12 + + +def test_stratified_rollout_value_balances_and_rotates_cycles() -> None: + first = [stratified_rollout_value(index, 1, 3) for index in range(3)] + second = [stratified_rollout_value(index, 1, 3) for index in range(3, 6)] + + assert first == [1, 2, 3] + assert second == [2, 3, 1] + + def test_update_horizon_keeps_optimizer_budget_fixed_across_segment_lengths() -> None: short_env, short_policy, short_algorithm = _make_components(ent_coef=0.2) short_trainer = DifferentiableTrainer( diff --git a/tests/learning/test_observation_normalization.py b/tests/learning/test_observation_normalization.py new file mode 100644 index 000000000..d871ca877 --- /dev/null +++ b/tests/learning/test_observation_normalization.py @@ -0,0 +1,76 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for semantic-mask-aware running observation normalization.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.learning.rl.normalization import RunningObservationNormalizer + + +def test_running_normalizer_matches_combined_population_statistics() -> None: + normalizer = RunningObservationNormalizer(3, "cpu") + first = torch.tensor([[1.0, 2.0, 3.0], [3.0, 4.0, 5.0]]) + second = torch.tensor([[5.0, 6.0, 7.0]]) + + normalizer.update(first) + normalizer.update(second) + + combined = torch.cat([first, second]) + # The small pseudo-count has zero mean, unit variance, matching the NMG + # training reference rather than an exact batch-only moment. + expected_count = 3.0001 + expected_mean = combined.sum(dim=0) / expected_count + expected_m2 = ( + combined.square().sum(dim=0) + 1.0e-4 - expected_count * expected_mean.square() + ) + torch.testing.assert_close(normalizer.mean, expected_mean) + torch.testing.assert_close(normalizer.var, expected_m2 / expected_count) + assert normalizer.count == pytest.approx(expected_count) + + +def test_running_normalizer_preserves_semantic_fields_and_gradients() -> None: + normalizer = RunningObservationNormalizer( + 3, + "cpu", + normalize_mask=torch.tensor([True, False, True]), + ) + normalizer.update(torch.tensor([[1.0, 0.0, 3.0], [3.0, 1.0, 7.0]])) + observation = torch.tensor([[2.0, 1.0, 5.0]], requires_grad=True) + + normalized = normalizer.normalize(observation) + normalized.sum().backward() + + assert normalized[0, 1] == 1.0 + assert observation.grad is not None + assert torch.isfinite(observation.grad).all() + assert observation.grad[0, 1] == 1.0 + + +def test_running_normalizer_checkpoint_round_trip() -> None: + source = RunningObservationNormalizer(2, "cpu", torch.tensor([True, False])) + source.update(torch.tensor([[2.0, 1.0], [4.0, 0.0]])) + restored = RunningObservationNormalizer(2, "cpu") + + restored.load_state_dict(source.state_dict()) + + torch.testing.assert_close(restored.mean, source.mean) + torch.testing.assert_close(restored.var, source.var) + assert restored.count == source.count + assert torch.equal(restored.normalize_mask, source.normalize_mask) diff --git a/tests/learning/test_shared_rollout.py b/tests/learning/test_shared_rollout.py index 907325948..388baae65 100644 --- a/tests/learning/test_shared_rollout.py +++ b/tests/learning/test_shared_rollout.py @@ -192,7 +192,7 @@ def test_embodied_env_writes_next_fields_into_external_rollout(): env_cfg.num_envs = 2 env_cfg.sim_cfg = SimulationManagerCfg( headless=True, - sim_device=torch.device("cpu"), + device=torch.device("cpu"), render_cfg=RenderCfg(renderer="hybrid"), gpu_id=0, ) diff --git a/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py b/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py index 7c286ede3..3e94a3253 100644 --- a/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py +++ b/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py @@ -33,7 +33,7 @@ pytest.skip("cuRobo V2 requires CUDA", allow_module_level=True) from embodichain.lab.sim import SimulationManager, SimulationManagerCfg # noqa: E402 -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg # noqa: E402 +from embodichain.lab.sim.cfg import RigidBodyPhysicsCfg # noqa: E402 from embodichain.lab.sim.objects import RigidObjectCfg # noqa: E402 from embodichain.lab.sim.robots import FrankaPandaCfg # noqa: E402 from embodichain.lab.sim.shapes import CubeCfg # noqa: E402 @@ -68,12 +68,13 @@ def _make_franka_curobo_engine(): cfg=RigidObjectCfg( uid="demo_block", shape=CubeCfg(size=DEMO_BLOCK_DIMS), - attrs=RigidBodyAttributesCfg(), - body_type="kinematic", + attrs=RigidBodyPhysicsCfg(), + body_type="static", init_pos=DEMO_BLOCK_POS, init_rot=[0.0, 0.0, 0.0], ) ) + sim.prepare() mg = MotionGenerator( MotionGenCfg( planner_cfg=CuroboPlannerCfg( diff --git a/tests/sim/atomic_actions/test_motion_strategy_e2e.py b/tests/sim/atomic_actions/test_motion_strategy_e2e.py index 6b7127c21..a3c083471 100644 --- a/tests/sim/atomic_actions/test_motion_strategy_e2e.py +++ b/tests/sim/atomic_actions/test_motion_strategy_e2e.py @@ -54,6 +54,7 @@ def _setup(self): } ) ) + sim.prepare() mg = MotionGenerator( MotionGenCfg(planner_cfg=ToppraPlannerCfg(robot_uid=self.ROBOT_UID)) ) diff --git a/tests/sim/atomic_actions/test_newton_collision_pipeline_schedule.py b/tests/sim/atomic_actions/test_newton_collision_pipeline_schedule.py new file mode 100644 index 000000000..2ea7c730b --- /dev/null +++ b/tests/sim/atomic_actions/test_newton_collision_pipeline_schedule.py @@ -0,0 +1,37 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Atomic-action Newton native-contact configuration tests.""" + +from __future__ import annotations + +import importlib + +import pytest + +from embodichain.lab.sim.cfg.simulation import NewtonPhysicsCfg + + +@pytest.mark.no_sim +def test_atomic_action_tutorial_disables_external_collision_pipeline() -> None: + module = importlib.import_module("scripts.tutorials.atomic_action.tutorial_utils") + + physics_cfg = module._tutorial_physics_cfg("newton") + + assert isinstance(physics_cfg, NewtonPhysicsCfg) + assert physics_cfg.collision_cfg is None + dexsim_cfg = physics_cfg.to_dexsim_cfg(gpu_id=0) + assert dexsim_cfg.collision_pipeline_cfg is None + assert dexsim_cfg.solver_cfg.use_mujoco_contacts is True diff --git a/tests/sim/atomic_actions/test_tutorial_utils.py b/tests/sim/atomic_actions/test_tutorial_utils.py index 139b1be9b..2ea292e0a 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -27,6 +27,7 @@ from types import SimpleNamespace from unittest.mock import MagicMock, call, patch +import numpy as np import pytest import torch @@ -34,6 +35,12 @@ ArticulationAffordanceGeometry, TimedTrajectory, ) +from embodichain.lab.sim.cfg import ( + DefaultPhysicsCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + RigidBodyMaterialCfg, +) from embodichain.lab.sim.motion.solvers import BaseSolver from scripts.tutorials.atomic_action.dynamic_obstacle_recovery import ( _animate_obstacle_to_pose, @@ -48,15 +55,22 @@ create_dual_tutorial_robot_cfg, ) from scripts.tutorials.atomic_action.tutorial_utils import ( + NEWTON_GRASP_CONTACT_DAMPING, + NEWTON_GRASP_CONTACT_STIFFNESS, + NEWTON_NATIVE_CONTACT_DIMENSION, ROBOTIQ_2F_140_TCP, ROBOTIQ_HAND_JOINT_PATTERN, TUTORIAL_ROBOTS, + add_tutorial_robot, broadcast_pose_batch, broadcast_waypoint_pose_batch, clone_local_pose_from_first_env, + configure_newton_gripper_contacts, + configure_newton_link_contacts, create_antipodal_semantics, create_curobo_motion_generator, create_franka_panda_robot_cfg, + create_tutorial_rigid_body_physics, create_tutorial_argument_parser, create_tutorial_robot_cfg, create_ur10_robotiq_robot_cfg, @@ -64,6 +78,7 @@ create_parallel_jaw_grasp_pose_generator, get_hand_open_close_qpos, replay_trajectory, + run_tutorial, should_open_tutorial_window, should_wait_for_tutorial_input, ) @@ -244,6 +259,118 @@ def _run_obstacle_animation(*, pace_wall_time: bool) -> tuple[MagicMock, MagicMo return obstacle, adapter +def test_atomic_action_tutorial_uses_native_mujoco_contact_settings() -> None: + module = importlib.import_module("scripts.tutorials.atomic_action.tutorial_utils") + + default_cfg = module._tutorial_physics_cfg("default") + newton_cfg = module._tutorial_physics_cfg("newton") + + assert isinstance(default_cfg, DefaultPhysicsCfg) + assert isinstance(newton_cfg, NewtonPhysicsCfg) + assert newton_cfg.num_substeps == 10 + assert newton_cfg.collision_cfg is None + assert newton_cfg.solver_cfg == { + "solver_type": "mujoco_warp", + "solver": "newton", + "integrator": "implicitfast", + "iterations": 20, + "ls_iterations": 100, + "nconmax": 1_000, + "njmax": 2_000, + "cone": "elliptic", + "impratio": 1_000.0, + "use_mujoco_contacts": True, + "enable_multiccd": True, + } + + dexsim_cfg = newton_cfg.to_dexsim_cfg(gpu_id=0) + assert dexsim_cfg.solver_cfg.solver_type == "mujoco_warp" + assert dexsim_cfg.solver_cfg.solver == "newton" + assert dexsim_cfg.solver_cfg.integrator == "implicitfast" + assert dexsim_cfg.solver_cfg.iterations == 20 + assert dexsim_cfg.solver_cfg.ls_iterations == 100 + assert dexsim_cfg.solver_cfg.nconmax == 1_000 + assert dexsim_cfg.solver_cfg.njmax == 2_000 + assert dexsim_cfg.solver_cfg.cone == "elliptic" + assert dexsim_cfg.solver_cfg.impratio == pytest.approx(1_000.0) + assert dexsim_cfg.solver_cfg.use_mujoco_contacts is True + assert dexsim_cfg.solver_cfg.enable_multiccd is True + assert dexsim_cfg.collision_pipeline_cfg is None + + +class _WarpArray: + """Small in-memory stand-in for a writable Warp array.""" + + def __init__(self, values: list[int]) -> None: + self.values = np.asarray(values, dtype=np.int32) + + def numpy(self) -> np.ndarray: + return self.values + + def assign(self, values: np.ndarray) -> None: + self.values = np.asarray(values, dtype=np.int32).copy() + + +def test_atomic_action_tutorial_enables_native_mujoco_torsional_friction() -> None: + module = importlib.import_module("scripts.tutorials.atomic_action.tutorial_utils") + mjc_geom_condim = np.asarray([3, 3, 3], dtype=np.int32) + mjw_geom_condim = _WarpArray([3, 3, 3]) + backend = SimpleNamespace( + solver_type="mujoco_warp", + cfg=SimpleNamespace( + requires_grad=False, + solver_cfg=SimpleNamespace(use_mujoco_contacts=True), + ), + model=SimpleNamespace(requires_grad=False), + solver=SimpleNamespace( + mj_model=SimpleNamespace(geom_condim=mjc_geom_condim), + mjw_model=SimpleNamespace(geom_condim=mjw_geom_condim), + ), + ) + sim = SimpleNamespace(is_newton_backend=True, _world=object()) + + with patch( + "dexsim.engine.newton_physics.backend_registry.get_newton_backend", + return_value=backend, + ): + configured = module._configure_newton_native_contact_dimension(sim) + + expected = np.full(3, NEWTON_NATIVE_CONTACT_DIMENSION, dtype=np.int32) + assert configured is True + assert np.array_equal(mjc_geom_condim, expected) + assert np.array_equal(mjw_geom_condim.numpy(), expected) + + +def test_atomic_action_tutorial_leaves_external_newton_contacts_unchanged() -> None: + module = importlib.import_module("scripts.tutorials.atomic_action.tutorial_utils") + mjc_geom_condim = np.asarray([3, 3], dtype=np.int32) + mjw_geom_condim = _WarpArray([3, 3]) + backend = SimpleNamespace( + solver_type="mujoco_warp", + cfg=SimpleNamespace( + requires_grad=False, + solver_cfg=SimpleNamespace(use_mujoco_contacts=False), + ), + model=SimpleNamespace(requires_grad=False), + solver=SimpleNamespace( + mj_model=SimpleNamespace(geom_condim=mjc_geom_condim), + mjw_model=SimpleNamespace(geom_condim=mjw_geom_condim), + ), + ) + sim = SimpleNamespace(is_newton_backend=True, _world=object()) + + with patch( + "dexsim.engine.newton_physics.backend_registry.get_newton_backend", + return_value=backend, + ): + configured = module._configure_newton_native_contact_dimension(sim) + + expected = np.full(2, 3, dtype=np.int32) + assert configured is False + assert np.array_equal(mjc_geom_condim, expected) + assert np.array_equal(mjw_geom_condim.numpy(), expected) + + @pytest.mark.parametrize( ( "module_name", @@ -472,8 +599,8 @@ def test_franka_tutorial_config_uses_ur5_gripper_component() -> None: assert franka_cfg.init_qpos[-2:] == [0.0, 0.0] assert franka_cfg.init_rot == FRANKA_TUTORIAL_BASE_ROTATION for property_name in ("stiffness", "damping", "max_effort"): - ur5_values = getattr(ur5_cfg.drive_pros, property_name) - franka_values = getattr(franka_cfg.drive_pros, property_name) + ur5_values = getattr(ur5_cfg.joint_drive_props, property_name) + franka_values = getattr(franka_cfg.joint_drive_props, property_name) assert franka_values["gripper_finger1_joint_1"] == ( ur5_values["gripper_finger1_joint_1"] ) @@ -694,12 +821,95 @@ def test_curobo_motion_generator_factory_selects_curobo_backend() -> None: with patch( "scripts.tutorials.atomic_action.tutorial_utils.MotionGenerator" ) as motion_generator_cls: - result = create_curobo_motion_generator(robot) + result = create_curobo_motion_generator(robot, use_cuda_graph=False) cfg = motion_generator_cls.call_args.kwargs["cfg"] assert result is motion_generator_cls.return_value assert cfg.planner_cfg.planner_type == "curobo" assert cfg.planner_cfg.robot_uid == "tutorial_robot" + assert cfg.planner_cfg.use_cuda_graph is False + + +def test_tutorial_rigid_body_physics_groups_backend_specific_properties() -> None: + physics = create_tutorial_rigid_body_physics( + mass=0.05, + static_friction=0.8, + dynamic_friction=0.4, + restitution=0.1, + linear_damping=0.2, + angular_damping=0.3, + max_depenetration_velocity=1.5, + enable_ccd=True, + min_position_iters=4, + min_velocity_iters=2, + contact_offset=0.01, + rest_offset=0.001, + ) + + assert physics.mass_props.mass == 0.05 + assert physics.material_props.static_friction == 0.8 + assert physics.material_props.dynamic_friction == 0.4 + assert physics.material_props.restitution == 0.1 + assert physics.rigid_props.linear_damping == 0.2 + assert physics.rigid_props.angular_damping == 0.3 + assert physics.rigid_props.max_depenetration_velocity == 1.5 + assert physics.rigid_props.enable_ccd is True + assert physics.rigid_props.min_position_iters == 4 + assert physics.rigid_props.min_velocity_iters == 2 + assert physics.collision_props.contact_offset == 0.01 + assert physics.collision_props.rest_offset == 0.001 + + +def test_tutorial_rigid_body_physics_adds_only_newton_contact_response() -> None: + empty_physics = create_tutorial_rigid_body_physics() + default_physics = create_tutorial_rigid_body_physics( + static_friction=0.8, + dynamic_friction=0.4, + ) + newton_physics = create_tutorial_rigid_body_physics( + static_friction=0.8, + dynamic_friction=0.4, + newton_contact=True, + ) + + assert empty_physics.material_props is None + assert type(default_physics.material_props) is RigidBodyMaterialCfg + assert type(newton_physics.material_props) is NewtonRigidBodyMaterialCfg + assert newton_physics.material_props.static_friction == pytest.approx(0.8) + assert newton_physics.material_props.dynamic_friction == pytest.approx(0.4) + assert newton_physics.material_props.ke == pytest.approx( + NEWTON_GRASP_CONTACT_STIFFNESS + ) + assert newton_physics.material_props.kd == pytest.approx( + NEWTON_GRASP_CONTACT_DAMPING + ) + + +def test_run_tutorial_uses_deferred_simulation_cleanup() -> None: + sim = MagicMock() + sim.is_window_recording.return_value = False + + with ( + patch( + "scripts.tutorials.atomic_action.tutorial_utils." + "SimulationManager.is_instantiated", + return_value=True, + ), + patch( + "scripts.tutorials.atomic_action.tutorial_utils." + "SimulationManager.get_instance", + return_value=sim, + ), + patch( + "scripts.tutorials.atomic_action.tutorial_utils." + "SimulationManager.flush_cleanup_queue" + ) as flush_cleanup_queue, + ): + run_tutorial(lambda: None) + + sim.wait_window_record_saves.assert_called_once_with() + sim.destroy.assert_called_once_with(exit_process=False) + flush_cleanup_queue.assert_called_once_with() def test_shared_robot_selection_keeps_ur5_default_and_accepts_all_variants() -> None: @@ -714,6 +924,24 @@ def test_shared_robot_selection_keeps_ur5_default_and_accepts_all_variants() -> assert ur10_args.robot == "ur10" +def test_handover_tutorial_defaults_to_cpu_for_grasp_planning() -> None: + module = importlib.import_module("scripts.tutorials.atomic_action.hand_over") + + with patch("sys.argv", ["hand_over.py"]): + args = module.parse_arguments() + + assert args.device == "cpu" + + +def test_assemble_tutorial_uses_center_grasp_for_laid_soda_can() -> None: + module = importlib.import_module("scripts.tutorials.atomic_action.assemble") + + assert module.PICKUP_OBJECT_PART == "center" + assert "pick_object_part=PICKUP_OBJECT_PART" in inspect.getsource( + module.run_assemble_demo + ) + + def test_arm_direction_uses_selected_robot_solver_roots() -> None: robot = MagicMock() robot.cfg.solver_cfg = { @@ -760,6 +988,7 @@ def test_place_tutorial_registers_pick_object_with_simulation_engine_factory() - ) sim = MagicMock() sim.device = torch.device("cpu") + sim.is_newton_backend = False sim.sim_config.physics_dt = PHYSICS_DT robot = MagicMock() robot.get_qpos.return_value = torch.zeros(1, 8) @@ -799,6 +1028,165 @@ def test_place_tutorial_registers_pick_object_with_simulation_engine_factory() - engine.initial_context.assert_called_once_with(control_dt=PHYSICS_DT) +@pytest.mark.parametrize( + "link_name", + ( + "gripper_finger1_link_1", + "left_gripper_finger2_link_1", + "right_gripper_finger1_link_1", + "left_inner_finger_pad", + "right_left_outer_knuckle", + ), +) +def test_shared_tutorial_gripper_uses_newton_contact_material( + link_name: str, +) -> None: + sim = SimpleNamespace(is_newton_backend=True) + robot_cfg = SimpleNamespace(link_attrs=None) + + configure_newton_gripper_contacts(sim, robot_cfg) + + override = robot_cfg.link_attrs["newton_gripper_contacts"] + material = override.attrs.material_props + assert isinstance(material, NewtonRigidBodyMaterialCfg) + assert material.ke == pytest.approx(NEWTON_GRASP_CONTACT_STIFFNESS) + assert material.kd == pytest.approx(NEWTON_GRASP_CONTACT_DAMPING) + assert override.attrs.mass_props.recompute_inertia is True + assert re.fullmatch(override.link_names_expr[0], link_name) + + +def test_shared_tutorial_preserves_default_gripper_contact_config() -> None: + existing_link_attrs = {"existing": MagicMock()} + sim = SimpleNamespace(is_newton_backend=False) + robot_cfg = SimpleNamespace(link_attrs=existing_link_attrs) + + configure_newton_gripper_contacts(sim, robot_cfg) + + assert robot_cfg.link_attrs is existing_link_attrs + + +def test_add_tutorial_robot_authors_newton_contacts_before_spawn() -> None: + sim = MagicMock() + sim.is_newton_backend = True + robot_cfg = SimpleNamespace(link_attrs=None) + + with patch( + "scripts.tutorials.atomic_action.tutorial_utils.create_tutorial_robot_cfg", + return_value=robot_cfg, + ): + result = add_tutorial_robot(sim, "ur5") + + assert result is sim.add_robot.return_value + sim.add_robot.assert_called_once_with(cfg=robot_cfg) + material = robot_cfg.link_attrs["newton_gripper_contacts"].attrs.material_props + assert material.ke == pytest.approx(NEWTON_GRASP_CONTACT_STIFFNESS) + assert material.kd == pytest.approx(NEWTON_GRASP_CONTACT_DAMPING) + + +def test_shared_tutorial_tunes_selected_newton_articulation_link() -> None: + sim = SimpleNamespace(is_newton_backend=True) + articulation_cfg = SimpleNamespace(link_attrs={"existing": MagicMock()}) + + configure_newton_link_contacts( + sim, + articulation_cfg, + group_name="newton_handle_contacts", + link_names_expr=["door_handle"], + ) + + assert "existing" in articulation_cfg.link_attrs + override = articulation_cfg.link_attrs["newton_handle_contacts"] + assert override.link_names_expr == ["door_handle"] + assert override.attrs.material_props.ke == pytest.approx( + NEWTON_GRASP_CONTACT_STIFFNESS + ) + assert override.attrs.material_props.kd == pytest.approx( + NEWTON_GRASP_CONTACT_DAMPING + ) + + +@pytest.mark.parametrize( + ("module_name", "factory_name", "group_name", "contact_link"), + ( + ("slide", "create_drawer", "newton_handle_contacts", "large_handle_bar"), + ("open_door", "create_microwave", "newton_handle_contacts", "door_handle"), + ("twist", "create_microwave", "newton_knob_contacts", "cap_1"), + ("press", "create_microwave", "newton_button_contacts", "button_cap"), + ), +) +def test_articulation_contact_tutorials_author_newton_material_before_spawn( + module_name: str, + factory_name: str, + group_name: str, + contact_link: str, +) -> None: + module = importlib.import_module(f"scripts.tutorials.atomic_action.{module_name}") + sim = MagicMock() + sim.is_newton_backend = True + + with patch.object(module, "get_data_path", return_value="/tmp/tutorial.urdf"): + result = getattr(module, factory_name)(sim) + + assert result is sim.add_articulation.return_value + cfg = sim.add_articulation.call_args.kwargs["cfg"] + assert cfg.asset_physics_mode == "overlay" + assert cfg.root_props.fixed_base is True + override = cfg.link_attrs[group_name] + assert override.link_names_expr == [contact_link] + assert override.attrs.material_props.ke == pytest.approx( + NEWTON_GRASP_CONTACT_STIFFNESS + ) + assert override.attrs.material_props.kd == pytest.approx( + NEWTON_GRASP_CONTACT_DAMPING + ) + + +@pytest.mark.parametrize( + ("is_newton_backend", "expected_material_type"), + ( + (False, RigidBodyMaterialCfg), + (True, NewtonRigidBodyMaterialCfg), + ), +) +def test_place_cube_uses_backend_scoped_contact_material( + is_newton_backend: bool, + expected_material_type: type[RigidBodyMaterialCfg], +) -> None: + module = importlib.import_module("scripts.tutorials.atomic_action.place") + sim = MagicMock() + sim.is_newton_backend = is_newton_backend + obj = MagicMock() + sim.add_rigid_object.return_value = obj + + with patch.object(module, "clone_local_pose_from_first_env"): + result = module.create_pick_object(sim) + + cfg = sim.add_rigid_object.call_args.kwargs["cfg"] + material = cfg.attrs.material_props + assert type(material) is expected_material_type + assert material.dynamic_friction == pytest.approx(0.97) + assert material.static_friction == pytest.approx(0.99) + if is_newton_backend: + assert material.ke == pytest.approx(NEWTON_GRASP_CONTACT_STIFFNESS) + assert material.kd == pytest.approx(NEWTON_GRASP_CONTACT_DAMPING) + result.clear_dynamics.assert_called_once_with() + + +def test_move_held_object_cup_starts_above_the_ground() -> None: + module = importlib.import_module("scripts.tutorials.atomic_action.move_held_object") + sim = MagicMock() + sim.is_newton_backend = True + obj = MagicMock() + sim.add_rigid_object.return_value = obj + + with patch.object(module, "clone_local_pose_from_first_env"): + module.create_pick_object(sim) + + cfg = sim.add_rigid_object.call_args.kwargs["cfg"] + assert cfg.init_pos == [*module.OBJECT_XY, module.OBJECT_INITIAL_Z] + assert module.OBJECT_INITIAL_Z == pytest.approx(0.05) + + def test_atomic_action_tutorial_scene_strategies_cover_every_entry_point() -> None: classified = ( set(RIGID_SCENE_TUTORIAL_MODULES) @@ -898,6 +1286,77 @@ def test_replay_timed_trajectory_uses_arrival_intervals() -> None: assert robot.set_qpos.call_count == 3 +def test_replay_timed_trajectory_settles_once_after_newton_native_grasp() -> None: + sim = MagicMock() + sim.is_newton_backend = True + sim.sim_config.physics_dt = 0.1 + robot = MagicMock() + robot.control_parts = {"arm": ["joint1"], "hand": ["finger_joint"]} + robot.get_joint_ids.side_effect = lambda *, name: [2] if name == "hand" else [0, 1] + trajectory = TimedTrajectory.from_positions( + torch.tensor([[[0.0, 0.0, 0.0], [0.1, 0.0, 0.02], [0.2, 0.0, 0.02]]]), + env_ids=torch.tensor([0], dtype=torch.long), + dt=torch.tensor([[0.0, 0.1, 0.1]]), + ) + + with patch("scripts.tutorials.atomic_action.tutorial_utils.time.sleep"): + replay_trajectory( + sim, + robot, + trajectory, + Namespace(auto_play=False), + video_prefix="unused", + hold_steps=0, + ) + + assert sim.update.call_args_list == [call(step=1), call(step=4), call(step=1)] + robot.get_joint_ids.assert_called_once_with(name="hand") + + +def test_replay_timed_trajectory_settles_after_each_native_hand_phase() -> None: + sim = MagicMock() + sim.is_newton_backend = True + sim.sim_config.physics_dt = 0.1 + robot = MagicMock() + robot.control_parts = {"arm": ["joint1"], "hand": ["finger_joint"]} + robot.get_joint_ids.side_effect = lambda *, name: [2] if name == "hand" else [0, 1] + trajectory = TimedTrajectory.from_positions( + torch.tensor( + [ + [ + [0.0, 0.0, 0.0], + [0.1, 0.0, 0.01], + [0.2, 0.0, 0.02], + [0.3, 0.0, 0.02], + [0.4, 0.0, 0.01], + [0.5, 0.0, 0.0], + ] + ] + ), + env_ids=torch.tensor([0], dtype=torch.long), + dt=torch.tensor([[0.0, 0.1, 0.1, 0.1, 0.1, 0.1]]), + ) + + with patch("scripts.tutorials.atomic_action.tutorial_utils.time.sleep"): + replay_trajectory( + sim, + robot, + trajectory, + Namespace(auto_play=False), + video_prefix="unused", + hold_steps=0, + ) + + assert sim.update.call_args_list == [ + call(step=1), + call(step=1), + call(step=4), + call(step=1), + call(step=1), + call(step=4), + ] + + def test_broadcast_pose_batch_rejects_wrong_env_count() -> None: poses = torch.eye(4, dtype=torch.float32).unsqueeze(0).repeat(2, 1, 1) diff --git a/tests/sim/motion/planners/test_curobo_integration.py b/tests/sim/motion/planners/test_curobo_integration.py index df12238fe..b56cfcba6 100644 --- a/tests/sim/motion/planners/test_curobo_integration.py +++ b/tests/sim/motion/planners/test_curobo_integration.py @@ -36,7 +36,7 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg # noqa: E402 from embodichain.lab.sim.objects import RigidObjectCfg # noqa: E402 from embodichain.lab.sim.robots import FrankaPandaCfg # noqa: E402 -from embodichain.lab.sim.cfg import RigidBodyAttributesCfg # noqa: E402 +from embodichain.lab.sim.cfg import RigidBodyPhysicsCfg # noqa: E402 from embodichain.lab.sim.shapes import CubeCfg # noqa: E402 from embodichain.lab.sim.motion.motion_generator import ( MotionGenCfg, @@ -74,12 +74,13 @@ def _make_sim_robot(num_envs: int = 1): cfg=RigidObjectCfg( uid="demo_block", shape=CubeCfg(size=DEMO_BLOCK_DIMS), - attrs=RigidBodyAttributesCfg(), - body_type="kinematic", + attrs=RigidBodyPhysicsCfg(), + body_type="static", init_pos=DEMO_BLOCK_POS, init_rot=[0.0, 0.0, 0.0], ) ) + sim.prepare() return sim, robot, block diff --git a/tests/sim/motion/planners/test_curobo_planner.py b/tests/sim/motion/planners/test_curobo_planner.py index 4609677f1..828a67712 100644 --- a/tests/sim/motion/planners/test_curobo_planner.py +++ b/tests/sim/motion/planners/test_curobo_planner.py @@ -33,6 +33,7 @@ import torch import yaml +from embodichain.utils.math import matrix_from_quat from embodichain.lab.sim.motion.planners import CuroboPlannerCfg from embodichain.lab.sim.motion.planners.curobo.curobo_planner import ( CuroboPlanOptions, @@ -127,9 +128,14 @@ def test_public_config_imports_without_curobo(): def test_matrix_to_position_quaternion_uses_wxyz(): matrix = torch.eye(4).unsqueeze(0) + xyzw = torch.tensor([[1.0, 2.0, 3.0, 4.0]]) / math.sqrt(30.0) + matrix[:, :3, :3] = matrix_from_quat(xyzw) position, quaternion = _matrix_to_position_quaternion(matrix) assert torch.equal(position, torch.zeros(1, 3)) - assert torch.equal(quaternion, torch.tensor([[1.0, 0.0, 0.0, 0.0]])) + torch.testing.assert_close( + quaternion, + torch.tensor([[4.0, 1.0, 2.0, 3.0]]) / math.sqrt(30.0), + ) assert position.is_contiguous() assert quaternion.is_contiguous() @@ -468,7 +474,7 @@ def _identity_pose( translation: tuple[float, float, float] = (0.45, 0.0, 0.18), ) -> torch.Tensor: return torch.tensor( - [*translation, 1.0, 0.0, 0.0, 0.0], + [*translation, 0.0, 0.0, 0.0, 1.0], dtype=torch.float32, ) @@ -530,7 +536,7 @@ def test_cuboid_entry_off_origin_mesh_offsets_center(): def test_cuboid_entry_rotated_pose_preserves_center(): quaternion = torch.tensor( - [math.cos(math.pi / 4), 0.0, 0.0, math.sin(math.pi / 4)], + [0.0, 0.0, math.sin(math.pi / 4), math.cos(math.pi / 4)], dtype=torch.float32, ) pose = torch.cat([torch.tensor([0.45, 0.0, 0.18]), quaternion]) @@ -543,7 +549,9 @@ def test_cuboid_entry_rotated_pose_preserves_center(): )[0] assert fields["pose"][:3] == pytest.approx([0.45, 0.0, 0.18]) - assert fields["pose"][3:] == pytest.approx(quaternion.tolist()) + assert fields["pose"][3:] == pytest.approx( + [math.cos(math.pi / 4), 0.0, 0.0, math.sin(math.pi / 4)] + ) def test_cuboid_entry_accepts_homogeneous_pose(): @@ -572,7 +580,7 @@ def test_mesh_entry_serializes_flat_face_buffer(): assert (top_key, name) == ("mesh", "demo_block") assert len(fields["vertices"]) == 8 assert len(fields["faces"]) == 36 - assert fields["pose"] == pytest.approx(_identity_pose().tolist()) + assert fields["pose"] == pytest.approx([0.45, 0.0, 0.18, 1.0, 0.0, 0.0, 0.0]) def test_invalid_obstacle_representation_raises(): @@ -927,7 +935,7 @@ def test_generated_mesh_yaml_loads_in_curobo_scene_cfg(tmp_path): def _build_curobo_scene(sim_device: str = "cuda") -> tuple[object, object, object]: from embodichain.lab.sim import SimulationManager, SimulationManagerCfg - from embodichain.lab.sim.cfg import RigidBodyAttributesCfg + from embodichain.lab.sim.cfg import RigidBodyPhysicsCfg from embodichain.lab.sim.objects import RigidObjectCfg from embodichain.lab.sim.robots import FrankaPandaCfg from embodichain.lab.sim.shapes import CubeCfg @@ -948,12 +956,13 @@ def _build_curobo_scene(sim_device: str = "cuda") -> tuple[object, object, objec cfg=RigidObjectCfg( uid="block", shape=CubeCfg(size=_SIM_BLOCK_DIMS), - attrs=RigidBodyAttributesCfg(), - body_type="kinematic", + attrs=RigidBodyPhysicsCfg(), + body_type="static", init_pos=_SIM_BLOCK_POS, init_rot=(0.0, 0.0, 0.0), ) ) + sim.prepare() return sim, robot, block diff --git a/tests/sim/motion/planners/test_neural_planner.py b/tests/sim/motion/planners/test_neural_planner.py index dc8a50196..6705f9691 100644 --- a/tests/sim/motion/planners/test_neural_planner.py +++ b/tests/sim/motion/planners/test_neural_planner.py @@ -101,7 +101,7 @@ def compute_fk( batch = qpos.shape[0] if qpos.dim() > 1 else 1 if to_matrix: return torch.eye(4).repeat(batch, 1, 1) - return torch.tensor([[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]]).repeat(batch, 1) + return torch.tensor([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]).repeat(batch, 1) class FakeSimulationManager: diff --git a/tests/sim/motion/planners/test_toppra_batched.py b/tests/sim/motion/planners/test_toppra_batched.py index 4cfcde346..4394e23db 100644 --- a/tests/sim/motion/planners/test_toppra_batched.py +++ b/tests/sim/motion/planners/test_toppra_batched.py @@ -140,13 +140,14 @@ def _make_planner(self): from embodichain.lab.sim.robots import CobotMagicCfg sim = SimulationManager( - SimulationManagerCfg(headless=True, sim_device="cpu", num_envs=2) + SimulationManagerCfg(headless=True, device="cpu", num_envs=2) ) robot = sim.add_robot( cfg=CobotMagicCfg.from_dict( {"uid": "t", "init_pos": [0, 0, 0.7775], "init_qpos": [0.0] * 16} ) ) + sim.prepare() planner = ToppraPlanner(ToppraPlannerCfg(robot_uid="t", max_workers=1)) return planner, sim @@ -253,13 +254,14 @@ def test_plan_batched_pool_path(self, mp_context): from embodichain.lab.sim.robots import CobotMagicCfg sim = SimulationManager( - SimulationManagerCfg(headless=True, sim_device="cpu", num_envs=3) + SimulationManagerCfg(headless=True, device="cpu", num_envs=3) ) sim.add_robot( cfg=CobotMagicCfg.from_dict( {"uid": "p", "init_pos": [0, 0, 0.7775], "init_qpos": [0.0] * 16} ) ) + sim.prepare() planner = ToppraPlanner( ToppraPlannerCfg(robot_uid="p", max_workers=2, mp_context=mp_context) ) @@ -315,7 +317,7 @@ def test_workers_reaped_on_gc(self, mp_context): from embodichain.lab.sim.robots import CobotMagicCfg sim = SimulationManager( - SimulationManagerCfg(headless=True, sim_device="cpu", num_envs=3) + SimulationManagerCfg(headless=True, device="cpu", num_envs=3) ) sim.add_robot( cfg=CobotMagicCfg.from_dict( @@ -326,6 +328,7 @@ def test_workers_reaped_on_gc(self, mp_context): } ) ) + sim.prepare() planner = ToppraPlanner( ToppraPlannerCfg( robot_uid="close_reap", max_workers=2, mp_context=mp_context @@ -384,13 +387,14 @@ def test_batched_equals_inline_single(self): from embodichain.lab.sim.robots import CobotMagicCfg sim = SimulationManager( - SimulationManagerCfg(headless=True, sim_device="cpu", num_envs=4) + SimulationManagerCfg(headless=True, device="cpu", num_envs=4) ) sim.add_robot( cfg=CobotMagicCfg.from_dict( {"uid": "r", "init_pos": [0, 0, 0.7775], "init_qpos": [0.0] * 16} ) ) + sim.prepare() planner = ToppraPlanner(ToppraPlannerCfg(robot_uid="r", max_workers=1)) try: B, dofs = 4, 6 diff --git a/tests/sim/motion/planners/test_toppra_planner.py b/tests/sim/motion/planners/test_toppra_planner.py index ad5823132..f430b9dbb 100644 --- a/tests/sim/motion/planners/test_toppra_planner.py +++ b/tests/sim/motion/planners/test_toppra_planner.py @@ -36,7 +36,7 @@ def setup_simulation(self): cls = type(self) if hasattr(cls, "sim"): return - cls.sim_config = SimulationManagerCfg(headless=True, sim_device="cpu") + cls.sim_config = SimulationManagerCfg(headless=True, device="cpu") cls.sim = SimulationManager(cls.sim_config) cfg_dict = { @@ -45,6 +45,7 @@ def setup_simulation(self): "init_qpos": [0.0] * 16, } cls.robot = cls.sim.add_robot(cfg=CobotMagicCfg.from_dict(cfg_dict)) + cls.sim.prepare() def setup_method(self): self.setup_simulation() diff --git a/tests/sim/motion/solvers/test_differential_solver.py b/tests/sim/motion/solvers/test_differential_solver.py index c055ad617..00566eca2 100644 --- a/tests/sim/motion/solvers/test_differential_solver.py +++ b/tests/sim/motion/solvers/test_differential_solver.py @@ -33,7 +33,7 @@ class BaseSolverTest: def setup_simulation(self, solver_type: str): # Set up simulation with specified device (CPU or CUDA) - config = SimulationManagerCfg(headless=True, sim_device="cpu") + config = SimulationManagerCfg(headless=True, device="cpu") self.sim = SimulationManager(config) # Load robot URDF file @@ -61,6 +61,7 @@ def setup_simulation(self, solver_type: str): } self.robot: Robot = self.sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) + self.sim.prepare() @pytest.mark.parametrize("arm_name", ["left_arm", "right_arm"]) def test_differential_solver(self, arm_name: str): diff --git a/tests/sim/motion/solvers/test_neural_ik_solver.py b/tests/sim/motion/solvers/test_neural_ik_solver.py index 70af97f44..77d4afffb 100644 --- a/tests/sim/motion/solvers/test_neural_ik_solver.py +++ b/tests/sim/motion/solvers/test_neural_ik_solver.py @@ -52,7 +52,7 @@ class TestNeuralIKSolver: def _setup(self, tmp_path): checkpoint_path = _create_fake_checkpoint(tmp_path) - config = SimulationManagerCfg(headless=True, sim_device="cpu") + config = SimulationManagerCfg(headless=True, device="cpu") self.sim = SimulationManager(config) cfg = FrankaPandaCfg.from_dict({"robot_type": "panda"}) @@ -75,6 +75,7 @@ def _setup(self, tmp_path): ) self.robot = self.sim.add_robot(cfg=cfg) + self.sim.prepare() self.sim.update(step=100) def teardown_method(self): diff --git a/tests/sim/motion/solvers/test_opw_solver.py b/tests/sim/motion/solvers/test_opw_solver.py index 6822e211f..41e3e15df 100644 --- a/tests/sim/motion/solvers/test_opw_solver.py +++ b/tests/sim/motion/solvers/test_opw_solver.py @@ -70,8 +70,8 @@ def grid_sample_qpos_from_limits( class BaseSolverTest: sim = None # Define as a class attribute - def setup_simulation(self, sim_device): - config = SimulationManagerCfg(headless=True, sim_device=sim_device) + def setup_simulation(self, device): + config = SimulationManagerCfg(headless=True, device=device) self.sim = SimulationManager(config) cfg_dict = { @@ -125,6 +125,7 @@ def setup_simulation(self, sim_device): } self.robot: Robot = self.sim.add_robot(cfg=CobotMagicCfg.from_dict(cfg_dict)) + self.sim.prepare() @pytest.mark.parametrize("arm_name", ["left_arm", "right_arm"]) def test_ik(self, arm_name: str): diff --git a/tests/sim/motion/solvers/test_pink_solver.py b/tests/sim/motion/solvers/test_pink_solver.py index 134272c29..da21962d6 100644 --- a/tests/sim/motion/solvers/test_pink_solver.py +++ b/tests/sim/motion/solvers/test_pink_solver.py @@ -68,7 +68,7 @@ class BaseSolverTest: def setup_simulation(self, solver_type: str): # Set up simulation with specified device (CPU or CUDA) - config = SimulationManagerCfg(headless=True, sim_device="cpu") + config = SimulationManagerCfg(headless=True, device="cpu") self.sim = SimulationManager(config) # Load robot URDF file @@ -98,6 +98,7 @@ def setup_simulation(self, solver_type: str): } self.robot: Robot = self.sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) + self.sim.prepare() def test_differential_solver(self): # Test differential solver with a 1x4x4 homogeneous matrix pose and a joint_seed diff --git a/tests/sim/motion/solvers/test_pinocchio_solver.py b/tests/sim/motion/solvers/test_pinocchio_solver.py index b6f4f9956..4b428545f 100644 --- a/tests/sim/motion/solvers/test_pinocchio_solver.py +++ b/tests/sim/motion/solvers/test_pinocchio_solver.py @@ -33,7 +33,7 @@ class BaseSolverTest: def setup_simulation(self, solver_type: str): # Set up simulation with specified device (CPU or CUDA) - config = SimulationManagerCfg(headless=True, sim_device="cpu") + config = SimulationManagerCfg(headless=True, device="cpu") self.sim = SimulationManager(config) # Load robot URDF file @@ -61,6 +61,7 @@ def setup_simulation(self, solver_type: str): } self.robot: Robot = self.sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) + self.sim.prepare() @pytest.mark.parametrize("arm_name", ["left_arm", "right_arm"]) def test_ik(self, arm_name: str): diff --git a/tests/sim/motion/solvers/test_pytorch_solver.py b/tests/sim/motion/solvers/test_pytorch_solver.py index be720e7ce..a2e742a84 100644 --- a/tests/sim/motion/solvers/test_pytorch_solver.py +++ b/tests/sim/motion/solvers/test_pytorch_solver.py @@ -73,7 +73,7 @@ class BaseSolverTest: def setup_simulation(self, solver_type: str): # Set up simulation with specified device (CPU or CUDA) - config = SimulationManagerCfg(headless=True, sim_device="cpu") + config = SimulationManagerCfg(headless=True, device="cpu") self.sim = SimulationManager(config) # Load robot URDF file @@ -104,6 +104,7 @@ def setup_simulation(self, solver_type: str): } self.robot: Robot = self.sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) + self.sim.prepare() # Wait for robot to stabilize. self.sim.update(step=100) diff --git a/tests/sim/motion/solvers/test_srs_solver.py b/tests/sim/motion/solvers/test_srs_solver.py index 60dc394fd..700cb3c80 100644 --- a/tests/sim/motion/solvers/test_srs_solver.py +++ b/tests/sim/motion/solvers/test_srs_solver.py @@ -401,7 +401,7 @@ class BaseRobotSolverTest: def setup_simulation(self, solver_type: str, device: str = "cpu"): # Set up simulation with specified device (CPU or CUDA) - config = SimulationManagerCfg(headless=True, sim_device=device) + config = SimulationManagerCfg(headless=True, device=device) self.sim = SimulationManager(config) # Load robot URDF file @@ -426,7 +426,7 @@ def setup_simulation(self, solver_type: str, device: str = "cpu"): "torso": ["ANKLE", "KNEE", "BUTTOCK", "WAIST"], "head": [f"NECK{i + 1}" for i in range(2)], }, - "drive_pros": { + "joint_drive_props": { "stiffness": { "LEFT_J[1-7]": 1e4, "RIGHT_J[1-7]": 1e4, @@ -453,14 +453,18 @@ def setup_simulation(self, solver_type: str, device: str = "cpu"): }, }, "attrs": { - "mass": 1e-1, - "static_friction": 0.95, - "dynamic_friction": 0.9, - "linear_damping": 0.7, - "angular_damping": 0.7, - "max_depenetration_velocity": 10.0, - "min_position_iters": 32, - "min_velocity_iters": 8, + "mass_props": {"mass": 1e-1}, + "rigid_props": { + "linear_damping": 0.7, + "angular_damping": 0.7, + "max_depenetration_velocity": 10.0, + "min_position_iters": 32, + "min_velocity_iters": 8, + }, + "material_props": { + "static_friction": 0.95, + "dynamic_friction": 0.9, + }, }, "solver_cfg": { "left_arm": { @@ -489,6 +493,7 @@ def setup_simulation(self, solver_type: str, device: str = "cpu"): } self.robot: Robot = self.sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) + self.sim.prepare() # Wait for robot to stabilize. self.sim.update(step=100) diff --git a/tests/sim/motion/solvers/test_ur_solver.py b/tests/sim/motion/solvers/test_ur_solver.py index 2d138c8d0..8572189ff 100644 --- a/tests/sim/motion/solvers/test_ur_solver.py +++ b/tests/sim/motion/solvers/test_ur_solver.py @@ -29,7 +29,6 @@ JointDrivePropertiesCfg, RobotCfg, LightCfg, - RigidBodyAttributesCfg, RigidObjectCfg, URDFCfg, ) @@ -79,8 +78,8 @@ def grid_sample_qpos_from_limits( class BaseSolverTest: sim = None # Define as a class attribute - def setup_simulation(self, sim_device): - config = SimulationManagerCfg(headless=True, sim_device=sim_device) + def setup_simulation(self, device): + config = SimulationManagerCfg(headless=True, device=device) self.sim = SimulationManager(config) ur10_urdf_path = get_data_path("UniversalRobots/UR10/UR10.urdf") @@ -94,7 +93,7 @@ def setup_simulation(self, sim_device): {"component_type": "hand", "urdf_path": gripper_urdf_path}, ] ), - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( stiffness={"Joint[0-9]": 1e4, "FINGER[1-2]": 1e3}, damping={"Joint[0-9]": 1e3, "FINGER[1-2]": 1e2}, max_effort={"Joint[0-9]": 1e5, "FINGER[1-2]": 1e4}, @@ -128,6 +127,7 @@ def setup_simulation(self, sim_device): init_pos=(0, 0, 0), ) self.robot: Robot = self.sim.add_robot(cfg=cfg) + self.sim.prepare() def test_ik(self): # Test inverse kinematics (IK) with a 1x4x4 homogeneous matrix pose and a joint_seed diff --git a/tests/sim/motion/test_motion_generator.py b/tests/sim/motion/test_motion_generator.py index a68bdb9c3..efdcd145e 100644 --- a/tests/sim/motion/test_motion_generator.py +++ b/tests/sim/motion/test_motion_generator.py @@ -54,7 +54,7 @@ def setup_simulation(self): cls = type(self) if hasattr(cls, "robot_sim"): return - cls.config = SimulationManagerCfg(headless=True, sim_device="cpu") + cls.config = SimulationManagerCfg(headless=True, device="cpu") cls.robot_sim = SimulationManager(cls.config) cfg_dict = { @@ -97,6 +97,7 @@ def setup_simulation(self): cls.robot: Robot = cls.robot_sim.add_robot( cfg=CobotMagicCfg.from_dict(cfg_dict) ) + cls.robot_sim.prepare() cls.arm_name = "left_arm" diff --git a/tests/sim/motion/workspace/test_analyzer.py b/tests/sim/motion/workspace/test_analyzer.py index f5dace3fc..2e1f1e3f6 100644 --- a/tests/sim/motion/workspace/test_analyzer.py +++ b/tests/sim/motion/workspace/test_analyzer.py @@ -35,7 +35,7 @@ class BaseWorkspaceAnalyzeTest: sim = None # Define as a class attribute def setup_simulation(self): - config = SimulationManagerCfg(headless=True, sim_device="cpu") + config = SimulationManagerCfg(headless=True, device="cpu") self.sim = SimulationManager(config) cfg_dict = { @@ -76,6 +76,7 @@ def setup_simulation(self): } self.robot: Robot = self.sim.add_robot(cfg=CobotMagicCfg.from_dict(cfg_dict)) + self.sim.prepare() def teardown_method(self): """Clean up resources after each test method.""" diff --git a/tests/sim/motion/workspace/test_cache.py b/tests/sim/motion/workspace/test_cache.py index 8a6d636ee..9ff8416ea 100644 --- a/tests/sim/motion/workspace/test_cache.py +++ b/tests/sim/motion/workspace/test_cache.py @@ -508,7 +508,7 @@ def _robot_ns(**overrides) -> argparse.Namespace: init_pos=[0.0, 0.0, 0.0], init_rot=[0.0, 0.0, 0.0], fix_base=True, - use_usd_properties=False, + asset_physics_mode="overlay", ) defaults.update(overrides) return argparse.Namespace(**defaults) @@ -551,6 +551,7 @@ def test_build_robot_cfg_urdf_defaults_solver_urdf(): assert cfg.control_parts == {"arm": ["fr3_joint[1-7]"]} assert cfg.solver_cfg["arm"].end_link_name == "fr3_hand_tcp" assert cfg.solver_cfg["arm"].urdf_path == "/tmp/panda.urdf" + assert cfg.asset_physics_mode == "overlay" def test_build_robot_cfg_usd_requires_urdf(): @@ -573,6 +574,15 @@ def test_build_robot_cfg_usd_with_urdf(): assert cfg.solver_cfg["arm"].urdf_path == "/tmp/robot.urdf" +def test_build_robot_cfg_accepts_source_independent_preserve_mode(): + """The asset physics policy applies to either USD or URDF sources.""" + from embodichain.lab.scripts.analyze_workspace import build_robot_cfg + + cfg, _part, _urdf = build_robot_cfg(_robot_ns(asset_physics_mode="preserve")) + + assert cfg.asset_physics_mode == "preserve" + + def test_build_robot_cfg_asset_requires_ee_link(): """--asset without --ee-link raises a clear error.""" from embodichain.lab.scripts.analyze_workspace import build_robot_cfg @@ -721,6 +731,7 @@ def _make_cobotmagic_sim(tmp_path): }, } robot = sim.add_robot(cfg=CobotMagicCfg.from_dict(cfg_dict)) + sim.prepare() return sim, robot diff --git a/tests/sim/motion/workspace/test_sim_utils.py b/tests/sim/motion/workspace/test_sim_utils.py new file mode 100644 index 000000000..9d266993f --- /dev/null +++ b/tests/sim/motion/workspace/test_sim_utils.py @@ -0,0 +1,101 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest + +from embodichain.lab.sim.cfg import RigidObjectCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg +from embodichain.lab.sim.utility.sim_utils import _load_rigid_mesh_prototype + + +class _FakeActor: + def add_rigidbody(self, *args, **kwargs) -> None: + pass + + +class _FakeArena: + def __init__(self) -> None: + self.load_actor_called = False + self.acd_method: str | None = None + + def load_actor(self, *args, **kwargs) -> _FakeActor: + self.load_actor_called = True + return _FakeActor() + + def load_actor_with_acd(self, *args, method: str, **kwargs) -> _FakeActor: + self.acd_method = method + return _FakeActor() + + +def test_load_rigid_mesh_uses_shape_collision_defaults() -> None: + arena = _FakeArena() + cfg = RigidObjectCfg(uid="mesh", shape=MeshCfg(fpath="mesh.obj")) + + _load_rigid_mesh_prototype( + arena, + cfg, + cache_dir=None, + body_type=None, + is_newton_backend=False, + ) + + assert arena.load_actor_called + + +def test_load_rigid_mesh_forwards_shape_acd_method() -> None: + arena = _FakeArena() + cfg = RigidObjectCfg( + uid="mesh", + shape=MeshCfg( + fpath="mesh.obj", + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=2, + acd_method="vhacd", + ), + ), + ) + + _load_rigid_mesh_prototype( + arena, + cfg, + cache_dir=None, + body_type=None, + is_newton_backend=False, + ) + + assert arena.acd_method == "vhacd" + + +def test_load_rigid_mesh_rejects_dynamic_triangle_mesh_collision() -> None: + cfg = RigidObjectCfg( + uid="mesh", + shape=MeshCfg( + fpath="mesh.obj", + collision=MeshCollisionCfg(approximation="triangle_mesh"), + ), + ) + + with pytest.raises(ValueError, match="only for static"): + _load_rigid_mesh_prototype( + _FakeArena(), + cfg, + cache_dir=None, + body_type=None, + is_newton_backend=False, + ) diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index 0471f8119..545072d56 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -20,6 +20,7 @@ from types import SimpleNamespace from unittest.mock import MagicMock +import numpy as np import pytest import torch @@ -33,15 +34,24 @@ ArticulationCfg, JointDrivePropertiesCfg, LinkPhysicsOverrideCfg, - RigidBodyAttributesCfg, - RigidBodyAttributesOverrideCfg, + MassPropertiesCfg, + physics_cfg_for_backend, + RigidBodyPhysicsCfg, ) -from embodichain.lab.sim.utility.sim_utils import _resolve_link_physics_groups from embodichain.data import get_data_path from dexsim.types import ActorType, DriveType ART_PATH = "SlidingBoxDrawer/SlidingBoxDrawer.urdf" NUM_ARENAS = 10 +NEWTON_EFFORT_TARGET_MODE = 4 +DRIVE_TEST_STIFFNESS = 12.0 +DRIVE_TEST_DAMPING = 4.0 + + +def _teardown_newton_physics() -> None: + from dexsim.engine.newton_physics import teardown_newton_physics + + teardown_newton_physics() class _GravityEntity: @@ -184,6 +194,48 @@ def test_get_parent_joint_chain_returns_backend_neutral_child_to_root_values(): assert chain[0].origin_pose[0, 3].item() == 0.0 +@pytest.mark.no_sim +def test_get_parent_joint_chain_uses_newton_joint_descriptors_when_needed(): + articulation = object.__new__(Articulation) + articulation._data = SimpleNamespace( + link_names=["body", "door", "door_handle"], + is_newton_backend=True, + ) + fixed = SimpleNamespace( + name="handle_fixed", + joint_type=SimpleNamespace(name="FIXED"), + parent_link_name="door", + child_link_name="door_handle", + origin_pose=np.eye(4, dtype=np.float32), + axis=np.zeros(3, dtype=np.float32), + lower_limit=np.asarray([0.0], dtype=np.float32), + upper_limit=np.asarray([0.0], dtype=np.float32), + ) + hinge = SimpleNamespace( + name="door_hinge", + joint_type=SimpleNamespace(name="REVOLUTE"), + parent_link_name="body", + child_link_name="door", + origin_pose=np.eye(4, dtype=np.float32), + axis=np.asarray([0.0, 0.0, 1.0], dtype=np.float32), + lower_limit=np.asarray([0.0], dtype=np.float32), + upper_limit=np.asarray([2.0], dtype=np.float32), + ) + joint_descs = {fixed.name: fixed, hinge.name: hinge} + entity = SimpleNamespace( + get_joint_names=lambda: [fixed.name, hinge.name], + get_joint_info=lambda _: None, + get_joint_desc=joint_descs.__getitem__, + ) + articulation._entities = [entity] + + chain = articulation.get_parent_joint_chain("door_handle") + + assert [joint.name for joint in chain] == ["handle_fixed", "door_hinge"] + assert [joint.joint_type for joint in chain] == ["fixed", "revolute"] + assert chain[1].joint_limits == (0.0, 2.0) + + def _make_render_node_articulation( asset_type: type[Articulation] = Articulation, num_envs: int = 2 ) -> tuple[Articulation, list[object]]: @@ -244,7 +296,9 @@ def test_get_link_render_nodes_rejects_incomplete_arena_topology(failure: str) - def _link_static_friction(art: Articulation, link_name: str, env_idx: int = 0) -> float: - return art._entities[env_idx].get_physical_attr(link_name).static_friction + return art.get_link_physical_attr(link_names=[link_name], env_ids=[env_idx])[ + 0 + ].static_friction class _EntityMethodOverride: @@ -261,56 +315,61 @@ def __getattr__(self, name: str): return getattr(self._entity, name) -class TestRigidBodyAttributesOverride: +class TestLinkPhysicsOverrideCfg: """Pure-Python tests for per-link physics config merging.""" - def test_merge_with_applies_only_set_fields(self): - base = RigidBodyAttributesCfg( - static_friction=0.3, - dynamic_friction=0.25, - linear_damping=0.5, + def test_grouped_override_applies_only_configured_fields(self): + base = RigidBodyPhysicsCfg.from_dict( + { + "rigid_props": {"linear_damping": 0.5}, + "material_props": { + "static_friction": 0.3, + "dynamic_friction": 0.25, + }, + } + ) + override = RigidBodyPhysicsCfg.from_dict( + {"material_props": {"static_friction": 0.85}} ) - override = RigidBodyAttributesOverrideCfg(static_friction=0.85) - merged = override.merge_with(base) + merged = override.to_dexsim_physical_attr(base=base.to_dexsim_physical_attr()) assert abs(merged.static_friction - 0.85) < 1e-6 assert abs(merged.dynamic_friction - 0.25) < 1e-6 assert abs(merged.linear_damping - 0.5) < 1e-6 - def test_resolve_link_physics_overlap_raises(self): - link_names = ["outer_box", "handle_xpos", "inner_drawer"] - link_attrs = { - "box": LinkPhysicsOverrideCfg( - link_names_expr=["outer_box", "handle_xpos"], - attrs=RigidBodyAttributesOverrideCfg(static_friction=0.9), - ), - "handle": LinkPhysicsOverrideCfg( - link_names_expr=["handle_xpos"], - attrs=RigidBodyAttributesOverrideCfg(static_friction=0.8), - ), - } - with pytest.raises(ValueError, match="multiple link_attrs groups"): - _resolve_link_physics_groups(link_names, link_attrs) - class BaseArticulationTest: """Shared test logic for CPU and CUDA.""" - def setup_simulation(self, sim_device): + def setup_simulation(self, device, physics: str = "default"): + physics_cfg = physics_cfg_for_backend(physics) + if physics == "newton": + physics_cfg.solver_cfg = { + "solver_type": "mujoco_warp", + "njmax": 8192, + "nconmax": 8192, + } config = SimulationManagerCfg( - headless=True, sim_device=sim_device, num_envs=NUM_ARENAS + headless=True, + device=device, + num_envs=NUM_ARENAS, + physics_cfg=physics_cfg, ) self.sim = SimulationManager(config) + self.physics = physics art_path = get_data_path(ART_PATH) assert os.path.isfile(art_path) - cfg_dict = {"fpath": art_path, "drive_pros": {"drive_type": "force"}} + cfg_dict = { + "fpath": art_path, + "asset_physics_mode": "overlay", + "joint_drive_props": {"drive_type": "force"}, + } self.art: Articulation = self.sim.add_articulation( cfg=ArticulationCfg.from_dict(cfg_dict) ) - if sim_device == "cuda" and getattr(self.sim, "is_use_gpu_physics", False): - self.sim.init_gpu_physics() + self.sim.prepare() def test_local_pose_behavior(self): """Test set_local_pose and get_local_pose: @@ -318,21 +377,148 @@ def test_local_pose_behavior(self): """ # Set initial poses - pose = torch.eye(4, device=self.sim.device) - pose[2, 3] = 1.0 - pose = pose.unsqueeze(0).repeat(NUM_ARENAS, 1, 1) + distinct_xyzw = torch.tensor( + [1.0, 2.0, 3.0, 4.0], device=self.sim.device + ) / torch.sqrt(torch.tensor(30.0, device=self.sim.device)) + pose = torch.zeros(NUM_ARENAS, 7, device=self.sim.device) + pose[:, 2] = 1.0 + pose[:, 3:7] = distinct_xyzw self.art.set_local_pose(pose, env_ids=None) # --- Check poses immediately after setting - xyz = self.art.get_local_pose()[0, :3] - + actual_pose = self.art.get_local_pose() + xyz = actual_pose[0, :3] expected_pos = torch.tensor( [0.0, 0.0, 1.0], device=self.sim.device, dtype=torch.float32 ) assert torch.allclose( xyz, expected_pos, atol=1e-5 ), f"FAIL: Drawer pose not set correctly: {xyz.tolist()}" + torch.testing.assert_close( + actual_pose[:, 3:7], + distinct_xyzw.unsqueeze(0).expand(NUM_ARENAS, -1), + atol=1e-5, + rtol=1e-5, + ) + + def test_replicated_link_shapes_are_isolated_by_environment(self): + """Every articulation link shape should use its environment group.""" + for env_index, entity in enumerate(self.art._entities): + if self.physics == "newton": + shape_ids = [ + shape_id + for link in entity.physics_articulation.links + for shape_id in link.shape_ids + ] + assert shape_ids + groups = ( + entity.physics_articulation.runtime.model.shape_collision_group.numpy() + ) + assert {int(groups[shape_id]) for shape_id in shape_ids} == { + env_index + 1 + } + continue + + expected = np.asarray([env_index, 1, 0, 0], dtype=np.uint32) + physical_links = [ + link + for link in entity.articulation_desc.links + if link.rigid_body is not None + ] + assert physical_links + for link in physical_links: + np.testing.assert_array_equal( + link.rigid_body.collision_filter_data, + expected, + ) + + def test_body_data_exposes_link_mass_properties(self): + """Current and initialization-time link mass properties share one layout.""" + data = self.art.body_data + + assert data.mass.shape == (NUM_ARENAS, self.art.num_links) + assert data.inertia.shape == (NUM_ARENAS, self.art.num_links, 3) + assert data.com_pose.shape == (NUM_ARENAS, self.art.num_links, 7) + assert data.default_mass.shape == data.mass.shape + assert data.default_inertia.shape == data.inertia.shape + assert data.default_com_pose.shape == data.com_pose.shape + assert torch.allclose(self.art.default_link_masses, data.default_mass) + + def test_reset_restores_default_link_mass_properties(self): + """Partial reset restores mass, inertia, and COM only for selected rows.""" + data = self.art.body_data + link_name = self.art.link_names[0] + link_id = self.art.link_names.index(link_name) + env_ids = [0, 1] + default_mass = data.default_mass[env_ids, link_id : link_id + 1].clone() + default_inertia = data.default_inertia[env_ids, link_id : link_id + 1].clone() + default_com_pose = data.default_com_pose[env_ids, link_id : link_id + 1].clone() + changed_mass = default_mass + 0.5 + changed_inertia = default_inertia * 1.25 + changed_com_pose = default_com_pose.clone() + changed_com_pose[..., 0] += 0.02 + changed_com_pose[..., 3:7] = torch.tensor( + [1.0, 2.0, 3.0, 4.0], device=self.sim.device + ) / torch.sqrt(torch.tensor(30.0, device=self.sim.device)) + + self.art.set_mass(changed_mass, link_names=[link_name], env_ids=env_ids) + self.art.set_inertia( + changed_inertia, + link_names=[link_name], + env_ids=env_ids, + ) + self.art.set_com_pose( + changed_com_pose, + link_names=[link_name], + env_ids=env_ids, + ) + self.sim.prepare() + + assert torch.allclose( + data.default_mass[env_ids, link_id : link_id + 1], default_mass + ) + assert torch.allclose( + data.default_inertia[env_ids, link_id : link_id + 1], default_inertia + ) + assert torch.allclose( + data.default_com_pose[env_ids, link_id : link_id + 1], default_com_pose + ) + + self.art.reset(env_ids=[env_ids[0]]) + self.sim.prepare() + mass_after_partial = self.art.get_mass(link_names=[link_name], env_ids=env_ids) + inertia_after_partial = self.art.get_inertia( + link_names=[link_name], env_ids=env_ids + ) + com_after_partial = self.art.get_com_pose( + link_names=[link_name], env_ids=env_ids + ) + + assert torch.allclose(mass_after_partial[0], default_mass[0], atol=1e-5) + assert torch.allclose(mass_after_partial[1], changed_mass[1], atol=1e-5) + assert torch.allclose(inertia_after_partial[0], default_inertia[0], atol=1e-5) + assert torch.allclose(inertia_after_partial[1], changed_inertia[1], atol=1e-5) + assert torch.allclose(com_after_partial[0], default_com_pose[0], atol=1e-5) + assert torch.allclose(com_after_partial[1], changed_com_pose[1], atol=1e-5) + + self.art.reset(env_ids=[env_ids[1]]) + self.sim.prepare() + assert torch.allclose( + self.art.get_mass(link_names=[link_name], env_ids=env_ids), + default_mass, + atol=1e-5, + ) + assert torch.allclose( + self.art.get_inertia(link_names=[link_name], env_ids=env_ids), + default_inertia, + atol=1e-5, + ) + assert torch.allclose( + self.art.get_com_pose(link_names=[link_name], env_ids=env_ids), + default_com_pose, + atol=1e-5, + ) def test_control_api(self): """Test control API for setting and getting joint positions.""" @@ -515,12 +701,14 @@ def test_get_joint_drive_with_joint_ids(self): armature, expected_armature, atol=1e-5 ), "FAIL: armature does not match expected filtered values" - def test_default_drive_type_is_none_after_construction(self): - """A default ArticulationCfg creates passive backend joint drives.""" + def test_explicit_passive_drive_after_construction(self): + """An explicit passive overlay disables backend joint drives.""" passive_articulation = self.sim.add_articulation( cfg=ArticulationCfg( uid="passive_drawer", fpath=get_data_path(ART_PATH), + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(drive_type="none"), ) ) @@ -529,6 +717,50 @@ def test_default_drive_type_is_none_after_construction(self): ] assert passive_articulation.get_joint_drive_type() == expected_drive_types + if self.sim.is_newton_backend: + expected_target_modes = [ + [0] * passive_articulation.dof for _ in range(NUM_ARENAS) + ] + assert passive_articulation.get_joint_target_mode() == expected_target_modes + + def test_preserve_mode_ignores_urdf_physics_overrides(self): + """Preserve mode keeps source-resolved URDF link and joint physics.""" + source = self.sim.add_articulation( + cfg=ArticulationCfg( + uid="source_drawer", + fpath=get_data_path(ART_PATH), + asset_physics_mode="preserve", + init_pos=(-1.0, 0.0, 0.0), + ) + ) + preserved = self.sim.add_articulation( + cfg=ArticulationCfg( + uid="preserved_drawer", + fpath=get_data_path(ART_PATH), + asset_physics_mode="preserve", + init_pos=(1.0, 0.0, 0.0), + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=123.0)), + joint_drive_props=JointDrivePropertiesCfg( + drive_type="none", + stiffness=987.0, + damping=654.0, + max_effort=321.0, + max_velocity=123.0, + ), + qpos_limits={".*": [-0.01, 0.01]}, + ) + ) + + assert torch.allclose(preserved.body_data.mass, source.body_data.mass) + assert torch.allclose( + preserved.body_data.qpos_limits, + source.body_data.qpos_limits, + ) + for preserved_value, source_value in zip( + preserved.get_joint_drive(), source.get_joint_drive() + ): + assert torch.allclose(preserved_value, source_value) + def test_joint_limit_getters_support_env_and_joint_filters(self): """Test joint limit getters support joint_ids and env_ids filtering.""" all_qpos_limits = self.art.body_data.qpos_limits @@ -955,7 +1187,8 @@ def test_qpos_limits_from_cfg_dict_can_tighten(self): cfg = ArticulationCfg( uid="drawer_cfg_qpos_limits", fpath=get_data_path(ART_PATH), - drive_pros=JointDrivePropertiesCfg(drive_type="force"), + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(drive_type="force"), qpos_limits={".*": [-0.05, 0.05]}, ) art: Articulation = self.sim.add_articulation(cfg=cfg) @@ -979,7 +1212,8 @@ def test_qpos_limits_from_cfg_can_expand(self): cfg = ArticulationCfg( uid="drawer_expanded_limits", fpath=get_data_path(ART_PATH), - drive_pros=JointDrivePropertiesCfg(drive_type="force"), + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(drive_type="force"), qpos_limits={joint_name: [expanded_lower, expanded_upper]}, ) art: Articulation = self.sim.add_articulation(cfg=cfg) @@ -1021,8 +1255,8 @@ def teardown_method(self): class BaseArticulationLinkPhysicsTest: """Tests for per-link physics configuration (isolated sim per test).""" - def setup_simulation(self, sim_device: str) -> None: - config = SimulationManagerCfg(headless=True, sim_device=sim_device, num_envs=2) + def setup_simulation(self, device: str) -> None: + config = SimulationManagerCfg(headless=True, device=device, num_envs=2) self.sim = SimulationManager(config) self.art_path = get_data_path(ART_PATH) assert os.path.isfile(self.art_path) @@ -1043,10 +1277,14 @@ def test_global_attrs_applied_to_all_links(self): cfg = ArticulationCfg( uid="drawer_global_attrs", fpath=self.art_path, - drive_pros=JointDrivePropertiesCfg(drive_type="force"), - attrs=RigidBodyAttributesCfg(static_friction=global_friction), + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(drive_type="force"), + attrs=RigidBodyPhysicsCfg.from_dict( + {"material_props": {"static_friction": global_friction}} + ), ) art: Articulation = self.sim.add_articulation(cfg=cfg) + self.sim.prepare() for link_name in art.link_names: assert abs(_link_static_friction(art, link_name) - global_friction) < 1e-3 @@ -1057,18 +1295,22 @@ def test_link_attrs_override_selected_links(self): cfg = ArticulationCfg( uid="drawer_link_attrs", fpath=self.art_path, - drive_pros=JointDrivePropertiesCfg(drive_type="force"), - attrs=RigidBodyAttributesCfg(static_friction=global_friction), + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(drive_type="force"), + attrs=RigidBodyPhysicsCfg.from_dict( + {"material_props": {"static_friction": global_friction}} + ), link_attrs={ "handle": LinkPhysicsOverrideCfg( link_names_expr=["handle_xpos"], - attrs=RigidBodyAttributesOverrideCfg( - static_friction=handle_friction + attrs=RigidBodyPhysicsCfg.from_dict( + {"material_props": {"static_friction": handle_friction}} ), ), }, ) art: Articulation = self.sim.add_articulation(cfg=cfg) + self.sim.prepare() assert abs(_link_static_friction(art, "handle_xpos") - handle_friction) < 1e-3 for link_name in art.link_names: if link_name == "handle_xpos": @@ -1081,17 +1323,19 @@ def test_link_attrs_from_dict(self): { "uid": "drawer_link_attrs_dict", "fpath": self.art_path, - "drive_pros": {"drive_type": "force"}, - "attrs": {"static_friction": 0.4}, + "asset_physics_mode": "overlay", + "joint_drive_props": {"drive_type": "force"}, + "attrs": {"material_props": {"static_friction": 0.4}}, "link_attrs": { "handle": { "link_names_expr": ["handle_xpos"], - "attrs": {"static_friction": 0.77}, + "attrs": {"material_props": {"static_friction": 0.77}}, } }, } ) art: Articulation = self.sim.add_articulation(cfg=cfg) + self.sim.prepare() assert abs(_link_static_friction(art, "handle_xpos") - 0.77) < 1e-3 assert abs(_link_static_friction(art, "outer_box") - 0.4) < 1e-3 @@ -1100,19 +1344,31 @@ def test_set_link_physical_attr_runtime(self): cfg = ArticulationCfg( uid="drawer_runtime_attrs", fpath=self.art_path, - drive_pros=JointDrivePropertiesCfg(drive_type="force"), + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(drive_type="force"), ) art: Articulation = self.sim.add_articulation(cfg=cfg) + self.sim.prepare() + source_friction = { + link_name: _link_static_friction(art, link_name) + for link_name in art.link_names + } handle_friction = 0.66 art.set_link_physical_attr( - RigidBodyAttributesOverrideCfg(static_friction=handle_friction), + RigidBodyPhysicsCfg.from_dict( + {"material_props": {"static_friction": handle_friction}} + ), link_names=["handle_xpos"], ) + self.sim.prepare() assert abs(_link_static_friction(art, "handle_xpos") - handle_friction) < 1e-3 for link_name in art.link_names: if link_name == "handle_xpos": continue - assert abs(_link_static_friction(art, link_name) - 0.5) < 1e-3 + assert ( + abs(_link_static_friction(art, link_name) - source_friction[link_name]) + < 1e-3 + ) class TestArticulationLinkPhysicsCPU(BaseArticulationLinkPhysicsTest): @@ -1135,6 +1391,117 @@ def setup_method(self): self.setup_simulation("cuda") +class TestArticulationNewton(BaseArticulationTest): + """Articulation coverage on the DexSim Newton physics backend.""" + + def setup_method(self): + self.setup_simulation("cuda", physics="newton") + + def teardown_method(self): + self.sim.destroy() + import embodichain.lab.sim as om + + om.SimulationManager.flush_cleanup_queue() + _teardown_newton_physics() + import gc + + gc.collect() + + def test_control_api(self): + """Newton articulation direct state and control buffers round-trip.""" + qpos_zero = torch.zeros( + (NUM_ARENAS, self.art.dof), dtype=torch.float32, device=self.sim.device + ) + qpos = qpos_zero.clone() + qpos[:, -1] = 0.1 + + self.art.set_qpos(qpos, env_ids=None, target=False) + assert torch.allclose(self.art.body_data.qpos, qpos, atol=1e-5) + + self.art.set_qpos(qpos_zero, env_ids=None, target=False) + self.art.set_qpos(qpos, env_ids=None, target=True) + assert torch.allclose(self.art.body_data.target_qpos, qpos, atol=1e-5) + + qvel = torch.full( + (NUM_ARENAS, self.art.dof), + 0.2, + dtype=torch.float32, + device=self.sim.device, + ) + self.art.set_qvel(qvel, env_ids=None, target=False) + assert torch.allclose(self.art.body_data.qvel, qvel, atol=1e-5) + + qf = torch.ones( + (NUM_ARENAS, self.art.dof), dtype=torch.float32, device=self.sim.device + ) + self.art.set_qf(qf, env_ids=None) + assert torch.allclose(self.art.body_data.qf, qf, atol=1e-5) + + self.art.clear_dynamics() + assert torch.allclose(self.art.body_data.qvel, qpos_zero, atol=1e-5) + assert torch.allclose(self.art.body_data.qf, qpos_zero, atol=1e-5) + + @pytest.mark.gpu + def test_runtime_effort_drive_mode(self): + """Newton authors effort mode and removes effective PD gains.""" + shape = (NUM_ARENAS, self.art.dof) + self.art.set_joint_drive( + stiffness=torch.full( + shape, + DRIVE_TEST_STIFFNESS, + dtype=torch.float32, + device=self.sim.device, + ), + damping=torch.full( + shape, + DRIVE_TEST_DAMPING, + dtype=torch.float32, + device=self.sim.device, + ), + drive_type="force", + target_mode="effort", + ) + + assert self.art.get_joint_target_mode() == [ + [NEWTON_EFFORT_TARGET_MODE] * self.art.dof for _ in range(NUM_ARENAS) + ] + stiffness, damping, *_ = self.art.get_joint_drive() + assert torch.count_nonzero(stiffness) == 0 + assert torch.count_nonzero(damping) == 0 + + @pytest.mark.skip( + reason="DexSim Newton articulation visual-material helpers are render-Skeleton only." + ) + def test_set_visual_material(self): + super().test_set_visual_material() + + @pytest.mark.skip( + reason="DexSim Newton articulation physical-visible helpers are render-Skeleton only." + ) + def test_set_physical_visible(self): + super().test_set_physical_visible() + + def test_set_mass_rebuilds_mass_on_newton(self): + """A retained Newton per-link mass takes effect at prepare().""" + link_name = self.art.link_names[0] + original = self.art.get_mass(link_names=[link_name])[0, 0].item() + new_mass = original + 1.5 + self.art.set_mass( + torch.full( + (NUM_ARENAS, 1), + new_mass, + dtype=torch.float32, + device=self.sim.device, + ), + link_names=[link_name], + ) + self.sim.prepare() + live_mass = self.art.get_mass(link_names=[link_name])[0, 0].item() + assert ( + abs(live_mass - new_mass) < 1e-3 + ), f"per-link mass {new_mass} not applied after Newton rebuild (got {live_mass})" + + if __name__ == "__main__": test = TestArticulationCPU() test.setup_method() diff --git a/tests/sim/objects/test_articulation_drive_compat.py b/tests/sim/objects/test_articulation_drive_compat.py new file mode 100644 index 000000000..7bf03ca19 --- /dev/null +++ b/tests/sim/objects/test_articulation_drive_compat.py @@ -0,0 +1,90 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from types import SimpleNamespace + +import numpy as np +import pytest +import torch +from dexsim.types import DriveType + +from embodichain.lab.sim.objects.articulation import Articulation + +pytestmark = pytest.mark.no_sim + + +def test_newton_target_modes_map_to_portable_drive_types() -> None: + target_modes = np.asarray([0, 1, 2, 3, 4], dtype=np.int32) + entity = SimpleNamespace( + get_newton_drive=lambda: (None, None, None, None, None, None, target_modes) + ) + articulation = object.__new__(Articulation) + articulation._data = SimpleNamespace( + is_newton_backend=True, + dof=len(target_modes), + ) + articulation._all_indices = np.asarray([0], dtype=np.int32) + articulation._entities = [entity] + + assert articulation.get_joint_drive_type() == [ + [ + DriveType.NONE, + DriveType.FORCE, + DriveType.FORCE, + DriveType.FORCE, + DriveType.NONE, + ] + ] + + +def test_newton_drive_type_query_honors_joint_selection() -> None: + target_modes = np.asarray([0, 3, 0], dtype=np.int32) + entity = SimpleNamespace( + get_newton_drive=lambda: (None, None, None, None, None, None, target_modes) + ) + articulation = object.__new__(Articulation) + articulation._data = SimpleNamespace(is_newton_backend=True, dof=3) + articulation._all_indices = np.asarray([0], dtype=np.int32) + articulation._entities = [entity] + + assert articulation.get_joint_drive_type(joint_ids=[2, 1]) == [ + [DriveType.NONE, DriveType.FORCE] + ] + + +def test_runtime_effort_mode_disables_pd_gains_on_newton() -> None: + calls: list[dict[str, object]] = [] + entity = SimpleNamespace(set_newton_drive=lambda **kwargs: calls.append(kwargs)) + articulation = object.__new__(Articulation) + articulation._spawn_result = object() + articulation._entities = [entity] + articulation._all_indices = np.asarray([0], dtype=np.int32) + articulation._data = SimpleNamespace(is_newton_backend=True, dof=1) + articulation.device = torch.device("cpu") + + articulation.set_joint_drive( + stiffness=torch.tensor([[12.0]]), + damping=torch.tensor([[4.0]]), + drive_type="force", + target_mode="effort", + ) + + assert len(calls) == 1 + assert calls[0]["target_mode"] == 4 + assert calls[0]["target_ke"] == 0.0 + assert calls[0]["target_kd"] == 0.0 diff --git a/tests/sim/objects/test_asset_material_initialization.py b/tests/sim/objects/test_asset_material_initialization.py index 6602e811b..c668a019f 100644 --- a/tests/sim/objects/test_asset_material_initialization.py +++ b/tests/sim/objects/test_asset_material_initialization.py @@ -24,11 +24,16 @@ from embodichain.lab.sim.material import VisualMaterialInst from embodichain.lab.sim.objects.articulation import Articulation -from embodichain.lab.sim.objects.cloth_object import ClothObject +from embodichain.lab.sim.objects.deformable import SurfaceDeformableObject from embodichain.lab.sim.objects.rigid_object import RigidObject -from embodichain.lab.sim.objects.soft_object import SoftObject - -_ASSET_TYPES = (RigidObject, Articulation, SoftObject, ClothObject) +from embodichain.lab.sim.objects.deformable import VolumeDeformableObject + +_ASSET_TYPES = ( + RigidObject, + Articulation, + VolumeDeformableObject, + SurfaceDeformableObject, +) _LINK_NAME = "link" @@ -59,6 +64,7 @@ def _make_asset(asset_type, materials): asset = asset_type.__new__(asset_type) asset._entities = [entity] + asset._spawn_result = None asset._all_indices = [0] asset.is_shared_visual_material = False asset.uid = asset_type.__name__ @@ -191,10 +197,14 @@ def test_asset_restores_only_changed_segments(asset_type): def test_asset_reset_restores_selected_environment_material(asset_type): asset = asset_type.__new__(asset_type) + asset._entities = [MagicMock(name="entity")] + asset._declared_num_instances = 1 + asset._spawn_result = MagicMock(name="spawn_result") asset._all_indices = [0] asset.device = torch.device("cpu") asset.cfg = SimpleNamespace( attrs=MagicMock(), + init_local_pose=None, init_pos=(0.0, 0.0, 0.0), init_rot=(0.0, 0.0, 0.0), init_qpos=(0.0,), @@ -203,9 +213,12 @@ def test_asset_reset_restores_selected_environment_material(asset_type): asset.set_local_pose = MagicMock() if asset_type is RigidObject: + asset._data = None asset.set_attrs = MagicMock() asset.clear_dynamics = MagicMock() elif asset_type is Articulation: + asset._data = MagicMock(is_newton_backend=True) + asset._restore_default_physical_properties = MagicMock() asset.set_qpos = MagicMock() asset.clear_dynamics = MagicMock() asset._world = MagicMock() diff --git a/tests/sim/objects/test_cloth_object.py b/tests/sim/objects/test_cloth_object.py index 8ddaecaa6..6ea82d92e 100644 --- a/tests/sim/objects/test_cloth_object.py +++ b/tests/sim/objects/test_cloth_object.py @@ -19,13 +19,23 @@ import os from dexsim.utility.path import get_resources_data_path from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.cfg import ClothPhysicalAttributesCfg +from embodichain.lab.sim.cfg import ( + SurfaceElementPropertiesCfg, + SurfaceDeformablePhysicsCfg, + NewtonPhysicsCfg, +) from embodichain.lab.sim.shapes import MeshCfg -from embodichain.lab.sim.objects import ClothObjectCfg, ClothObject +from embodichain.lab.sim.objects import ( + SurfaceDeformableObject, + SurfaceDeformableObjectCfg, + DeformableObject, + DeformableObjectData, +) import open3d as o3d import pytest import torch import tempfile +import warp as wp def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): @@ -44,7 +54,7 @@ def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): # Vectorized vertex positions using PyTorch x_lin = torch.linspace(-w / 2.0, w / 2.0, steps=nx + 1, dtype=torch.float64) y_lin = torch.linspace(-h / 2.0, h / 2.0, steps=ny + 1, dtype=torch.float64) - yy, xx = torch.meshgrid(y_lin, x_lin) # shapes: (ny+1, nx+1) + yy, xx = torch.meshgrid(y_lin, x_lin, indexing="ij") xx_flat = xx.reshape(-1) yy_flat = yy.reshape(-1) zz_flat = torch.full_like(xx_flat, 0, dtype=torch.float64) @@ -69,9 +79,12 @@ def setup_simulation(self): height=1080, headless=True, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device="cuda", + device="cuda", num_envs=4, arena_space=3.0, + physics_cfg=NewtonPhysicsCfg( + solver_cfg={"solver_type": "vbd"}, + ), ) # Create the simulation instance @@ -90,27 +103,27 @@ def setup_simulation(self): cloth_save_path = os.path.join(tempfile.gettempdir(), "cloth_mesh.ply") o3d.io.write_triangle_mesh(cloth_save_path, cloth_mesh) # add softbody to the scene - self.cloth: ClothObject = self.sim.add_cloth_object( - cfg=ClothObjectCfg( + self.cloth: SurfaceDeformableObject = self.sim.add_deformable_object( + cfg=SurfaceDeformableObjectCfg( uid="cloth", shape=MeshCfg(fpath=cloth_save_path), init_pos=[0.5, 0.0, 0.3], init_rot=[0, 0, 0], - physical_attr=ClothPhysicalAttributesCfg( - mass=0.01, - youngs=1e10, - poissons=0.4, - thickness=0.04, - bending_stiffness=0.01, - bending_damping=0.1, - dynamic_friction=0.95, - min_position_iters=30, + attrs=SurfaceDeformablePhysicsCfg( + density=1.0, + surface_props=SurfaceElementPropertiesCfg( + tri_ke=1.0e4, + tri_ka=1.0e4, + tri_kd=10.0, + edge_ke=100.0, + edge_kd=1.0, + ), ), ) ) + self.sim.prepare() def test_run_simulation(self): - self.sim.init_gpu_physics() for _ in range(100): self.sim.update(step=1) self.cloth.reset() @@ -118,29 +131,94 @@ def test_run_simulation(self): self.sim.update(step=1) def test_remove(self): - self.sim.remove_asset(self.cloth.uid) - assert ( - self.cloth.uid not in self.sim._soft_objects - ), "Cow UID still present after removal" + with pytest.raises(NotImplementedError, match="pending removal"): + self.sim.remove_asset(self.cloth.uid) + assert self.sim.get_deformable_object(self.cloth.uid) is self.cloth def test_get_current_vertex_positions(self): - vertex_positions = self.cloth.get_current_vertex_position() + vertex_positions = self.cloth.data.nodal_pos_w assert vertex_positions.shape == ( self.sim.num_envs, - self.cloth._data.n_vertices, + self.cloth.data.n_nodes, 3, ), "Vertex positions shape mismatch" def test_get_deformable_mesh_geometry(self): """Test current cloth vertices and matching surface triangles.""" - self.sim.init_gpu_physics() - vertices = self.cloth.get_current_vertex_position() - triangles = self.cloth.get_triangles(env_ids=[0]) + self.sim.prepare() + vertices = self.cloth.data.nodal_pos_w + triangles = self.cloth.get_surface_triangles(env_ids=[0]) assert vertices.ndim == 3 and vertices.shape[0] == self.sim.num_envs assert triangles.ndim == 3 and triangles.shape[0] == 1 assert int(triangles.max()) < vertices.shape[1] + def test_set_local_pose_updates_selected_particle_batch(self): + """Setting one instance pose writes only its packed simulation nodes.""" + before = self.cloth.data.nodal_pos_w + translation = torch.tensor([0.5, 0.0, 0.0], device=self.cloth.device) + pose = torch.eye( + 4, + dtype=torch.float32, + device=self.cloth.device, + ).unsqueeze(0) + pose[:, :3, 3] = ( + torch.as_tensor( + self.cloth.cfg.init_pos, + dtype=torch.float32, + device=self.cloth.device, + ) + + translation + ) + + self.cloth.set_local_pose(pose, env_ids=[0]) + after = self.cloth.data.nodal_pos_w + + torch.testing.assert_close(after[0], before[0] + translation) + torch.testing.assert_close(after[1:], before[1:]) + + def test_unified_deformable_contract(self): + self.sim.update(step=5) + assert isinstance(self.cloth, DeformableObject) + assert isinstance(self.cloth, SurfaceDeformableObject) + assert self.cloth.deformable_type == "surface" + assert self.sim.get_deformable_object("cloth") is self.cloth + assert self.sim.get_deformable_object_uid_list() == ["cloth"] + + assert type(self.cloth.data) is DeformableObjectData + positions = self.cloth.data.nodal_pos_w + velocities = self.cloth.data.nodal_vel_w + state = self.cloth.data.nodal_state_w + default_state = self.cloth.data.default_nodal_state_w + assert positions.shape[-1] == 3 + assert velocities.shape == positions.shape + assert state.shape == (*positions.shape[:-1], 6) + assert default_state.shape == state.shape + native_velocities = torch.stack( + [ + wp.to_torch(particle_set.get_particle_velocities()).clone() + for particle_set in self.cloth.data.entities + ] + ) + assert torch.count_nonzero(native_velocities) > 0 + torch.testing.assert_close(velocities, native_velocities) + render_vertices = self.cloth.get_surface_vertices() + assert render_vertices.shape[0] == self.sim.num_envs + arena_offsets = torch.as_tensor( + self.sim.arena_offsets, + dtype=render_vertices.dtype, + device=render_vertices.device, + ) + arena_local_vertices = render_vertices - arena_offsets[:, None, :] + torch.testing.assert_close( + arena_local_vertices, + arena_local_vertices[0].expand_as(arena_local_vertices), + ) + torch.testing.assert_close( + self.cloth.get_surface_triangles(env_ids=[0]), + self.cloth.get_surface_triangles(env_ids=[0]), + ) + def teardown_method(self): """Clean up resources after each test method.""" self.sim.destroy() diff --git a/tests/sim/objects/test_deformable_object.py b/tests/sim/objects/test_deformable_object.py new file mode 100644 index 000000000..6ea1a7c44 --- /dev/null +++ b/tests/sim/objects/test_deformable_object.py @@ -0,0 +1,328 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Contract tests for the unified deformable-object API.""" + +from __future__ import annotations + +import pickle +from types import SimpleNamespace +from typing import Any, Sequence +from unittest.mock import Mock + +from dexsim.scene import Scene + +import numpy as np +import pytest +import torch + +from embodichain.lab.sim.cfg import ( + SurfaceDeformablePhysicsCfg, + SurfaceDeformableObjectCfg, + DeformableObjectCfg, + NewtonPhysicsCfg, + VolumeDeformableObjectCfg, + VolumeDeformablePhysicsCfg, +) +from embodichain.lab.sim.objects import ( + SurfaceDeformableObject, + DeformableObject, + DeformableObjectData, + VolumeDeformableObject, +) +from embodichain.lab.sim.physics import DefaultPhysicsBackend, NewtonPhysicsBackend +from embodichain.lab.sim.sim_manager import SimulationManager +from embodichain.utils.configclass import update_class_from_dict + +pytestmark = pytest.mark.no_sim + + +class _ParticleSet: + def __init__(self, offset: float) -> None: + self.positions = torch.tensor( + [[offset, 0.0, 0.0], [offset + 1.0, 0.0, 0.0]], + dtype=torch.float32, + ) + self.velocities = torch.zeros_like(self.positions) + + @property + def particle_count(self) -> int: + return len(self.positions) + + +class _ParticleBatch: + def __init__(self, particle_sets: Sequence[_ParticleSet]) -> None: + self.particle_sets = list(particle_sets) + + def fetch_particle_positions(self, out: torch.Tensor) -> int: + out.copy_(torch.cat([item.positions for item in self.particle_sets])) + return len(self.particle_sets) + + def fetch_particle_velocities(self, out: torch.Tensor) -> int: + out.copy_(torch.cat([item.velocities for item in self.particle_sets])) + return len(self.particle_sets) + + def apply_particle_positions(self, data: torch.Tensor) -> int: + for index, particle_set in enumerate(self.particle_sets): + start = index * particle_set.particle_count + end = start + particle_set.particle_count + particle_set.positions.copy_(data[start:end]) + return len(self.particle_sets) + + def apply_particle_velocities(self, data: torch.Tensor) -> int: + for index, particle_set in enumerate(self.particle_sets): + start = index * particle_set.particle_count + end = start + particle_set.particle_count + particle_set.velocities.copy_(data[start:end]) + return len(self.particle_sets) + + +class _ParticleScene: + def create_particle_set_batch( + self, particle_sets: Sequence[_ParticleSet] + ) -> _ParticleBatch: + return _ParticleBatch(particle_sets) + + +class _RenderParticleSet: + def __init__(self, vertex_count: int) -> None: + self.vertices = np.zeros((vertex_count, 3), dtype=np.float32) + + def get_render_vertices(self) -> np.ndarray: + return self.vertices + + def get_render_triangles(self) -> np.ndarray: + return np.empty((0, 3), dtype=np.int32) + + +@pytest.mark.parametrize( + "config_type", [VolumeDeformableObjectCfg, SurfaceDeformableObjectCfg] +) +def test_config_serialization_preserves_nested_values(config_type) -> None: + cfg = config_type(uid="deformable", particle_radius=0.02) + cfg.attrs.density = 12.0 + restored = config_type(uid="restored", particle_radius=0.01) + update_class_from_dict(restored, cfg.to_dict()) + + assert restored.to_dict() == cfg.to_dict() + assert pickle.loads(pickle.dumps(cfg)).to_dict() == cfg.to_dict() + copied = cfg.copy() + copied.attrs.density = 24.0 + assert cfg.attrs.density == 12.0 + assert config_type().attrs.density != 12.0 + + +def test_default_only_deformable_fields_are_not_accepted() -> None: + with pytest.raises(TypeError, match="dynamic_friction"): + VolumeDeformablePhysicsCfg(dynamic_friction=0.1) + with pytest.raises(TypeError, match="thickness"): + SurfaceDeformablePhysicsCfg(thickness=0.01) + + +def test_common_data_contract_combines_and_derives_nodal_state() -> None: + particles = _ParticleSet(0.0) + particles.positions[1] = torch.tensor([2.0, 4.0, 6.0]) + particles.velocities[:] = torch.tensor([[1.0, 2.0, 3.0], [3.0, 4.0, 5.0]]) + data = DeformableObjectData([particles], _ParticleScene(), torch.device("cpu")) + + assert data.nodal_state_w.shape == (1, 2, 6) + torch.testing.assert_close(data.nodal_state_w[..., :3], data.nodal_pos_w) + torch.testing.assert_close(data.nodal_state_w[..., 3:], data.nodal_vel_w) + torch.testing.assert_close(data.root_pos_w, torch.tensor([[1.0, 2.0, 3.0]])) + torch.testing.assert_close(data.root_vel_w, torch.tensor([[2.0, 3.0, 4.0]])) + + +def test_backend_capabilities_are_newton_only() -> None: + default = DefaultPhysicsBackend(SimpleNamespace()) + newton = NewtonPhysicsBackend(SimpleNamespace()) + + assert not default.supports_volume_deformables + assert not default.supports_surface_deformables + assert newton.supports_volume_deformables + assert newton.supports_surface_deformables + + +def test_manager_rejects_deformables_on_default_backend() -> None: + sim = object.__new__(SimulationManager) + sim.physics = DefaultPhysicsBackend(SimpleNamespace()) + + with pytest.raises(NotImplementedError, match="require the Newton backend"): + sim.add_deformable_object(VolumeDeformableObjectCfg(uid="soft")) + + +def test_deformable_facade_rejects_default_spawn_scene() -> None: + scene = Mock(spec=Scene) + scene.backend = "dexsim" + cloth = SurfaceDeformableObject(SurfaceDeformableObjectCfg(uid="cloth")) + cloth._initialize_spawn_declaration(1) + cloth.attach_spawn_handles([object()]) + + with pytest.raises(NotImplementedError, match="Default backend"): + cloth.bind_spawn(scene) + + assert cloth.is_declared + assert not cloth.is_spawn_bound + + +@pytest.mark.parametrize("solver_type", ["mujoco_warp", "featherstone"]) +def test_manager_rejects_non_particle_newton_solver(solver_type: str) -> None: + sim = object.__new__(SimulationManager) + sim.physics = NewtonPhysicsBackend(SimpleNamespace()) + sim.physics._configured_solver_type = solver_type + sim.device = torch.device("cuda") + + with pytest.raises(NotImplementedError, match="does not support deformable"): + sim.add_deformable_object(VolumeDeformableObjectCfg(uid="soft")) + + +@pytest.mark.parametrize("solver_type", ["auto", "dexuni"]) +@pytest.mark.parametrize( + "config_type", [SurfaceDeformableObjectCfg, VolumeDeformableObjectCfg] +) +def test_manager_declares_deformables_with_supported_solver( + config_type, + solver_type: str, +) -> None: + from embodichain.lab.sim.shapes import MeshCfg + from embodichain.lab.sim.spawn.scene import SpawnScene + + sim = object.__new__(SimulationManager) + sim.physics = NewtonPhysicsBackend(SimpleNamespace()) + sim.physics._configured_solver_type = solver_type + sim.device = torch.device("cuda") + sim.sim_config = SimpleNamespace(physics_cfg=NewtonPhysicsCfg()) + sim._deformable_objects = {} + sim._visualization_topology_revision = 0 + sim._spawn_scene = object.__new__(SpawnScene) + sim._spawn_scene._num_envs = 2 + sim._spawn_scene._assets = {} + sim._spawn_scene.builder = SimpleNamespace( + is_finalized=False, + result=None, + materials={}, + add_soft_object=lambda descriptor: descriptor, + add_cloth_object=lambda descriptor: descriptor, + ) + + facade = sim.add_deformable_object( + config_type(uid="deformable", shape=MeshCfg(fpath="mesh.obj")) + ) + + assert facade.is_declared + assert facade.num_instances == 2 + assert sim.get_deformable_object("deformable") is facade + + +def test_manager_rejects_gradient_mode_deformable_mutation() -> None: + sim = object.__new__(SimulationManager) + sim.physics = NewtonPhysicsBackend(SimpleNamespace()) + sim.physics._configured_solver_type = "vbd" + sim.device = torch.device("cuda") + sim.sim_config = SimpleNamespace(physics_cfg=NewtonPhysicsCfg(requires_grad=True)) + + with pytest.raises(NotImplementedError, match="requires_grad=True"): + sim.add_deformable_object(VolumeDeformableObjectCfg(uid="soft")) + + +def test_particle_data_fetches_and_partially_applies_packed_state() -> None: + particle_sets = [_ParticleSet(0.0), _ParticleSet(10.0)] + data = DeformableObjectData( + particle_sets, + _ParticleScene(), + torch.device("cpu"), + ) + + assert data.n_nodes == 2 + default_state = data.default_nodal_state_w + torch.testing.assert_close( + data.nodal_pos_w, + torch.stack([item.positions for item in particle_sets]), + ) + torch.testing.assert_close( + data.default_nodal_state_w[..., 3:], + torch.zeros((2, 2, 3)), + ) + + positions = torch.tensor( + [[[20.0, 1.0, 2.0], [21.0, 3.0, 4.0]]], dtype=torch.float32 + ) + velocities = torch.tensor([[[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]], dtype=torch.float32) + data._apply_nodal_state(positions, velocities, env_ids=[1]) + + torch.testing.assert_close( + particle_sets[0].positions[:, 0], torch.tensor([0.0, 1.0]) + ) + torch.testing.assert_close(particle_sets[1].positions, positions[0]) + torch.testing.assert_close(particle_sets[1].velocities, velocities[0]) + torch.testing.assert_close(data.default_nodal_state_w, default_state) + + +def test_particle_data_requires_equal_topology() -> None: + particle_sets: list[Any] = [_ParticleSet(0.0), _ParticleSet(1.0)] + particle_sets[1].positions = torch.zeros((3, 3), dtype=torch.float32) + particle_sets[1].velocities = torch.zeros((3, 3), dtype=torch.float32) + + with pytest.raises(ValueError, match="same particle count"): + DeformableObjectData( + particle_sets, + _ParticleScene(), + torch.device("cpu"), + ) + + +def test_deformable_rejects_replicated_render_vertex_mismatch() -> None: + deformable = object.__new__(SurfaceDeformableObject) + deformable.device = torch.device("cpu") + + with pytest.raises(RuntimeError, match="render-clone topology mismatch"): + deformable._initialize_topology([_RenderParticleSet(3), _RenderParticleSet(4)]) + + +def test_manager_stores_both_topologies_in_one_registry() -> None: + sim = object.__new__(SimulationManager) + volume = object.__new__(VolumeDeformableObject) + surface = object.__new__(SurfaceDeformableObject) + sim._deformable_objects = {"volume": volume, "surface": surface} + + assert sim.get_deformable_object("volume") is volume + assert sim.get_deformable_object_uid_list() == ["volume", "surface"] + + +@pytest.mark.parametrize( + "property_name", + [ + "nodal_pos_w", + "nodal_vel_w", + "nodal_state_w", + "default_nodal_state_w", + "root_pos_w", + "root_vel_w", + ], +) +def test_data_reads_return_independent_snapshots(property_name: str) -> None: + particles = _ParticleSet(1.0) + data = DeformableObjectData([particles], _ParticleScene(), torch.device("cpu")) + expected = getattr(data, property_name).clone() + snapshot = getattr(data, property_name) + snapshot.fill_(100.0) + torch.testing.assert_close(getattr(data, property_name), expected) + + snapshot = getattr(data, property_name) + expected = snapshot.clone() + particles.positions.add_(2.0) + particles.velocities.add_(1.0) + _ = getattr(data, property_name) + torch.testing.assert_close(snapshot, expected) diff --git a/tests/sim/objects/test_dual_arm.py b/tests/sim/objects/test_dual_arm.py index 67745bced..c02bc8bd9 100644 --- a/tests/sim/objects/test_dual_arm.py +++ b/tests/sim/objects/test_dual_arm.py @@ -20,6 +20,7 @@ import numpy as np import pytest +from embodichain.lab.sim.cfg import NewtonJointDrivePropertiesCfg from embodichain.lab.sim.robots.dual_arm import ( DualArmRobotCfg, _transform_from_xyz_rpy, @@ -162,13 +163,27 @@ def test_build_dual_arm_dual_part_toggle(): assert "dual_arm" not in cfg.control_parts -def test_build_dual_arm_preserves_gravity_setting() -> None: - base = URRobotCfg.from_dict({"robot_type": "ur5", "enable_gravity": False}) +def test_build_dual_arm_mirrors_newton_joint_overrides(): + base = URRobotCfg.from_dict({"robot_type": "ur5"}) + base.joint_drive_props = NewtonJointDrivePropertiesCfg( + stiffness={"joint[1-6]": 12.0}, + target_mode={"joint[1-6]": "position"}, + friction=0.2, + ) mounts = resolve_mounts({"preset": "side_by_side", "separation": 0.6}) cfg = build_dual_arm_cfg(base, mounts) - assert cfg.enable_gravity is False + assert isinstance(cfg.joint_drive_props, NewtonJointDrivePropertiesCfg) + assert cfg.joint_drive_props.stiffness == { + "left_joint[1-6]": 12.0, + "right_joint[1-6]": 12.0, + } + assert cfg.joint_drive_props.target_mode == { + "left_joint[1-6]": "position", + "right_joint[1-6]": "position", + } + assert cfg.joint_drive_props.friction == 0.2 # --------------------------------------------------------------------------- # diff --git a/tests/sim/objects/test_light.py b/tests/sim/objects/test_light.py index 0b7bbd794..322d42430 100644 --- a/tests/sim/objects/test_light.py +++ b/tests/sim/objects/test_light.py @@ -25,7 +25,7 @@ class TestLight: def setup_method(self): # Setup SimulationManager - config = SimulationManagerCfg(headless=True, sim_device="cpu", num_envs=10) + config = SimulationManagerCfg(headless=True, device="cpu", num_envs=10) self.sim = SimulationManager(config) # Create batch of lights @@ -37,6 +37,7 @@ def setup_method(self): "uid": "point_light", } self.light = self.sim.add_light(cfg=LightCfg.from_dict(cfg_dict)) + self.sim.prepare() def test_set_color_with_env_ids(self): """Test set_color with and without env_ids.""" @@ -169,7 +170,7 @@ class TestLightTypes: @pytest.fixture(autouse=True) def setup(self): """Create a SimulationManager for each test.""" - config = SimulationManagerCfg(headless=True, sim_device="cpu", num_envs=4) + config = SimulationManagerCfg(headless=True, device="cpu", num_envs=4) self.sim = SimulationManager(config) yield self.sim.destroy() @@ -214,9 +215,9 @@ def test_create_each_light_type(self, light_type, expected_num_instances): assert light.is_global, f"{light_type} should be a global light" def test_unknown_light_type_errors(self): - """Passing an invalid light_type raises RuntimeError.""" + """Passing an invalid light_type raises ValueError.""" cfg = LightCfg(uid="bad", light_type="invalid") - with pytest.raises(RuntimeError, match="Unsupported light type"): + with pytest.raises(ValueError, match="Unsupported light type"): self.sim.add_light(cfg=cfg) def test_mesh_light_empty_path_warns(self): diff --git a/tests/sim/objects/test_rigid_constraint.py b/tests/sim/objects/test_rigid_constraint.py index 9911135d3..6cd8bb626 100644 --- a/tests/sim/objects/test_rigid_constraint.py +++ b/tests/sim/objects/test_rigid_constraint.py @@ -263,8 +263,7 @@ def __init__(self, num_envs=4, arenas=None): self._robots = {} self._rigid_objects = {} self._rigid_object_groups = {} - self._soft_objects = {} - self._cloth_objects = {} + self._deformable_objects = {} self._articulations = {} self._constraints = {} self.device = torch.device("cpu") diff --git a/tests/sim/objects/test_rigid_object.py b/tests/sim/objects/test_rigid_object.py index 929810cae..d281ce595 100644 --- a/tests/sim/objects/test_rigid_object.py +++ b/tests/sim/objects/test_rigid_object.py @@ -13,41 +13,73 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- - from __future__ import annotations import os -import torch + +import numpy as np import pytest +import torch from embodichain.lab.sim import ( SimulationManager, SimulationManagerCfg, VisualMaterialCfg, ) -from embodichain.lab.sim.objects import RigidObject -from embodichain.lab.sim.cfg import RigidObjectCfg, RigidBodyAttributesCfg -from embodichain.lab.sim.shapes import MeshCfg from embodichain.data import get_data_path -from dexsim.types import ActorType - -from embodichain.lab.sim.cfg import RenderCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import ( + MassPropertiesCfg, + NewtonCollisionPropertiesCfg, + NewtonRigidBodyMaterialCfg, + RigidBodyPhysicsCfg, + RigidObjectCfg, + physics_cfg_for_backend, +) +from embodichain.lab.sim.objects import RigidObject +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, MeshCollisionCfg +from embodichain.utils.math import matrix_from_quat DUCK_PATH = "ToyDuck/toy_duck.glb" TABLE_PATH = "ShopTableSimple/shop_table_simple.ply" CHAIR_PATH = "Chair/chair.glb" NUM_ARENAS = 2 Z_TRANSLATION = 2.0 +# Newton stores a full inertia tensor and converts it to/from the principal-frame +# diagonal in float32. The two quaternion rotations introduce small round-trip +# error for imported meshes whose COM frame is not axis-aligned. +NEWTON_INERTIA_ROUND_TRIP_ATOL = 2e-4 + + +def _make_test_com_pose(device: torch.device) -> torch.Tensor: + """Create per-env COM poses using EmbodiChain xyzw quaternion convention.""" + return torch.tensor( + [ + [0.04, -0.02, 0.03, 0.0, 0.0, 0.0, 1.0], + [-0.01, 0.05, 0.02, 0.0, 0.0, 0.70710677, 0.70710677], + ], + device=device, + dtype=torch.float32, + ) + + +def _teardown_newton_physics() -> None: + from dexsim.engine.newton_physics import teardown_newton_physics + + teardown_newton_physics() class BaseRigidObjectTest: """Shared test logic for CPU and CUDA.""" - def setup_simulation(self, sim_device): + def setup_simulation(self, device: str, physics: str = "default"): config = SimulationManagerCfg( - headless=True, sim_device=sim_device, num_envs=NUM_ARENAS + headless=True, + device=device, + num_envs=NUM_ARENAS, + physics_cfg=physics_cfg_for_backend(physics), ) self.sim = SimulationManager(config) + self.physics = physics self.sim.enable_physics(False) duck_path = get_data_path(DUCK_PATH) assert os.path.isfile(duck_path) @@ -62,9 +94,7 @@ def setup_simulation(self, sim_device): "shape_type": "Mesh", "fpath": duck_path, }, - "attrs": { - "mass": 1.0, - }, + "attrs": {"mass_props": {"mass": 1.0}}, "body_type": "dynamic", } self.duck: RigidObject = self.sim.add_rigid_object( @@ -78,12 +108,13 @@ def setup_simulation(self, sim_device): self.chair: RigidObject = self.sim.add_rigid_object( cfg=RigidObjectCfg( - uid="chair", shape=MeshCfg(fpath=chair_path), body_type="kinematic" + uid="chair", + shape=MeshCfg(fpath=chair_path), + body_type="kinematic", ), ) - if sim_device == "cuda" and getattr(self.sim, "is_use_gpu_physics", False): - self.sim.init_gpu_physics() + self.sim.prepare() self.sim.enable_physics(True) @@ -95,6 +126,34 @@ def test_is_static(self): not self.chair.is_static ), "Chair should be kinematic but is marked static" + def test_replicated_collision_shapes_are_isolated_by_environment(self): + """Every rigid shape should use its replicated environment group.""" + for env_index in range(NUM_ARENAS): + for rigid_object in (self.duck, self.table, self.chair): + entity = rigid_object._entities[env_index] + if self.physics == "newton": + shape_ids = entity.physics_body.shape_ids + assert shape_ids + groups = ( + entity.physics_body.runtime.model.shape_collision_group.numpy() + ) + assert {int(groups[shape_id]) for shape_id in shape_ids} == { + env_index + 1 + } + else: + np.testing.assert_array_equal( + entity.object_desc.physics.collision_filter_data, + np.asarray([env_index, 1, 0, 0], dtype=np.uint32), + ) + + def test_spawn_clones_distinct_entities(self): + """Multi-env rigid objects are spawned via prototype + clone_actor_to.""" + assert len(self.duck._entities) == NUM_ARENAS + handles = {entity.get_native_handle() for entity in self.duck._entities} + assert len(handles) == NUM_ARENAS, "Each arena clone must be a distinct actor" + assert {entity.get_name() for entity in self.duck._entities} == {"duck"} + assert len({entity.path for entity in self.duck._entities}) == NUM_ARENAS + def test_local_pose_behavior(self): """Test set_local_pose and get_local_pose: - duck pose is correctly set @@ -160,9 +219,32 @@ def test_local_pose_behavior(self): assert all( abs(x) < 1e-5 for x in table_xyz_after ), f"FAIL: Table moved unexpectedly: {table_xyz_after}" - assert torch.allclose( - chair_xyz_after, expected_chair_pos, atol=1e-5 - ), f"FAIL: Chair pose changed unexpectedly: {chair_xyz_after.tolist()}" + if self.chair.body_type == "kinematic" and self.physics != "newton": + assert torch.allclose( + chair_xyz_after, expected_chair_pos, atol=1e-5 + ), f"FAIL: Chair pose changed unexpectedly: {chair_xyz_after.tolist()}" + # Newton: kinematic bodies are not pose-locked yet (DexSim TODO). + + def test_dynamic_pose_write_persists_across_physics_step(self): + """A dynamic pose reset must update Newton's FREE-joint state too.""" + target_xy = torch.tensor( + [[0.31, -0.27], [-0.42, 0.36]], + dtype=torch.float32, + device=self.sim.device, + ) + pose = torch.eye(4, device=self.sim.device).repeat(NUM_ARENAS, 1, 1) + pose[:, :2, 3] = target_xy + pose[:, 2, 3] = Z_TRANSLATION + + self.duck.set_local_pose(pose) + self.sim.update(step=1) + + torch.testing.assert_close( + self.duck.get_local_pose()[:, :2], + target_xy, + atol=1.0e-4, + rtol=0.0, + ) def test_add_force_torque(self): """Test that add_force applies force correctly to the duck object.""" @@ -333,7 +415,13 @@ def test_add_sdf_mesh(self): sdf = self.sim.add_rigid_object( cfg=RigidObjectCfg( uid="duck_sdf", - shape=MeshCfg(fpath=duck_path, sdf_resolution=128), + shape=MeshCfg( + fpath=duck_path, + collision=MeshCollisionCfg( + approximation="sdf", + sdf_resolution=128, + ), + ), body_type="dynamic", ) ) @@ -361,6 +449,8 @@ def test_body_data(self): """Test the body_data property for dynamic objects.""" # Dynamic object should have body_data assert self.duck.body_data is not None, "Dynamic duck should have body_data" + assert self.duck.body_data.mass.shape == (NUM_ARENAS,) + assert self.duck.body_data.inertia.shape == (NUM_ARENAS, 3) # Static object should return None with warning assert self.table.body_data is None, "Static table should not have body_data" @@ -368,6 +458,29 @@ def test_body_data(self): # Kinematic object should have body_data assert self.chair.body_data is not None, "Kinematic chair should have body_data" + def test_default_physical_properties_remain_at_initialized_values(self): + """Test runtime writes do not mutate the mass-property snapshots.""" + assert self.duck.body_data is not None + data = self.duck.body_data + initial_mass = self.duck.get_mass().clone() + initial_inertia = self.duck.get_inertia().clone() + initial_com_pose = data.com_pose.clone() + + assert torch.allclose(data.default_mass, initial_mass) + assert torch.allclose(data.default_inertia, initial_inertia) + assert torch.allclose(data.default_com_pose, initial_com_pose) + assert torch.allclose(self.duck.default_mass, data.default_mass) + + self.duck.set_mass(initial_mass + 0.5) + self.duck.set_inertia(initial_inertia + 0.1) + changed_com_pose = initial_com_pose.clone() + changed_com_pose[:, :3] += 0.05 + self.duck.set_com_pose(changed_com_pose) + + assert torch.allclose(data.default_mass, initial_mass) + assert torch.allclose(data.default_inertia, initial_inertia) + assert torch.allclose(data.default_com_pose, initial_com_pose) + def test_physical_attributes(self): """Test getting and setting physical attributes and body states.""" # 1. Body state @@ -403,30 +516,108 @@ def test_physical_attributes(self): # 2. is_non_dynamic assert not self.duck.is_non_dynamic, "Dynamic duck should not be is_non_dynamic" assert self.table.is_non_dynamic, "Static table should be is_non_dynamic" - assert self.chair.is_non_dynamic, "Kinematic chair should be is_non_dynamic" + assert self.chair.is_non_dynamic == (self.chair.body_type == "kinematic") + + if self.physics == "newton": + expected_mass = torch.ones(NUM_ARENAS, device=self.sim.device) + expected_inertia = self.duck.get_inertia() + assert expected_inertia.shape == (NUM_ARENAS, 3) + assert ( + expected_inertia >= 0 + ).all(), "Initial inertia should be non-negative" + + assert torch.allclose(self.duck.get_mass(), expected_mass) + assert self.duck.get_friction().shape == (NUM_ARENAS,) + assert torch.isfinite(self.duck.get_friction()).all() + assert self.duck.get_damping().shape == (NUM_ARENAS, 2) + assert torch.isfinite(self.duck.get_damping()).all() + + self.duck.set_attrs( + RigidBodyPhysicsCfg.from_dict({"mass_props": {"mass": 2.5}}) + ) + assert torch.allclose( + self.duck.get_mass(), + torch.full((NUM_ARENAS,), 2.5, device=self.sim.device), + ) + + # Actor type is topology, not a runtime batch property. + with pytest.raises(NotImplementedError, match="descriptor mutation"): + self.duck.set_body_type("kinematic") + assert self.duck.body_type == "dynamic" + + # Mass: set and verify round-trip + new_mass = torch.full((NUM_ARENAS,), 2.5, device=self.sim.device) + self.duck.set_mass(new_mass) + assert torch.allclose( + self.duck.get_mass(), new_mass, atol=1e-5 + ), f"Newton set_mass round-trip failed: {self.duck.get_mass()}" + + # Friction: set and verify round-trip + new_friction = torch.full((NUM_ARENAS,), 0.7, device=self.sim.device) + self.duck.set_friction(new_friction) + assert torch.allclose( + self.duck.get_friction(), new_friction, atol=1e-5 + ), f"Newton set_friction round-trip failed: {self.duck.get_friction()}" + + # Inertia: set and verify round-trip + new_inertia = torch.full((NUM_ARENAS, 3), 0.3, device=self.sim.device) + self.duck.set_inertia(new_inertia) + actual_inertia = self.duck.get_inertia() + assert torch.allclose( + actual_inertia, + new_inertia, + atol=NEWTON_INERTIA_ROUND_TRIP_ATOL, + rtol=0.0, + ), ( + "Newton set_inertia round-trip failed: " + f"max_abs_error={(actual_inertia - new_inertia).abs().max().item()}" + ) + + # Damping is a runtime no-op on Newton (not modelled per body) but + # mirrors onto metadata so get_damping stays consistent. + new_damping = torch.full((NUM_ARENAS, 2), 0.2, device=self.sim.device) + self.duck.set_damping(new_damping) + assert torch.allclose( + self.duck.get_damping(), new_damping, atol=1e-5 + ), "Newton set_damping should mirror onto metadata for get_damping" + + # Static Spawn actors do not have dynamic body ids. Their getters + # remain readable from source/backend metadata. Empty grouped cfgs + # intentionally preserve those values rather than authoring defaults. + assert self.table.get_mass().shape == (NUM_ARENAS,) + assert torch.isfinite(self.table.get_mass()).all() + assert self.table.get_friction().shape == (NUM_ARENAS,) + assert torch.isfinite(self.table.get_friction()).all() + assert self.table.get_damping().shape == (NUM_ARENAS, 2) + assert torch.isfinite(self.table.get_damping()).all() + assert torch.equal( + self.table.get_inertia(), + torch.zeros((NUM_ARENAS, 3), device=self.sim.device), + ) + return # 3. body_type assert self.duck.body_type == "dynamic" - self.duck.set_body_type("kinematic") - assert self.duck.body_type == "kinematic" - self.duck.set_body_type("dynamic") + with pytest.raises(NotImplementedError, match="descriptor mutation"): + self.duck.set_body_type("kinematic") assert self.duck.body_type == "dynamic" - assert self.chair.body_type == "kinematic" - self.chair.set_body_type("dynamic") - assert self.chair.body_type == "dynamic" - self.chair.set_body_type("kinematic") - assert self.chair.body_type == "kinematic" + if self.chair.body_type == "kinematic": + with pytest.raises(NotImplementedError, match="descriptor mutation"): + self.chair.set_body_type("dynamic") + assert self.chair.body_type == "kinematic" # 4. attrs - new_attrs = RigidBodyAttributesCfg(mass=2.5, density=1000.0) + new_attrs = RigidBodyPhysicsCfg.from_dict( + {"mass_props": {"mass": 2.5, "density": 1000.0}} + ) self.duck.set_attrs(new_attrs) masses = self.duck.get_mass() assert torch.allclose( masses, torch.tensor([2.5] * NUM_ARENAS, device=self.sim.device) ), f"Mass not set correctly: {masses.tolist()}" - partial_attrs = RigidBodyAttributesCfg(mass=3.0) + partial_attrs = RigidBodyPhysicsCfg.from_dict({"mass_props": {"mass": 3.0}}) self.duck.set_attrs(partial_attrs, env_ids=[0]) masses = self.duck.get_mass() assert torch.allclose( @@ -482,29 +673,62 @@ def test_physical_attributes(self): self.duck.get_body_scale(), new_scale ), f"Body scale not set correctly" - # 6. COM pose - com_pose = torch.zeros((NUM_ARENAS, 7), device=self.sim.device) - com_pose[:, 3] = 1.0 # Unit quaternion - com_pose[0, :3] = torch.tensor([0.1, 0.1, 0.1], device=self.sim.device) - - self.duck.set_com_pose(com_pose) - - # Static object should not be able to set COM pose - self.table.set_com_pose(com_pose) # Should log warning but not crash - + def test_set_com_pose(self): + """Test setting full and partial center-of-mass poses.""" assert self.duck.body_data is not None assert self.duck.body_data.default_com_pose is not None assert self.duck.body_data.default_com_pose.shape == ( NUM_ARENAS, 7, - ), f"Default COM pose should have shape (NUM_ARENAS, 7)" + ), "Default COM pose should have shape (NUM_ARENAS, 7)" + + com_pose = _make_test_com_pose(self.sim.device) - com_pose = self.duck.body_data.com_pose - assert isinstance(com_pose, torch.Tensor), "com_pose should be a torch.Tensor" - assert com_pose.shape == ( + self.duck.set_com_pose(com_pose) + + actual_com_pose = self.duck.body_data.com_pose + assert isinstance( + actual_com_pose, torch.Tensor + ), "com_pose should be a torch.Tensor" + assert actual_com_pose.shape == ( NUM_ARENAS, 7, - ), f"COM pose should have shape (NUM_ARENAS, 7), got {com_pose.shape}" + ), f"COM pose should have shape (NUM_ARENAS, 7), got {actual_com_pose.shape}" + assert torch.allclose(actual_com_pose, com_pose, atol=1e-5), ( + "COM pose did not match after full set: " + f"expected {com_pose.tolist()}, got {actual_com_pose.tolist()}" + ) + + partial_com_pose = torch.tensor( + [[0.07, -0.03, 0.04, 0.0, 0.38268343, 0.0, 0.9238795]], + device=self.sim.device, + dtype=torch.float32, + ) + expected_com_pose = com_pose.clone() + expected_com_pose[1] = partial_com_pose[0] + + self.duck.set_com_pose(partial_com_pose, env_ids=[1]) + + actual_com_pose = self.duck.body_data.com_pose + assert torch.allclose(actual_com_pose, expected_com_pose, atol=1e-5), ( + "COM pose did not preserve untouched envs after partial set: " + f"expected {expected_com_pose.tolist()}, got {actual_com_pose.tolist()}" + ) + + assert self.chair.body_data is not None + chair_com_pose_before = self.chair.body_data.com_pose.clone() + self.chair.set_com_pose(com_pose) + if self.chair.body_type == "kinematic": + assert torch.allclose( + self.chair.body_data.com_pose, chair_com_pose_before, atol=1e-5 + ), "Kinematic rigid object COM pose should not change" + else: + assert torch.allclose( + self.chair.body_data.com_pose, com_pose, atol=1e-5 + ), "Dynamic rigid object COM pose should change" + + # Static object should not be able to set COM pose. + self.table.set_com_pose(com_pose) def test_misc_properties(self): """Test miscellaneous properties like collision filter, vertices, and visual materials.""" @@ -579,6 +803,291 @@ def test_misc_properties(self): 1.0, ], f"Material {i} base color incorrect" + def test_geometry_data(self): + """Test mesh-level read APIs: get_triangles and scaled get_vertices. + + Covers: + - ``get_triangles`` — shape ``(N, num_tris, 3)``, int32, partial env_ids. + - ``get_vertices(scale=True)`` — scaled vertices differ from unscaled. + """ + # --- get_triangles (full) --- + triangles = self.duck.get_triangles() + assert isinstance( + triangles, torch.Tensor + ), "get_triangles should return a torch.Tensor" + assert triangles.ndim == 3, "Triangles tensor should be 3-D (N, num_tris, 3)" + assert ( + triangles.shape[0] == NUM_ARENAS + ), f"First dim should be {NUM_ARENAS}, got {triangles.shape[0]}" + assert triangles.shape[2] == 3, "Last dim should be 3 (vertex indices)" + assert ( + triangles.dtype == torch.int32 + ), f"Triangles dtype should be int32, got {triangles.dtype}" + + # --- get_triangles (partial) --- + partial_tris = self.duck.get_triangles(env_ids=[0]) + assert ( + partial_tris.shape[0] == 1 + ), "Partial get_triangles should return 1 instance" + + # --- get_vertices(scale=True) --- + new_scale = torch.full( + (NUM_ARENAS, 3), 2.0, device=self.sim.device, dtype=torch.float32 + ) + self.duck.set_body_scale(new_scale) + + verts_raw = self.duck.get_vertices() + verts_scaled = self.duck.get_vertices(scale=True) + assert torch.allclose( + verts_scaled, verts_raw * 2.0, atol=1e-5 + ), "Scaled vertices should be 2x the raw vertices" + + def test_enable_collision(self): + """Test enable_collision toggle for individual arenas. + + Covers: + - ``enable_collision`` with ``enable=False`` (per-instance mask). + - ``enable_collision`` with ``enable=True`` (restore). + - partial ``env_ids`` subset. + """ + # Disable collision for all arenas and re-enable — no exception should be raised. + disable = torch.zeros(NUM_ARENAS, dtype=torch.bool, device=self.sim.device) + self.duck.enable_collision(disable) + + enable = torch.ones(NUM_ARENAS, dtype=torch.bool, device=self.sim.device) + self.duck.enable_collision(enable) + + # Partial: disable only env 0. + partial_disable = torch.zeros(1, dtype=torch.bool, device=self.sim.device) + self.duck.enable_collision(partial_disable, env_ids=[0]) + + # Restore env 0. + partial_enable = torch.ones(1, dtype=torch.bool, device=self.sim.device) + self.duck.enable_collision(partial_enable, env_ids=[0]) + + def test_reset(self): + """Test reset() restores initial pose and clears dynamics. + + Covers: + - ``reset()`` — all envs returned to ``cfg.init_pos`` (default origin). + - Velocities cleared to zero after reset. + - Partial ``env_ids`` reset: only the specified instance is restored. + """ + # Move duck far from origin and give it velocity. + pose_far = ( + torch.eye(4, device=self.sim.device).unsqueeze(0).repeat(NUM_ARENAS, 1, 1) + ) + pose_far[:, 2, 3] = 5.0 + self.duck.set_local_pose(pose_far) + + lin_vel = ( + torch.tensor([3.0, 0.0, 0.0], device=self.sim.device) + .unsqueeze(0) + .repeat(NUM_ARENAS, 1) + ) + self.duck.set_velocity(lin_vel=lin_vel) + + # Full reset. + self.duck.reset() + + pos_after = self.duck.get_local_pose()[:, :3] + origin = torch.zeros(NUM_ARENAS, 3, device=self.sim.device) + assert torch.allclose( + pos_after, origin, atol=1e-4 + ), f"Duck should be at origin after reset, got {pos_after.tolist()}" + + # Velocities should be zero after reset. + assert self.duck.body_data is not None + lin_vel_after = self.duck.body_data.lin_vel + assert torch.allclose( + lin_vel_after, torch.zeros_like(lin_vel_after), atol=1e-5 + ), f"Linear velocity should be zero after reset, got {lin_vel_after.tolist()}" + + # --- Partial reset: move duck again, reset only env 0 --- + self.duck.set_local_pose(pose_far) + self.duck.reset(env_ids=[0]) + + pos_partial = self.duck.get_local_pose()[:, :3] + assert torch.allclose( + pos_partial[0], origin[0], atol=1e-4 + ), f"Env 0 should be at origin after partial reset, got {pos_partial[0].tolist()}" + # Env 1 was not reset — it should still be displaced. + assert ( + pos_partial[1, 2].item() > 1.0 + ), f"Env 1 should remain displaced after partial reset, got z={pos_partial[1, 2].item()}" + + def test_reset_restores_default_physical_properties(self): + """Test full and partial reset restore mass, inertia, and COM defaults.""" + assert self.duck.body_data is not None + data = self.duck.body_data + default_mass = data.default_mass.clone() + default_inertia = data.default_inertia.clone() + default_com_pose = data.default_com_pose.clone() + + changed_mass = default_mass + 0.5 + changed_inertia = default_inertia + 0.1 + changed_com_pose = default_com_pose.clone() + changed_com_pose[:, :3] += 0.05 + self.duck.set_mass(changed_mass) + self.duck.set_inertia(changed_inertia) + self.duck.set_com_pose(changed_com_pose) + + self.duck.reset(env_ids=[0]) + + mass_after_partial = self.duck.get_mass() + inertia_after_partial = self.duck.get_inertia() + com_after_partial = data.com_pose + inertia_atol = ( + NEWTON_INERTIA_ROUND_TRIP_ATOL if self.physics == "newton" else 1e-5 + ) + assert torch.allclose(mass_after_partial[0], default_mass[0], atol=1e-5) + assert torch.allclose( + inertia_after_partial[0], + default_inertia[0], + atol=inertia_atol, + rtol=0.0, + ) + assert torch.allclose(com_after_partial[0], default_com_pose[0], atol=1e-5) + assert torch.allclose(mass_after_partial[1], changed_mass[1], atol=1e-5) + assert torch.allclose( + inertia_after_partial[1], + changed_inertia[1], + atol=inertia_atol, + rtol=0.0, + ) + assert torch.allclose(com_after_partial[1], changed_com_pose[1], atol=1e-5) + + self.duck.reset() + + assert torch.allclose(self.duck.get_mass(), default_mass, atol=1e-5) + assert torch.allclose( + self.duck.get_inertia(), + default_inertia, + atol=inertia_atol, + rtol=0.0, + ) + assert torch.allclose(data.com_pose, default_com_pose, atol=1e-5) + + def test_local_pose_matrix(self): + """Test ``get_local_pose(to_matrix=True)`` returns correct shape and values. + + Covers: + - Shape ``(N, 4, 4)`` output. + - Rotation and translation columns are consistent with the 7-vec form. + - Partial ``env_ids``. + """ + pose_7 = torch.eye(4, device=self.sim.device) + pose_7[0, 3] = 1.0 + pose_7[1, 3] = 2.0 + pose_7[2, 3] = 3.0 + expected_xyzw = torch.tensor( + [1.0, 2.0, 3.0, 4.0], device=self.sim.device + ) / torch.sqrt(torch.tensor(30.0, device=self.sim.device)) + pose_7[:3, :3] = matrix_from_quat(expected_xyzw.unsqueeze(0))[0] + pose_mat_input = pose_7.unsqueeze(0).repeat(NUM_ARENAS, 1, 1) + self.duck.set_local_pose(pose_mat_input) + + # 7-vec form + pose_vec = self.duck.get_local_pose(to_matrix=False) + assert pose_vec.shape == ( + NUM_ARENAS, + 7, + ), f"7-vec pose shape should be ({NUM_ARENAS}, 7), got {pose_vec.shape}" + torch.testing.assert_close( + pose_vec[:, 3:7], + expected_xyzw.unsqueeze(0).expand(NUM_ARENAS, -1), + atol=1e-5, + rtol=1e-5, + ) + + # Matrix form + pose_mat = self.duck.get_local_pose(to_matrix=True) + assert pose_mat.shape == ( + NUM_ARENAS, + 4, + 4, + ), f"Matrix pose shape should be ({NUM_ARENAS}, 4, 4), got {pose_mat.shape}" + + # Translation columns must match. + assert torch.allclose( + pose_mat[:, :3, 3], pose_vec[:, :3], atol=1e-5 + ), "Matrix translation column should match 7-vec xyz" + + # Last row must be [0, 0, 0, 1]. + last_row = ( + torch.tensor([0.0, 0.0, 0.0, 1.0], device=self.sim.device) + .unsqueeze(0) + .repeat(NUM_ARENAS, 1) + ) + assert torch.allclose( + pose_mat[:, 3, :], last_row, atol=1e-5 + ), "Last row of pose matrix should be [0, 0, 0, 1]" + + # Rotation matrix must be orthogonal (R @ R.T ≈ I). + R = pose_mat[:, :3, :3] + eye = torch.eye(3, device=self.sim.device).unsqueeze(0).repeat(NUM_ARENAS, 1, 1) + assert torch.allclose( + torch.bmm(R, R.transpose(1, 2)), eye, atol=1e-5 + ), "Rotation sub-matrix should be orthogonal" + + # Partial env_ids. + pose_mat_partial = self.duck.get_local_pose(to_matrix=True) + assert pose_mat_partial.shape[0] == NUM_ARENAS + + def test_body_data_vel_clear(self): + """Test ``body_data.vel``, partial ``clear_dynamics``, and verify dynamics reset. + + Covers: + - ``body_data.vel`` — shape ``(N, 6)`` concatenated lin+ang vel. + - ``clear_dynamics()`` — verifies all velocities become zero (not just called). + - ``clear_dynamics(env_ids=[0])`` — partial clear; only env 0 is zeroed. + """ + assert self.duck.body_data is not None + + lin_vel = ( + torch.tensor([2.0, 0.0, 0.0], device=self.sim.device) + .unsqueeze(0) + .repeat(NUM_ARENAS, 1) + ) + ang_vel = ( + torch.tensor([0.0, 3.0, 0.0], device=self.sim.device) + .unsqueeze(0) + .repeat(NUM_ARENAS, 1) + ) + self.duck.set_velocity(lin_vel=lin_vel, ang_vel=ang_vel) + + # --- body_data.vel --- + vel = self.duck.body_data.vel + assert vel.shape == ( + NUM_ARENAS, + 6, + ), f"vel shape should be ({NUM_ARENAS}, 6), got {vel.shape}" + assert torch.allclose( + vel[:, :3], lin_vel, atol=1e-5 + ), f"First 3 columns of vel should match lin_vel" + assert torch.allclose( + vel[:, 3:], ang_vel, atol=1e-5 + ), f"Last 3 columns of vel should match ang_vel" + + # --- clear_dynamics() full — verify velocities go to zero --- + self.duck.clear_dynamics() + vel_after_clear = self.duck.body_data.vel + assert torch.allclose( + vel_after_clear, torch.zeros_like(vel_after_clear), atol=1e-5 + ), f"Velocities should be zero after clear_dynamics, got {vel_after_clear.tolist()}" + + # --- clear_dynamics(env_ids=[0]) partial --- + # Give env 1 non-zero velocity again. + self.duck.set_velocity(lin_vel=lin_vel, ang_vel=ang_vel) + self.duck.clear_dynamics(env_ids=[0]) + vel_partial = self.duck.body_data.vel + assert torch.allclose( + vel_partial[0], torch.zeros(6, device=self.sim.device), atol=1e-5 + ), f"Env 0 should be zeroed after partial clear_dynamics, got {vel_partial[0].tolist()}" + assert not torch.allclose( + vel_partial[1], torch.zeros(6, device=self.sim.device), atol=1e-5 + ), "Env 1 should still have non-zero velocity after partial clear_dynamics" + def test_multi_mesh_geometry_is_combined(self): """GLB render meshes are exported as one complete indexed geometry.""" render_body = self.chair._entities[0].get_render_body() @@ -620,6 +1129,155 @@ class TestRigidObjectCUDA(BaseRigidObjectTest): def setup_method(self): self.setup_simulation("cuda") + def test_kinematic_binding_supports_pose_updates(self): + obj = self.sim.add_rigid_object( + cfg=RigidObjectCfg( + uid="gpu_kinematic", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + body_type="kinematic", + ) + ) + assert obj.body_data is not None + + pose = torch.eye(4, device=self.sim.device).repeat(NUM_ARENAS, 1, 1) + pose[:, :3, 3] = torch.tensor([0.2, -0.1, 0.5], device=self.sim.device) + obj.set_local_pose(pose) + self.sim.update(0.01) + + assert torch.allclose(obj.get_local_pose(to_matrix=True), pose, atol=1e-5) + + +class TestRigidObjectNewton(BaseRigidObjectTest): + """Full rigid-object coverage on the DexSim Newton physics backend.""" + + def setup_method(self): + self.setup_simulation("cuda", physics="newton") + + def teardown_method(self): + super().teardown_method() + _teardown_newton_physics() + + def test_physical_attributes(self): + """Newton getters and setters for mass, friction, inertia work via batch API.""" + super().test_physical_attributes() + + def test_newton_native_attrs_desc_native_spawn(self): + """Typed Newton attributes register through the public Spawn result. + + Newton-native contact/shape parameters are consumed by the descriptor + adapter without an independently owned manager or legacy patch path. + """ + duck_path = get_data_path(DUCK_PATH) + cfg = RigidObjectCfg( + uid="duck_newton_native", + shape=MeshCfg(fpath=duck_path), + body_type="dynamic", + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), + collision_props=NewtonCollisionPropertiesCfg(margin=0.01), + material_props=NewtonRigidBodyMaterialCfg( + dynamic_friction=0.5, + restitution=0.1, + ke=1e3, + kd=50.0, + ), + ), + ) + obj: RigidObject = self.sim.add_rigid_object(cfg=cfg) + self.sim.prepare() + + assert obj.num_instances == NUM_ARENAS + assert obj.body_type == "dynamic" + result = self.sim.spawn_result + handles = [ + result.get_object(f"{arena_name}/{obj.uid}") + for arena_name in result.arenas.names[1:] + ] + assert len(result.create_rigid_body_batch(handles)) == NUM_ARENAS + assert all(handle.is_valid for handle in handles) + assert all( + handle.desc is not None and handle.desc.physics is not None + for handle in handles + ) + # Common fields round-trip via the batch view (mass applied live). + assert torch.allclose( + obj.get_mass(), + torch.full((NUM_ARENAS,), 1.0, device=self.sim.device), + atol=1e-5, + ) + + @pytest.mark.skip( + reason="TODO: DexSim Newton SDF rigidbody path is not validated in EmbodiChain yet." + ) + def test_add_sdf_mesh(self): + super().test_add_sdf_mesh() + + +@pytest.mark.gpu +class TestRigidObjectNewtonMujoco: + """Focused standalone-rigid state synchronization on MuJoCo-Warp.""" + + def setup_method(self): + physics_cfg = physics_cfg_for_backend("newton") + physics_cfg.gravity = (0.0, 0.0, 0.0) + physics_cfg.solver_cfg = {"solver_type": "mujoco_warp"} + self.sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda", + num_envs=1, + physics_cfg=physics_cfg, + ) + ) + self.obj = self.sim.add_rigid_object( + RigidObjectCfg( + uid="free_body", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), + ), + init_pos=(0.0, 0.0, Z_TRANSLATION), + ) + ) + self.sim.prepare() + + def teardown_method(self): + self.sim.destroy() + SimulationManager.flush_cleanup_queue() + _teardown_newton_physics() + + def test_clear_dynamics_persists_across_mujoco_step(self): + """Clearing body velocity also clears its reduced FREE-joint velocity.""" + linear_velocity = torch.tensor( + [[0.4, -0.2, 0.3]], + dtype=torch.float32, + device=self.sim.device, + ) + angular_velocity = torch.tensor( + [[0.1, 0.2, -0.3]], + dtype=torch.float32, + device=self.sim.device, + ) + self.obj.set_velocity( + lin_vel=linear_velocity, + ang_vel=angular_velocity, + ) + self.sim.update(step=1) + assert not torch.allclose( + self.obj.body_data.vel, + torch.zeros((1, 6), device=self.sim.device), + ) + + self.obj.clear_dynamics() + self.sim.update(step=1) + + torch.testing.assert_close( + self.obj.body_data.vel, + torch.zeros((1, 6), device=self.sim.device), + atol=1.0e-5, + rtol=0.0, + ) + if __name__ == "__main__": # pytest.main(["-s", __file__]) diff --git a/tests/sim/objects/test_rigid_object_group.py b/tests/sim/objects/test_rigid_object_group.py index 4fadc3869..5bc43618f 100644 --- a/tests/sim/objects/test_rigid_object_group.py +++ b/tests/sim/objects/test_rigid_object_group.py @@ -17,12 +17,18 @@ from __future__ import annotations import os +from unittest.mock import Mock + import torch import pytest from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.objects import RigidBodyGroupData, RigidObjectGroup -from embodichain.lab.sim.cfg import RigidObjectGroupCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import ( + RigidObjectGroupCfg, + RigidObjectCfg, + physics_cfg_for_backend, +) from embodichain.lab.sim.shapes import MeshCfg from embodichain.data import get_data_path from dexsim.types import ActorType @@ -31,39 +37,51 @@ TABLE_PATH = "ShopTableSimple/shop_table_simple.ply" NUM_ARENAS = 4 Z_TRANSLATION = 2.0 +# Newton converts principal-frame inertia diagonals through a float32 full +# tensor, so imported non-axis-aligned COM frames are not bit-exact on readback. +NEWTON_INERTIA_ROUND_TRIP_ATOL = 2e-4 -@pytest.mark.no_sim -def test_cpu_body_data_reads_angular_velocity_from_angular_api(): - """CPU rigid-object groups must not report linear velocity as angular.""" +def _teardown_newton_physics() -> None: + from dexsim.engine.newton_physics import teardown_newton_physics - class VelocityEntity: - def get_linear_velocity(self): - return [1.0, 2.0, 3.0] + teardown_newton_physics() - def get_angular_velocity(self): - return [4.0, 5.0, 6.0] - body_data = object.__new__(RigidBodyGroupData) - body_data.entities = [[VelocityEntity(), VelocityEntity()]] - body_data.device = torch.device("cpu") +@pytest.mark.no_sim +def test_cpu_body_data_reads_angular_velocity_from_angular_api(): + """CPU rigid-object groups must not report linear velocity as angular.""" + expected = torch.tensor([[[4.0, 5.0, 6.0], [4.0, 5.0, 6.0]]]) + body_view = Mock() + body_view.fetch_angular_velocity.side_effect = lambda out: out.copy_( + expected.reshape(-1, 3) + ) + body_data = RigidBodyGroupData( + body_view, + num_instances=1, + num_objects=2, + device=torch.device("cpu"), + ) angular_velocity = body_data.ang_vel - assert torch.equal( - angular_velocity, - torch.tensor([[[4.0, 5.0, 6.0], [4.0, 5.0, 6.0]]]), - ) + assert torch.equal(angular_velocity, expected) + body_view.fetch_angular_velocity.assert_called_once() + body_view.fetch_linear_velocity.assert_not_called() class BaseRigidObjectGroupTest: """Shared test logic for CPU and CUDA.""" - def setup_simulation(self, sim_device): + def setup_simulation(self, device: str, physics: str = "default") -> None: config = SimulationManagerCfg( - headless=True, sim_device=sim_device, num_envs=NUM_ARENAS + headless=True, + device=device, + num_envs=NUM_ARENAS, + physics_cfg=physics_cfg_for_backend(physics), ) self.sim = SimulationManager(config) + self.physics = physics duck_path = get_data_path(DUCK_PATH) assert os.path.isfile(duck_path) @@ -91,8 +109,7 @@ def setup_simulation(self, sim_device): cfg=RigidObjectGroupCfg.from_dict(cfg_dict) ) - if sim_device == "cuda" and self.sim.is_use_gpu_physics: - self.sim.init_gpu_physics() + self.sim.prepare() self.sim.enable_physics(True) @@ -117,6 +134,118 @@ def test_local_pose_behavior(self): atol=1e-5, ), "FAIL: Local poses do not match after setting." + distinct_xyzw = torch.tensor( + [1.0, 2.0, 3.0, 4.0], device=self.sim.device + ) / torch.sqrt(torch.tensor(30.0, device=self.sim.device)) + vector_pose = torch.zeros( + NUM_ARENAS, + self.obj_group.num_objects, + 7, + device=self.sim.device, + ) + vector_pose[..., :3] = combined_pose[..., :3, 3] + vector_pose[..., 3:7] = distinct_xyzw + + self.obj_group.set_local_pose(vector_pose) + + torch.testing.assert_close( + self.obj_group.get_local_pose(), + vector_pose, + atol=1e-5, + rtol=1e-5, + ) + + def test_body_data_exposes_mass_properties(self): + """Current and initialization-time properties use [env, object] layout.""" + data = self.obj_group.body_data + expected_prefix = (NUM_ARENAS, self.obj_group.num_objects) + + assert data.mass.shape == expected_prefix + assert data.inertia.shape == (*expected_prefix, 3) + assert data.com_pose.shape == (*expected_prefix, 7) + assert data.default_mass.shape == data.mass.shape + assert data.default_inertia.shape == data.inertia.shape + assert data.default_com_pose.shape == data.com_pose.shape + + def test_reset_restores_default_mass_properties(self): + """Partial reset restores Group mass properties only in selected envs.""" + data = self.obj_group.body_data + env_ids = [0, 1] + obj_ids = [0] + default_mass = data.default_mass[env_ids, :1].clone() + default_inertia = data.default_inertia[env_ids, :1].clone() + default_com_pose = data.default_com_pose[env_ids, :1].clone() + changed_mass = default_mass + 0.5 + changed_inertia = default_inertia * 1.25 + changed_com_pose = default_com_pose.clone() + changed_com_pose[..., 0] += 0.02 + changed_com_pose[..., 3:7] = torch.tensor( + [1.0, 2.0, 3.0, 4.0], device=self.sim.device + ) / torch.sqrt(torch.tensor(30.0, device=self.sim.device)) + + self.obj_group.set_mass(changed_mass, env_ids=env_ids, obj_ids=obj_ids) + self.obj_group.set_inertia( + changed_inertia, + env_ids=env_ids, + obj_ids=obj_ids, + ) + self.obj_group.set_com_pose( + changed_com_pose, + env_ids=env_ids, + obj_ids=obj_ids, + ) + + assert torch.allclose(data.default_mass[env_ids, :1], default_mass) + assert torch.allclose(data.default_inertia[env_ids, :1], default_inertia) + assert torch.allclose(data.default_com_pose[env_ids, :1], default_com_pose) + + self.obj_group.reset(env_ids=[env_ids[0]]) + mass_after_partial = self.obj_group.get_mass(env_ids=env_ids, obj_ids=obj_ids) + inertia_after_partial = self.obj_group.get_inertia( + env_ids=env_ids, obj_ids=obj_ids + ) + com_after_partial = self.obj_group.get_com_pose( + env_ids=env_ids, obj_ids=obj_ids + ) + inertia_atol = ( + NEWTON_INERTIA_ROUND_TRIP_ATOL if self.physics == "newton" else 1e-5 + ) + + assert torch.allclose(mass_after_partial[0], default_mass[0], atol=1e-5) + assert torch.allclose(mass_after_partial[1], changed_mass[1], atol=1e-5) + assert torch.allclose( + inertia_after_partial[0], + default_inertia[0], + atol=inertia_atol, + rtol=0.0, + ) + assert torch.allclose( + inertia_after_partial[1], + changed_inertia[1], + atol=inertia_atol, + rtol=0.0, + ) + assert torch.allclose(com_after_partial[0], default_com_pose[0], atol=1e-5) + assert torch.allclose(com_after_partial[1], changed_com_pose[1], atol=1e-5) + + self.obj_group.reset(env_ids=[env_ids[1]]) + assert torch.allclose( + self.obj_group.get_mass(env_ids=env_ids, obj_ids=obj_ids), + default_mass, + atol=1e-5, + ) + assert torch.allclose( + self.obj_group.get_inertia(env_ids=env_ids, obj_ids=obj_ids), + default_inertia, + atol=inertia_atol, + rtol=0.0, + ) + assert torch.allclose( + self.obj_group.get_com_pose(env_ids=env_ids, obj_ids=obj_ids), + default_com_pose, + atol=1e-5, + ) + def test_get_user_ids(self): """Test get_user_ids method.""" user_ids = self.obj_group.get_user_ids() @@ -156,14 +285,13 @@ def test_set_visible(self): def teardown_method(self): """Clean up resources after each test method.""" - self.sim.destroy() - import embodichain.lab.sim as om - - om.SimulationManager.flush_cleanup_queue() + self.sim.destroy(exit_process=False) self.__dict__.clear() import gc gc.collect() + SimulationManager.flush_cleanup_queue() + gc.collect() class TestRigidObjectGroupCPU(BaseRigidObjectGroupTest): @@ -171,12 +299,20 @@ def setup_method(self): self.setup_simulation("cpu") -@pytest.mark.skip(reason="Skipping CUDA tests temporarily") class TestRigidObjectGroupCUDA(BaseRigidObjectGroupTest): def setup_method(self): self.setup_simulation("cuda") +class TestRigidObjectGroupNewton(BaseRigidObjectGroupTest): + def setup_method(self): + self.setup_simulation("cuda", physics="newton") + + def teardown_method(self): + super().teardown_method() + _teardown_newton_physics() + + if __name__ == "__main__": # pytest.main(["-s", __file__]) test = TestRigidObjectGroupCPU() diff --git a/tests/sim/objects/test_robot.py b/tests/sim/objects/test_robot.py index f423f33bd..e1d5e3905 100644 --- a/tests/sim/objects/test_robot.py +++ b/tests/sim/objects/test_robot.py @@ -26,7 +26,9 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.objects import Articulation, Robot +from embodichain.lab.sim.objects.backends.newton import _default_mujoco_mimic_solref from embodichain.lab.sim.robots.dexforce_w1 import DexforceW1Cfg +from embodichain.lab.sim.cfg import physics_cfg_for_backend from embodichain.data import get_data_path # Define control parts @@ -51,6 +53,20 @@ ], } +W1_ACTIVE_DOF = 40 # Dexforce W1 v021 scalar active-DOF count. + + +@pytest.mark.no_sim +def test_default_mujoco_mimic_solref_preserves_damping_and_timestep_floor(): + np.testing.assert_allclose( + _default_mujoco_mimic_solref(physics_dt=0.01, num_substeps=10), + [2.0e-3, 1.0e1], + ) + np.testing.assert_allclose( + _default_mujoco_mimic_solref(physics_dt=1.0e-4, num_substeps=10), + [1.0e-4, 1.0e1], + ) + def test_get_qf_selects_control_part_joint_efforts(): full_qf = torch.tensor( @@ -93,11 +109,11 @@ def test_compute_fk_forwards_named_joint_state_to_articulation(): # Base test class for CPU and CUDA class BaseRobotTest: @classmethod - def setup_simulation(cls, sim_device): + def setup_simulation(cls, device): if hasattr(cls, "sim"): return # Set up simulation with specified device (CPU or CUDA) - config = SimulationManagerCfg(headless=True, sim_device=sim_device, num_envs=10) + config = SimulationManagerCfg(headless=True, device=device, num_envs=10) cls.sim = SimulationManager(config) cfg = DexforceW1Cfg.from_dict( @@ -108,10 +124,7 @@ def setup_simulation(cls, sim_device): ) cls.robot: Robot = cls.sim.add_robot(cfg=cfg) - - # Initialize GPU physics if needed - if sim_device == "cuda" and getattr(cls.sim, "is_use_gpu_physics", False): - cls.sim.init_gpu_physics() + cls.sim.prepare() def test_get_joint_ids(self): left_joint_ids = self.robot.get_joint_ids("left_arm") @@ -285,6 +298,52 @@ def test_mimic(self): len(right_eef_ids_without_mimic) == 6 ), f"Expected 6 right eef joint IDs without mimic, got {len(right_eef_ids_without_mimic)}" + def test_default_mimic_tracks_closed_hand_target(self): + """Keep W1 hand mimic constraints equally stiff on CPU and CUDA.""" + self.robot.reset() + open_target = torch.tensor( + [[0.0, 1.5, 0.0, 0.0, 0.0, 0.0]], + dtype=torch.float32, + device=self.sim.device, + ) + close_target = torch.tensor( + [[0.1, 1.5, 0.3, 0.2, 0.3, 0.3]], + dtype=torch.float32, + device=self.sim.device, + ) + for target in (open_target, close_target): + self.robot.set_qpos( + target.repeat(self.robot.num_instances, 1), name="right_eef" + ) + self.sim.update(step=100) + + qpos = self.robot.body_data.qpos + target_qpos = self.robot.body_data.target_qpos + right_eef_ids = self.robot.get_joint_ids("right_eef") + right_mimic_errors = [] + for mimic_id, parent_id, multiplier, offset in zip( + self.robot.mimic_ids, + self.robot.mimic_parents, + self.robot.mimic_multipliers, + self.robot.mimic_offsets, + strict=True, + ): + if not self.robot.joint_names[parent_id].startswith("RIGHT_HAND"): + continue + right_mimic_errors.append( + torch.abs( + qpos[:, mimic_id] - (qpos[:, parent_id] * multiplier + offset) + ) + ) + + assert torch.max(torch.stack(right_mimic_errors)).item() < 0.02 + assert ( + torch.max( + torch.abs(qpos[:, right_eef_ids] - target_qpos[:, right_eef_ids]) + ).item() + < 0.01 + ) + def test_setter_and_getter_with_control_part(self): left_arm_qpos = self.robot.get_qpos(name="left_arm") assert left_arm_qpos.shape == (10, 7) @@ -477,7 +536,7 @@ def test_robot_cfg_merge(self): cfg = deepcopy(self.robot.cfg) cfg_dict = { - "drive_pros": { + "joint_drive_props": { "max_effort": { "(LEFT|RIGHT)_HAND_(THUMB[12]|INDEX|MIDDLE|RING|PINKY)": 1.0, }, @@ -492,7 +551,7 @@ def test_robot_cfg_merge(self): cfg = merge_robot_cfg(cfg, cfg_dict) assert ( - cfg.drive_pros.max_effort[ + cfg.joint_drive_props.max_effort[ "(LEFT|RIGHT)_HAND_(THUMB[12]|INDEX|MIDDLE|RING|PINKY)" ] == 1.0 @@ -544,6 +603,201 @@ def setup_method(self): self.setup_simulation("cuda") +def _teardown_newton_physics() -> None: + from dexsim.engine.newton_physics import teardown_newton_physics + + teardown_newton_physics() + + +class TestRobotNewton: + """Focused Robot-on-Newton coverage (spawn, prepare, control surface). + + A robot is a URDF articulation; the Newton ``load_urdf`` patch builds a + NewtonArticulation. This exercises the add_robot -> prepare -> control-part + / qpos path end-to-end on Newton. It does NOT inherit the + full BaseRobotTest suite because rebuilding the (complex, mimic-jointed) + dexforce_w1 Newton model per test method is prohibitively slow; the + default/CUDA classes already cover the shared control-part/FK/IK logic. + """ + + def setup_method(self): + physics_cfg = physics_cfg_for_backend("newton") + physics_cfg.solver_cfg = { + "solver_type": "mujoco_warp", + "njmax": 8192, + "nconmax": 8192, + } + config = SimulationManagerCfg( + headless=True, device="cuda", num_envs=1, physics_cfg=physics_cfg + ) + self.sim = SimulationManager(config) + cfg = DexforceW1Cfg.from_dict({"uid": "dexforce_w1", "version": "v021"}) + cfg.init_qpos = [0.0001 * (index + 1) for index in range(W1_ACTIVE_DOF)] + self.robot: Robot = self.sim.add_robot(cfg=cfg) + self.sim.prepare() + + def teardown_method(self): + self.sim.destroy() + import embodichain.lab.sim as om + + om.SimulationManager.flush_cleanup_queue() + _teardown_newton_physics() + import gc + + gc.collect() + + def test_newton_robot_spawn_and_control(self): + """Robot spawns on Newton, prepares, and exposes a working control surface.""" + assert self.sim.is_newton_backend + assert self.robot.body_data.is_ready + assert self.robot.dof > 0 + + state_joint_names = self.robot.body_data.articulation_view.joint_names + assert self.robot.joint_names == state_joint_names + source_joint_names = self.robot._entities[0].get_actived_joint_names() + initial_qpos_by_name = dict( + zip(source_joint_names, self.robot.cfg.init_qpos, strict=True) + ) + mimic_relations = list( + zip( + self.robot.mimic_ids, + self.robot.mimic_parents, + self.robot.mimic_multipliers, + self.robot.mimic_offsets, + strict=True, + ) + ) + assert all( + state_joint_names[mimic_id].endswith("_PIP") + and "_HAND_" in state_joint_names[parent_id] + for mimic_id, parent_id, _, _ in mimic_relations + ) + initial_qpos = self.robot.body_data.qpos[0].detach().cpu().tolist() + assert dict(zip(state_joint_names, initial_qpos, strict=True)) == pytest.approx( + initial_qpos_by_name + ) + + binding = self.robot._entities[0]._physics_binding + model = binding._runtime.model + runtime_joints = {joint.name: joint for joint in binding.joints} + mimic_joint0 = np.asarray(model.constraint_mimic_joint0.numpy()).reshape(-1) + mimic_joint1 = np.asarray(model.constraint_mimic_joint1.numpy()).reshape(-1) + row_by_pair = { + (int(child), int(parent)): row + for row, (child, parent) in enumerate( + zip(mimic_joint0, mimic_joint1, strict=True) + ) + } + constraint_rows = [] + for mimic_id, parent_id, _, _ in mimic_relations: + child = runtime_joints[state_joint_names[mimic_id]] + parent = runtime_joints[state_joint_names[parent_id]] + constraint_rows.append( + row_by_pair[(int(child.joint_id), int(parent.joint_id))] + ) + + solver = binding._runtime.solver + mapping = np.asarray(solver.mjc_eq_to_newton_mimic.numpy()) + selected_eq = np.isin(mapping, np.asarray(constraint_rows, dtype=np.int32)) + assert int(selected_eq.sum()) == len(mimic_relations) + eq_solref = np.asarray(solver.mjw_model.eq_solref.numpy()) + np.testing.assert_allclose( + eq_solref[selected_eq], + np.broadcast_to([2.0e-3, 1.0e1], (len(mimic_relations), 2)), + ) + target_ke = np.asarray(model.joint_target_ke.numpy()) + target_kd = np.asarray(model.joint_target_kd.numpy()) + target_mode = np.asarray(model.joint_target_mode.numpy()) + eq_active = np.asarray(solver.mjw_data.eq_active.numpy()) + assert np.all(eq_active[selected_eq]) + for mimic_id, parent_id, _, _ in mimic_relations: + child = runtime_joints[state_joint_names[mimic_id]] + parent = runtime_joints[state_joint_names[parent_id]] + assert target_ke[child.qd_start] == pytest.approx( + target_ke[parent.qd_start] * 1.0e-2 + ) + assert target_kd[child.qd_start] == pytest.approx( + target_kd[parent.qd_start] * 1.0e-2 + ) + assert target_mode[child.qd_start] == target_mode[parent.qd_start] + + # This physical check covers both hands and keeps the native coupled + # constraints bounded under the W1's self-contacts. Default can also + # deflect these compliant joints by several tenths of a radian. + self.sim.update(step=100) + settled_qpos = self.robot.body_data.qpos + settled_errors = [] + for mimic_id, parent_id, multiplier, offset in mimic_relations: + settled_errors.append( + torch.abs( + settled_qpos[:, mimic_id] + - (settled_qpos[:, parent_id] * multiplier + offset) + ) + ) + assert torch.max(torch.stack(settled_errors)).item() < 0.5 + + left_ids = self.robot.get_joint_ids("left_arm") + right_ids = self.robot.get_joint_ids("right_arm") + assert len(left_ids) > 0 and len(right_ids) > 0 + assert [ + state_joint_names[index] for index in left_ids + ] == self.robot.control_parts["left_arm"] + assert [ + state_joint_names[index] for index in right_ids + ] == self.robot.control_parts["right_arm"] + right_eef_ids = self.robot.get_joint_ids("right_eef") + assert [ + state_joint_names[index] for index in right_eef_ids + ] == self.robot.control_parts["right_eef"] + + right_qpos_limits = self.robot.get_qpos_limits(name="right_arm") + requested_target = torch.full( + (1, len(right_ids)), 0.1, dtype=torch.float32, device=self.sim.device + ) + expected_target = requested_target.clamp( + right_qpos_limits[..., 0], right_qpos_limits[..., 1] + ) + self.robot.set_qpos(requested_target, name="right_arm") + torch.testing.assert_close( + self.robot.body_data.target_qpos[:, right_ids], expected_target + ) + + hand_target = torch.tensor( + [[0.1, 1.0, 0.2, 0.3, 0.4, 0.5]], + dtype=torch.float32, + device=self.sim.device, + ) + self.robot.set_qpos(hand_target, name="right_eef") + target_qpos = self.robot.body_data.target_qpos + torch.testing.assert_close(target_qpos[:, right_eef_ids], hand_target) + for mimic_id, parent_id, multiplier, offset in mimic_relations: + torch.testing.assert_close( + target_qpos[:, mimic_id], + target_qpos[:, parent_id] * multiplier + offset, + ) + hand_velocity_target = torch.tensor( + [[0.05, 0.1, 0.15, 0.2, 0.25, 0.3]], + dtype=torch.float32, + device=self.sim.device, + ) + self.robot.set_qvel(hand_velocity_target, name="right_eef") + target_qvel = self.robot.body_data.target_qvel + torch.testing.assert_close(target_qvel[:, right_eef_ids], hand_velocity_target) + for mimic_id, parent_id, multiplier, _ in mimic_relations: + torch.testing.assert_close( + target_qvel[:, mimic_id], + target_qvel[:, parent_id] * multiplier, + ) + self.robot.set_qvel(torch.zeros_like(hand_velocity_target), name="right_eef") + + # State round-trip via the Newton articulation view. + qpos = torch.zeros( + (1, self.robot.dof), dtype=torch.float32, device=self.sim.device + ) + self.robot.set_qpos(qpos, env_ids=None, target=False) + assert torch.allclose(self.robot.body_data.qpos, qpos, atol=1e-5) + + if __name__ == "__main__": # Run tests directly test_cpu = TestRobotCUDA() diff --git a/tests/sim/objects/test_robot_cfg.py b/tests/sim/objects/test_robot_cfg.py index 6a81f9873..74d95fae5 100644 --- a/tests/sim/objects/test_robot_cfg.py +++ b/tests/sim/objects/test_robot_cfg.py @@ -21,7 +21,10 @@ import pytest from embodichain.lab.sim.cfg import ( + ArticulationRootPropertiesCfg, + CollisionPropertiesCfg, JointDrivePropertiesCfg, + RigidBodyPhysicsCfg, RobotCfg, ) from embodichain.lab.sim.motion.workspace import RobotWorkspaceCfg @@ -64,11 +67,15 @@ def resolve(path): def test_dexforce_w1_roundtrip(): cfg = DexforceW1Cfg.from_dict({"uid": "dexforce_w1", "version": "v021"}) + assert type(cfg.root_props) is ArticulationRootPropertiesCfg + assert cfg.root_props.min_position_iters == 32 + assert cfg.root_props.min_velocity_iters == 8 d = cfg.to_dict() assert d["uid"] == "dexforce_w1" cfg2 = DexforceW1Cfg.from_dict(d) assert cfg2.uid == "dexforce_w1" assert cfg2.version == DexforceW1Version.V021 + assert type(cfg2.root_props) is ArticulationRootPropertiesCfg def test_dexforce_w1_solver_cfg_is_srs_and_set_once(): @@ -409,7 +416,7 @@ def _build_defaults(self, init_dict=None): self.uid = "roundtrip" self.variant = _RoundTripVariant(init_dict.get("variant", "a")) self.control_parts = {"arm": ["J1", "J2"]} - self.drive_pros = JointDrivePropertiesCfg( + self.joint_drive_props = JointDrivePropertiesCfg( stiffness={"J[1-2]": 1e4}, damping={"J[1-2]": 1e3} ) @@ -426,10 +433,12 @@ def test_robotcfg_to_dict_roundtrip(): assert cfg2.uid == "roundtrip" assert cfg2.variant == _RoundTripVariant.B assert cfg2.control_parts == {"arm": ["J1", "J2"]} - assert cfg2.drive_pros.stiffness == {"J[1-2]": 1e4} + assert cfg2.joint_drive_props.stiffness == {"J[1-2]": 1e4} from embodichain.lab.sim.robots.cobotmagic import CobotMagicCfg +from embodichain.lab.sim.robots.franka_panda import FrankaPandaCfg +from embodichain.lab.sim.robots.ur_robot import URRobotCfg from embodichain.lab.sim.motion.solvers import OPWSolverCfg @@ -444,6 +453,13 @@ def test_cobotmagic_from_dict_and_roundtrip(): } assert isinstance(cfg.solver_cfg["left_arm"], OPWSolverCfg) assert isinstance(cfg.solver_cfg["right_arm"], OPWSolverCfg) + assert isinstance(cfg.attrs, RigidBodyPhysicsCfg) + assert type(cfg.attrs.collision_props) is CollisionPropertiesCfg + assert cfg.attrs.collision_props.contact_offset == pytest.approx(0.001) + assert cfg.attrs.collision_props.rest_offset == pytest.approx(0.0) + assert type(cfg.root_props) is ArticulationRootPropertiesCfg + assert cfg.root_props.min_position_iters == 8 + assert cfg.root_props.min_velocity_iters == 2 d = cfg.to_dict() assert d["uid"] == "CobotMagic" @@ -453,6 +469,27 @@ def test_cobotmagic_from_dict_and_roundtrip(): assert isinstance(cfg2.solver_cfg["left_arm"], OPWSolverCfg) +@pytest.mark.parametrize( + ("cfg_type", "init_dict"), + [ + (CobotMagicCfg, {}), + (FrankaPandaCfg, {}), + (URRobotCfg, {}), + (DexforceW1Cfg, {}), + ], +) +def test_specified_robots_use_portable_joint_drive_semantics( + cfg_type: type[RobotCfg], + init_dict: dict, +) -> None: + cfg = cfg_type.from_dict(init_dict) + + assert type(cfg.joint_drive_props) is JointDrivePropertiesCfg + assert cfg.joint_drive_props.drive_type == "force" + assert cfg.joint_drive_props.target_mode is None + assert cfg.joint_drive_props._resolve_modes() == ("position_velocity", "force") + + def test_robotcfg_save_to_file(tmp_path): cfg = _RoundTripCfg.from_dict({"variant": "b"}) fp = tmp_path / "cfg.json" @@ -523,7 +560,6 @@ def test_cobotmagic_pk_dof_matches_control_parts(): # URRobotCfg -- UR family (ur3 / ur3e / ur5 / ur5e / ur10 / ur10e) # --------------------------------------------------------------------------- # -from embodichain.lab.sim.robots.ur_robot import URRobotCfg from embodichain.lab.sim.motion.solvers import URSolverCfg UR_TYPES = ["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e"] @@ -562,7 +598,7 @@ def test_ur_robot_max_effort_scales_with_size(): ur3 = URRobotCfg.from_dict({"robot_type": "ur3"}) ur5 = URRobotCfg.from_dict({"robot_type": "ur5"}) ur10 = URRobotCfg.from_dict({"robot_type": "ur10"}) - eff = lambda c: c.drive_pros.max_effort["arm"] # noqa: E731 + eff = lambda c: c.joint_drive_props.max_effort["arm"] # noqa: E731 assert eff(ur3) < eff(ur5) < eff(ur10) diff --git a/tests/sim/objects/test_scene_backend.py b/tests/sim/objects/test_scene_backend.py new file mode 100644 index 000000000..f89d3821e --- /dev/null +++ b/tests/sim/objects/test_scene_backend.py @@ -0,0 +1,690 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import importlib.util +import inspect +from types import SimpleNamespace + +import pytest +import torch +from dexsim.scene import ArticulationBatch, RigidBodyBatch, Scene + +import embodichain.lab.sim.objects.backends as backends +import embodichain.lab.sim.objects.backends.newton as newton_backend +from embodichain.lab.sim.objects.articulation import Articulation, ArticulationData +from embodichain.lab.sim.objects.backends.scene import ( + SceneArticulationView, + SceneRigidBodyView, + _batch_pose, + _embodichain_pose, +) +from embodichain.lab.sim.objects.rigid_object import RigidBodyData, RigidObject + +pytestmark = pytest.mark.no_sim + + +def test_deprecated_batch_adapters_are_removed() -> None: + deprecated_exports = { + "DefaultArticulationView", + "DefaultRigidBodyView", + "NewtonArticulationView", + "NewtonRigidBodyView", + "SpawnArticulationView", + "SpawnRigidBodyView", + "apply_collision_filter_for_entities", + "apply_collision_filter_for_envs", + } + + assert deprecated_exports.isdisjoint(backends.__all__) + assert all(not hasattr(backends, name) for name in deprecated_exports) + assert ( + importlib.util.find_spec("embodichain.lab.sim.objects.backends.spawn") is None + ) + assert ( + importlib.util.find_spec("embodichain.lab.sim.objects.backends.default") is None + ) + + +def test_scene_views_match_installed_dexsim_batch_surface() -> None: + rigid_methods = { + "select", + "apply_pose", + "fetch_pose", + "apply_com_local_pose", + "fetch_com_local_pose", + "apply_linear_velocity", + "fetch_linear_velocity", + "apply_angular_velocity", + "fetch_angular_velocity", + "fetch_linear_acceleration", + "fetch_angular_acceleration", + "apply_force", + "apply_torque", + "apply_mass", + "fetch_mass", + "apply_inertia_diagonal", + "fetch_inertia_diagonal", + "apply_friction", + "fetch_friction", + "apply_restitution", + "fetch_restitution", + "apply_contact_offset", + "fetch_contact_offset", + "apply_damping", + "fetch_damping", + "apply_collision_filter", + "fetch_collision_filter", + } + articulation_methods = { + "select", + "apply_root_pose", + "fetch_root_pose", + "fetch_root_linear_velocity", + "fetch_root_angular_velocity", + "apply_joint_position", + "fetch_joint_position", + "apply_joint_target_position", + "fetch_joint_target_position", + "apply_joint_velocity", + "fetch_joint_velocity", + "apply_joint_target_velocity", + "fetch_joint_target_velocity", + "apply_joint_force", + "fetch_joint_force", + "fetch_joint_acceleration", + "fetch_link_pose", + "fetch_link_linear_velocity", + "fetch_link_angular_velocity", + "compute_kinematics", + } + articulation_metadata = { + "dof_counts", + "link_counts", + "joint_names_per_articulation", + "link_names_per_articulation", + "joint_layouts_per_articulation", + "dof_width", + "link_width", + } + + assert rigid_methods <= set(dir(RigidBodyBatch)) + assert articulation_methods <= set(dir(ArticulationBatch)) + assert articulation_metadata <= set(dir(ArticulationBatch)) + assert callable(Scene.create_rigid_body_batch) + assert callable(Scene.create_articulation_batch) + for method_name in ( + "apply_joint_position", + "apply_joint_target_position", + "apply_joint_velocity", + "apply_joint_target_velocity", + "apply_joint_force", + ): + assert ( + "dof_ids" + in inspect.signature(getattr(ArticulationBatch, method_name)).parameters + ) + + +def test_scene_pose_adapters_preserve_embodichain_xyzw_order() -> None: + pose = torch.tensor([[1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9]]) + expected_batch = torch.tensor([[0.1, 0.2, 0.3, 0.9, 1.0, 2.0, 3.0]]) + + torch.testing.assert_close(_batch_pose(pose), expected_batch) + torch.testing.assert_close(_embodichain_pose(expected_batch), pose) + + +class _SelectedRigidBatch: + def __init__(self, owner: _RigidBatch, rows: torch.Tensor) -> None: + self.owner = owner + self.rows = rows + + def apply_force(self, values: torch.Tensor) -> int: + self.owner.force[self.rows] = values + return len(self.rows) + + def apply_pose(self, values: torch.Tensor) -> int: + self.owner.pose[self.rows] = values + return len(self.rows) + + def apply_linear_velocity(self, values: torch.Tensor) -> int: + self.owner.linear_velocity[self.rows] = values + return len(self.rows) + + def apply_angular_velocity(self, values: torch.Tensor) -> int: + self.owner.angular_velocity[self.rows] = values + return len(self.rows) + + def apply_friction(self, values: torch.Tensor) -> int: + self.owner.friction[self.rows] = values + return len(self.rows) + + def apply_collision_filter(self, values: torch.Tensor) -> int: + self.owner.collision_filter[self.rows] = values + return len(self.rows) + + def fetch_friction(self, out: torch.Tensor) -> int: + out.copy_(self.owner.friction[self.rows]) + return len(self.rows) + + def fetch_collision_filter(self, out: torch.Tensor) -> int: + out.copy_(self.owner.collision_filter[self.rows]) + return len(self.rows) + + +class _RigidBatch: + def __init__(self) -> None: + self.force = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]]) + self.pose = torch.tensor( + [ + [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0, 2.0, 0.0, 0.0], + ] + ) + self.friction = torch.tensor([[0.1], [0.2], [0.3]]) + self.collision_filter = torch.zeros((3, 4), dtype=torch.int32) + self.linear_velocity = torch.zeros((3, 3)) + self.angular_velocity = torch.zeros((3, 3)) + self.selections: list[tuple[int, ...]] = [] + + def __len__(self) -> int: + return len(self.force) + + def select(self, rows: torch.Tensor) -> _SelectedRigidBatch: + selected = rows.detach().cpu().to(dtype=torch.long) + self.selections.append(tuple(selected.tolist())) + return _SelectedRigidBatch(self, selected) + + def apply_collision_filter(self, values: torch.Tensor) -> int: + self.collision_filter.copy_(values.to(dtype=self.collision_filter.dtype)) + return len(self) + + def fetch_friction(self, out: torch.Tensor) -> int: + out.copy_(self.friction) + return len(self) + + def fetch_collision_filter(self, out: torch.Tensor) -> int: + out.copy_(self.collision_filter) + return len(self) + + +class _SelectedArticulationBatch: + def __init__(self, owner: _ArticulationBatch, rows: torch.Tensor) -> None: + self.owner = owner + self.rows = rows + + def apply_joint_force( + self, + values: torch.Tensor, + *, + dof_ids: torch.Tensor, + ) -> int: + columns = dof_ids.detach().cpu().to(dtype=torch.long) + self.owner.force[self.rows[:, None], columns] = values + self.owner.last_dof_ids = tuple(columns.tolist()) + return len(self.rows) + + def fetch_root_pose(self, out: torch.Tensor) -> int: + self.owner.root_pose_fetch_rows.append(tuple(self.rows.tolist())) + out.copy_(self.owner.root_pose[self.rows]) + return len(self.rows) + + def apply_root_pose(self, values: torch.Tensor) -> int: + self.owner.root_pose_apply_rows.append(tuple(self.rows.tolist())) + self.owner.root_pose[self.rows] = values + return len(self.rows) + + +class _ArticulationBatch: + def __init__(self) -> None: + layouts = tuple( + SimpleNamespace(name=f"joint_{index}", dof_start=index, dof_count=1) + for index in range(3) + ) + self.dof_counts = (3, 3) + self.link_counts = (1, 1) + self.joint_names_per_articulation = (("joint_0", "joint_1", "joint_2"),) * 2 + self.link_names_per_articulation = (("root",),) * 2 + self.joint_layouts_per_articulation = (layouts,) * 2 + self.dof_width = 3 + self.link_width = 1 + self.force = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]) + # Scene articulation batches use xyzw + xyz layout. + self.root_pose = torch.tensor( + [ + [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0], + [0.0, 0.0, 0.0, 1.0, 2.0, 0.0, 1.0], + ] + ) + self.last_dof_ids: tuple[int, ...] | None = None + self.root_pose_fetch_rows: list[tuple[int, ...]] = [] + self.root_pose_apply_rows: list[tuple[int, ...]] = [] + self.selections: list[tuple[int, ...]] = [] + + def __len__(self) -> int: + return len(self.force) + + def select(self, rows: torch.Tensor) -> _SelectedArticulationBatch: + selected = rows.detach().cpu().to(dtype=torch.long) + self.selections.append(tuple(selected.tolist())) + return _SelectedArticulationBatch(self, selected) + + +class _Scene(Scene): + def __init__(self, backend: str = "newton") -> None: + self.backend = backend + self._topology_revision = 0 + self.rigid_batch = _RigidBatch() + self.articulation_batch = _ArticulationBatch() + self.rigid_batch_objects: list[object] | None = None + self.articulation_batch_objects: list[object] | None = None + + def create_rigid_body_batch(self, objects: list[object]) -> _RigidBatch: + self.rigid_batch_objects = objects + return self.rigid_batch + + def create_articulation_batch( + self, + articulations: list[object], + ) -> _ArticulationBatch: + self.articulation_batch_objects = articulations + return self.articulation_batch + + +class _ArticulationEntity: + def get_joint_position_limits(self) -> list[list[float]]: + return [[-1.0, 1.0]] * 3 + + def get_joint_velocity_limit(self) -> list[float]: + return [2.0] * 3 + + def get_joint_effort_limit(self) -> list[float]: + return [3.0] * 3 + + +def test_is_newton_scene_requires_current_scene_batch_api() -> None: + assert newton_backend.is_newton_scene(_Scene("newton")) is True + assert newton_backend.is_newton_scene(_Scene("dexsim")) is False + assert newton_backend.is_newton_scene(SimpleNamespace(backend="newton")) is False + + +@pytest.mark.parametrize("backend", ["dexsim", "newton"]) +def test_rigid_body_data_uses_scene_view(backend: str) -> None: + scene = _Scene(backend) + entities = [object(), object(), object()] + + data = RigidBodyData(entities, scene, torch.device("cpu")) + + assert isinstance(data.body_view, SceneRigidBodyView) + assert data.is_newton_backend is (backend == "newton") + assert scene.rigid_batch_objects == entities + + +def test_rigid_body_data_rejects_raw_physics_scene_path() -> None: + with pytest.raises(TypeError, match="requires a finalized DexSim Scene"): + RigidBodyData([object()], SimpleNamespace(), torch.device("cpu")) + + +@pytest.mark.parametrize("object_type", [RigidObject, Articulation]) +def test_materialized_object_construction_requires_scene(object_type: type) -> None: + with pytest.raises(TypeError, match="requires a finalized DexSim Scene"): + object_type(SimpleNamespace(), [object()]) + + +@pytest.mark.parametrize("backend", ["dexsim", "newton"]) +def test_articulation_data_uses_scene_view(backend: str) -> None: + scene = _Scene(backend) + entities = [_ArticulationEntity(), _ArticulationEntity()] + + data = ArticulationData(entities, scene, torch.device("cpu")) + + assert isinstance(data.articulation_view, SceneArticulationView) + assert data.is_newton_backend is (backend == "newton") + assert scene.articulation_batch_objects == entities + + +def test_articulation_data_rejects_raw_physics_scene_path() -> None: + with pytest.raises(TypeError, match="requires a finalized DexSim Scene"): + ArticulationData( + [_ArticulationEntity()], + SimpleNamespace(), + torch.device("cpu"), + ) + + +def test_rigid_partial_writes_delegate_to_selected_batch() -> None: + batch = _RigidBatch() + view = SceneRigidBodyView( + SimpleNamespace(backend="newton"), + batch, + torch.device("cpu"), + ) + + view.apply_force(torch.tensor([[10.0, 20.0, 30.0]]), torch.tensor([1])) + view.apply_friction(torch.tensor([[0.9]]), torch.tensor([2])) + + assert torch.equal( + batch.force, + torch.tensor([[1.0, 2.0, 3.0], [10.0, 20.0, 30.0], [7.0, 8.0, 9.0]]), + ) + assert torch.equal(batch.friction, torch.tensor([[0.1], [0.2], [0.9]])) + assert batch.selections == [(1,), (2,)] + + +def test_rigid_partial_fetch_reads_only_selected_batch() -> None: + batch = _RigidBatch() + view = SceneRigidBodyView( + SimpleNamespace(backend="dexsim"), + batch, + torch.device("cpu"), + ) + out = torch.empty((2, 1)) + + view.fetch_friction(out, torch.tensor([2, 0])) + + assert torch.equal(out, torch.tensor([[0.3], [0.1]])) + assert batch.selections == [(2, 0)] + + +def test_rigid_full_fetch_uses_original_batch() -> None: + batch = _RigidBatch() + view = SceneRigidBodyView( + SimpleNamespace(backend="dexsim"), + batch, + torch.device("cpu"), + ) + out = torch.empty((3, 1)) + + view.fetch_friction(out) + + assert torch.equal(out, batch.friction) + assert batch.selections == [] + + +def test_rigid_batch_failure_status_is_not_silently_ignored() -> None: + batch = _RigidBatch() + view = SceneRigidBodyView( + SimpleNamespace(backend="dexsim"), + batch, + torch.device("cpu"), + ) + selected = batch.select(torch.tensor([0])) + selected.fetch_friction = lambda _out: -2 + batch.select = lambda _rows: selected + + with pytest.raises(RuntimeError, match="fetch_friction.*status -2"): + view.fetch_friction(torch.empty((1, 1)), torch.tensor([0])) + + +def test_newton_rigid_pose_write_synchronizes_free_joint_state(monkeypatch) -> None: + batch = _RigidBatch() + current_state = object() + other_state = object() + runtime = SimpleNamespace( + model=object(), + current_state=current_state, + other_state=other_state, + ) + batch._binding = SimpleNamespace( + _runtime=runtime, + _indices=torch.tensor([10, 11, 12]), + ) + synchronized_states: list[tuple[object, object]] = [] + created_body_ids: list[tuple[int, ...]] = [] + + class _StateSync: + def synchronize(self, states: tuple[object, object]) -> None: + synchronized_states.append(states) + + def _create_state_sync(_model: object, body_ids: list[int]) -> _StateSync: + created_body_ids.append(tuple(body_ids)) + return _StateSync() + + monkeypatch.setattr( + newton_backend, + "_create_newton_standalone_state_sync", + _create_state_sync, + ) + view = SceneRigidBodyView( + SimpleNamespace(backend="newton", topology_revision=3), + batch, + torch.device("cpu"), + ) + + view.apply_pose( + torch.tensor([[4.0, 5.0, 6.0, 0.0, 0.0, 0.0, 1.0]]), + torch.tensor([1]), + ) + view.apply_pose( + torch.tensor([[7.0, 8.0, 9.0, 0.0, 0.0, 0.0, 1.0]]), + torch.tensor([2]), + ) + + assert created_body_ids == [(10, 11, 12)] + assert synchronized_states == [ + (current_state, other_state), + (current_state, other_state), + ] + assert torch.equal( + batch.pose[1:], + torch.tensor( + [ + [0.0, 0.0, 0.0, 1.0, 4.0, 5.0, 6.0], + [0.0, 0.0, 0.0, 1.0, 7.0, 8.0, 9.0], + ] + ), + ) + + +def test_newton_rigid_velocity_writes_synchronize_free_joint_state( + monkeypatch, +) -> None: + batch = _RigidBatch() + current_state = object() + other_state = object() + runtime = SimpleNamespace( + model=object(), + current_state=current_state, + other_state=other_state, + ) + batch._binding = SimpleNamespace( + _runtime=runtime, + _indices=torch.tensor([10, 11, 12]), + ) + synchronized_states: list[tuple[object, object]] = [] + created_body_ids: list[tuple[int, ...]] = [] + + class _StateSync: + def synchronize(self, states: tuple[object, object]) -> None: + synchronized_states.append(states) + + def _create_state_sync(_model: object, body_ids: list[int]) -> _StateSync: + created_body_ids.append(tuple(body_ids)) + return _StateSync() + + monkeypatch.setattr( + newton_backend, + "_create_newton_standalone_state_sync", + _create_state_sync, + ) + view = SceneRigidBodyView( + SimpleNamespace(backend="newton", topology_revision=3), + batch, + torch.device("cpu"), + ) + + view.apply_linear_velocity( + torch.tensor([[1.0, 2.0, 3.0]]), + torch.tensor([1]), + ) + view.apply_angular_velocity( + torch.tensor([[4.0, 5.0, 6.0]]), + torch.tensor([2]), + ) + + assert created_body_ids == [(10, 11, 12)] + assert synchronized_states == [ + (current_state, other_state), + (current_state, other_state), + ] + assert torch.equal(batch.linear_velocity[1], torch.tensor([1.0, 2.0, 3.0])) + assert torch.equal(batch.angular_velocity[2], torch.tensor([4.0, 5.0, 6.0])) + + +def test_articulation_partial_force_preserves_other_rows_and_dofs() -> None: + batch = _ArticulationBatch() + view = SceneArticulationView( + SimpleNamespace(backend="newton"), + batch, + torch.device("cpu"), + ) + + view.apply_qf( + torch.tensor([[50.0]]), + env_ids=torch.tensor([1]), + joint_ids=torch.tensor([1]), + ) + + assert torch.equal( + batch.force, + torch.tensor([[1.0, 2.0, 3.0], [4.0, 50.0, 6.0]]), + ) + assert batch.selections == [(1,)] + assert batch.last_dof_ids == (1,) + + +def test_newton_idempotent_root_pose_write_is_skipped() -> None: + batch = _ArticulationBatch() + view = SceneArticulationView( + SimpleNamespace(backend="newton"), + batch, + torch.device("cpu"), + ) + current_pose = torch.tensor( + [ + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + [2.0, 0.0, 1.0, 0.0, 0.0, 0.0, -1.0], + ] + ) + + view.apply_root_pose(current_pose, env_ids=torch.tensor([0, 1])) + + assert batch.root_pose_fetch_rows == [(0, 1)] + assert batch.root_pose_apply_rows == [] + + +def test_newton_root_pose_write_keeps_only_changed_rows() -> None: + batch = _ArticulationBatch() + view = SceneArticulationView( + SimpleNamespace(backend="newton"), + batch, + torch.device("cpu"), + ) + target_pose = torch.tensor( + [ + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + [3.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + ] + ) + + view.apply_root_pose(target_pose, env_ids=torch.tensor([0, 1])) + + assert batch.root_pose_fetch_rows == [(0, 1)] + assert batch.root_pose_apply_rows == [(1,)] + assert torch.equal( + batch.root_pose, + torch.tensor( + [ + [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0], + [0.0, 0.0, 0.0, 1.0, 3.0, 0.0, 1.0], + ] + ), + ) + + +def test_newton_root_pose_rejects_mismatched_selection_shape() -> None: + view = SceneArticulationView( + SimpleNamespace(backend="newton"), + _ArticulationBatch(), + torch.device("cpu"), + ) + + with pytest.raises(ValueError, match="Expected selected data shape \\(2, 7\\)"): + view.apply_root_pose( + torch.tensor([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]]), + env_ids=torch.tensor([0, 1]), + ) + + +def test_articulation_joint_selection_rejects_negative_indices() -> None: + view = SceneArticulationView( + SimpleNamespace(backend="dexsim"), + _ArticulationBatch(), + torch.device("cpu"), + ) + + with pytest.raises(IndexError, match="outside \\[0, 3\\)"): + view.apply_qf( + torch.tensor([[1.0]]), + env_ids=torch.tensor([0]), + joint_ids=torch.tensor([-1]), + ) + + +def test_scene_views_use_current_scene_batch_factories() -> None: + scene = _Scene() + rigid_entities = [object(), object(), object()] + articulation_entities = [object(), object()] + + rigid_view = SceneRigidBodyView.from_entities( + scene, rigid_entities, torch.device("cpu") + ) + articulation_view = SceneArticulationView.from_entities( + scene, articulation_entities, torch.device("cpu") + ) + + rigid_view.apply_friction(torch.tensor([[0.8]]), torch.tensor([1])) + articulation_view.apply_qf( + torch.tensor([[20.0]]), + env_ids=torch.tensor([1]), + joint_ids=torch.tensor([2]), + ) + + assert scene.rigid_batch_objects == rigid_entities + assert scene.articulation_batch_objects == articulation_entities + assert torch.equal(scene.rigid_batch.friction, torch.tensor([[0.1], [0.8], [0.3]])) + assert torch.equal( + scene.articulation_batch.force, + torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 20.0]]), + ) + + +def test_scene_rigid_collision_filter_delegates_to_batch() -> None: + scene = _Scene() + view = SceneRigidBodyView.from_entities( + scene, [object(), object(), object()], torch.device("cpu") + ) + expected = torch.tensor( + [[0, 1, 0, 0], [1, 1, 0, 0], [2, 1, 0, 0]], dtype=torch.int32 + ) + + view.apply_collision_filter(expected, torch.tensor([0, 1, 2])) + actual = torch.empty_like(expected) + view.fetch_collision_filter(actual) + + assert torch.equal(actual, expected) diff --git a/tests/sim/objects/test_soft_object.py b/tests/sim/objects/test_soft_object.py index d7334bb9e..084106dbf 100644 --- a/tests/sim/objects/test_soft_object.py +++ b/tests/sim/objects/test_soft_object.py @@ -20,15 +20,16 @@ from dexsim.utility.path import get_resources_data_path from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.cfg import ( - RenderCfg, - SoftbodyVoxelAttributesCfg, - SoftbodyPhysicalAttributesCfg, + NewtonPhysicsCfg, + VolumeDeformableMeshingCfg, + VolumeDeformablePhysicsCfg, ) from embodichain.lab.sim.shapes import MeshCfg from embodichain.lab.sim.objects import ( - SoftBodyData, - SoftObject, - SoftObjectCfg, + DeformableObject, + DeformableObjectData, + VolumeDeformableObject, + VolumeDeformableObjectCfg, ) import pytest import torch @@ -36,17 +37,6 @@ COW_PATH = get_resources_data_path("Model", "cow", "cow.obj") -def test_degenerate_soft_body_surface_is_empty() -> None: - """Degenerate collision geometry does not prevent visualization startup.""" - data = object.__new__(SoftBodyData) - data.device = torch.device("cpu") - data._rest_position_buffer = torch.zeros((1, 3, 4), dtype=torch.float32) - - triangles = data.collision_surface_triangles - - assert triangles.shape == (0, 3) - - class BaseSoftObjectTest: def setup_simulation(self): sim_cfg = SimulationManagerCfg( @@ -54,9 +44,14 @@ def setup_simulation(self): height=1080, headless=True, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device="cuda", - num_envs=4, + device="cuda", + # DexSim 0.5 currently changes the cow render topology while + # cloning it; keep the functional volume test single-instance. + num_envs=1, arena_space=3.0, + physics_cfg=NewtonPhysicsCfg( + solver_cfg={"solver_type": "vbd"}, + ), ) # Create the simulation instance @@ -65,32 +60,30 @@ def setup_simulation(self): assert os.path.isfile(COW_PATH) # Enable manual physics update for precise control - self.num_envs = 4 + self.num_envs = 1 # add softbody to the scene - self.cow: SoftObject = self.sim.add_soft_object( - cfg=SoftObjectCfg( + self.cow: VolumeDeformableObject = self.sim.add_deformable_object( + cfg=VolumeDeformableObjectCfg( uid="cow", shape=MeshCfg( fpath=get_resources_data_path("Model", "cow", "cow.obj"), ), init_pos=[0.0, 0.0, 3.0], - voxel_attr=SoftbodyVoxelAttributesCfg( + meshing=VolumeDeformableMeshingCfg( simulation_mesh_resolution=8, - maximal_edge_length=0.5, ), - physical_attr=SoftbodyPhysicalAttributesCfg( + attrs=VolumeDeformablePhysicsCfg( youngs=1e6, poissons=0.45, density=100, - dynamic_friction=0.1, - min_position_iters=30, + elasticity_damping=0.1, ), ), ) + self.sim.prepare() def test_run_simulation(self): - self.sim.init_gpu_physics() for _ in range(100): self.sim.update(step=1) self.cow.reset() @@ -99,19 +92,62 @@ def test_run_simulation(self): def test_get_deformable_mesh_geometry(self): """Test current collision vertices and matching surface triangles.""" - self.sim.init_gpu_physics() - vertices = self.cow.get_current_collision_vertices() + vertices = self.cow.data.nodal_pos_w triangles = self.cow.get_collision_surface_triangles(env_ids=[0]) assert vertices.ndim == 3 and vertices.shape[0] == self.sim.num_envs assert triangles.ndim == 3 and triangles.shape[0] == 1 assert int(triangles.max()) < vertices.shape[1] + def test_set_local_pose_updates_selected_particle_batch(self): + """Setting one instance pose writes only its packed simulation nodes.""" + before = self.cow.data.nodal_pos_w + translation = torch.tensor([0.5, 0.0, 0.0], device=self.cow.device) + pose = torch.eye( + 4, + dtype=torch.float32, + device=self.cow.device, + ).unsqueeze(0) + pose[:, :3, 3] = ( + torch.as_tensor( + self.cow.cfg.init_pos, + dtype=torch.float32, + device=self.cow.device, + ) + + translation + ) + + self.cow.set_local_pose(pose, env_ids=[0]) + after = self.cow.data.nodal_pos_w + + torch.testing.assert_close(after[0], before[0] + translation) + torch.testing.assert_close(after[1:], before[1:]) + + def test_unified_deformable_contract(self): + assert isinstance(self.cow, DeformableObject) + assert isinstance(self.cow, VolumeDeformableObject) + assert self.cow.deformable_type == "volume" + assert self.sim.get_deformable_object("cow") is self.cow + assert self.sim.get_deformable_object_uid_list() == ["cow"] + + assert type(self.cow.data) is DeformableObjectData + positions = self.cow.data.nodal_pos_w + velocities = self.cow.data.nodal_vel_w + state = self.cow.data.nodal_state_w + default_state = self.cow.data.default_nodal_state_w + assert positions.shape[-1] == 3 + assert velocities.shape == positions.shape + assert state.shape == (*positions.shape[:-1], 6) + assert default_state.shape == state.shape + render_vertices = self.cow.get_surface_vertices() + render_triangles = self.cow.get_surface_triangles(env_ids=[0]) + assert render_vertices.shape[0] == self.sim.num_envs + assert int(render_triangles.max()) < render_vertices.shape[1] + def test_remove(self): - self.sim.remove_asset(self.cow.uid) - assert ( - self.cow.uid not in self.sim._soft_objects - ), "Cow UID still present after removal" + with pytest.raises(NotImplementedError, match="pending removal"): + self.sim.remove_asset(self.cow.uid) + assert self.sim.get_deformable_object(self.cow.uid) is self.cow def teardown_method(self): """Clean up resources after each test method.""" diff --git a/tests/sim/objects/test_usd.py b/tests/sim/objects/test_usd.py index 46e9f38ce..dd8dc0b10 100644 --- a/tests/sim/objects/test_usd.py +++ b/tests/sim/objects/test_usd.py @@ -29,41 +29,39 @@ ArticulationCfg, RigidObjectCfg, JointDrivePropertiesCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, ) from embodichain.lab.sim.shapes import MeshCfg from embodichain.data import get_data_path -NUM_ARENAS = 1 +NUM_ARENAS = 2 class BaseUsdTest: """Shared test logic for CPU and CUDA.""" - def setup_simulation(self, sim_device): + def setup_simulation(self, device): config = SimulationManagerCfg( headless=True, - sim_device=sim_device, + device=device, num_envs=NUM_ARENAS, ) self.sim = SimulationManager(config) - if sim_device == "cuda" and getattr(self.sim, "is_use_gpu_physics", False): - self.sim.init_gpu_physics() - def test_import_rigid(self): - default_attr = RigidBodyAttributesCfg() + default_attr = RigidBodyPhysicsCfg() sugar_box_path = get_data_path("SugarBox/sugar_box_usd/sugar_box.usda") sugar_box: RigidObject = self.sim.add_rigid_object( cfg=RigidObjectCfg( uid="sugar_box", shape=MeshCfg(fpath=sugar_box_path), body_type="dynamic", - use_usd_properties=False, + asset_physics_mode="overlay", init_pos=[0.0, 1.0, 0.1], attrs=default_attr, ) ) + self.sim.prepare() body0 = sugar_box._entities[0].get_physical_body() print(sugar_box._entities[0].get_physical_attr()) assert pytest.approx(body0.get_mass()) == default_attr.mass @@ -75,20 +73,32 @@ def test_import_rigid(self): default_attr.min_position_iters, default_attr.min_velocity_iters, ) + assert len(sugar_box._entities) == NUM_ARENAS + handles = {entity.get_native_handle() for entity in sugar_box._entities} + assert len(handles) == NUM_ARENAS def test_import_articulation(self): - default_drive = JointDrivePropertiesCfg() + default_drive = JointDrivePropertiesCfg( + drive_type="force", + stiffness=1e4, + damping=1e3, + max_effort=1e10, + max_velocity=1e10, + friction=0.0, + armature=0.0, + ) h1_path = get_data_path("UnitreeH1Usd/H1_usd/h1.usd") h1: Articulation = self.sim.add_articulation( cfg=ArticulationCfg( uid="h1", fpath=h1_path, build_pk_chain=False, - use_usd_properties=False, + asset_physics_mode="overlay", init_pos=[0.0, 0.0, 1.2], - drive_pros=default_drive, + joint_drive_props=default_drive, ) ) + self.sim.prepare() stiffness = h1.body_data.joint_stiffness damping = h1.body_data.joint_damping @@ -106,17 +116,18 @@ def test_import_articulation(self): ) def test_usd_properties(self): - """In this test, we set use_usd_properties=True to verify that the USD properties are correctly applied.""" + """Verify that preserve mode keeps physics authored in USD assets.""" h1_path = get_data_path("UnitreeH1Usd/H1_usd/h1.usd") h1: Articulation = self.sim.add_articulation( cfg=ArticulationCfg( uid="h1_beta", fpath=h1_path, build_pk_chain=False, - use_usd_properties=True, + asset_physics_mode="preserve", init_pos=[1.0, 0.0, 1.2], ) ) + self.sim.prepare() stiffness = h1.body_data.joint_stiffness damping = h1.body_data.joint_damping @@ -152,14 +163,14 @@ def test_usd_properties(self): uid="sugar_box_beta", shape=MeshCfg(fpath=sugar_box_path), body_type="dynamic", - use_usd_properties=True, + asset_physics_mode="preserve", init_pos=[1.0, 1.0, 0.1], ) ) body0 = sugar_box._entities[0].get_physical_body() print(sugar_box._entities[0].get_physical_attr()) assert pytest.approx(body0.get_mass(), 0.001) == 0.514 - # TODO: nvidia physx attrs in usd currently are not fully suported + # TODO: vendor-specific rigid-body attributes in USD are not fully supported. # assert(body0.get_linear_damping()==0) # assert(body0.get_angular_damping()==0.05) # assert(body0.get_solver_iteration_counts()==(4, 1)) @@ -180,13 +191,13 @@ def teardown_method(self): gc.collect() -@pytest.mark.skip(reason="Skipping CUDA tests temporarily") +# @pytest.mark.skip(reason="Skipping CUDA tests temporarily") class TestUsdCPU(BaseUsdTest): def setup_method(self): self.setup_simulation("cpu") -@pytest.mark.skip(reason="Skipping CUDA tests temporarily") +# @pytest.mark.skip(reason="Skipping CUDA tests temporarily") class TestUsdCUDA(BaseUsdTest): def setup_method(self): self.setup_simulation("cuda") diff --git a/tests/sim/robots/__init__.py b/tests/sim/robots/__init__.py new file mode 100644 index 000000000..dd650e902 --- /dev/null +++ b/tests/sim/robots/__init__.py @@ -0,0 +1,15 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- diff --git a/tests/sim/robots/test_entrypoints.py b/tests/sim/robots/test_entrypoints.py new file mode 100644 index 000000000..96f7ce695 --- /dev/null +++ b/tests/sim/robots/test_entrypoints.py @@ -0,0 +1,89 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from pathlib import Path +import runpy +import sys +from types import ModuleType + +import pytest + +import embodichain.lab.sim as sim_module +from embodichain.lab.sim.cfg import NewtonPhysicsCfg + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +_ROBOT_ENTRYPOINTS = ( + "embodichain/lab/sim/robots/cobotmagic.py", + "embodichain/lab/sim/robots/franka_panda.py", + "embodichain/lab/sim/robots/ur_robot.py", + "embodichain/lab/sim/robots/dual_arm.py", + "embodichain/lab/sim/robots/dexforce_w1/cfg.py", +) + + +@pytest.mark.parametrize("relative_path", _ROBOT_ENTRYPOINTS) +def test_robot_entrypoint_selects_newton_backend( + monkeypatch: pytest.MonkeyPatch, + relative_path: str, +) -> None: + """Each robot smoke program must forward ``--physics newton``.""" + captured: dict[str, object] = {} + + class SimulationManagerCfgSpy: + def __init__(self, **kwargs: object) -> None: + captured.update(kwargs) + + class SimulationManagerSpy: + def __init__(self, _cfg: SimulationManagerCfgSpy) -> None: + pass + + def add_robot(self, *, cfg: object) -> object: + return cfg + + def prepare(self) -> None: + pass + + def update(self, *, step: int) -> None: + pass + + def open_window(self) -> None: + pass + + def destroy(self) -> None: + pass + + monkeypatch.setattr(sim_module, "SimulationManagerCfg", SimulationManagerCfgSpy) + monkeypatch.setattr(sim_module, "SimulationManager", SimulationManagerSpy) + ipython_module = ModuleType("IPython") + ipython_module.embed = lambda: None # type: ignore[attr-defined] + monkeypatch.setitem(sys.modules, "IPython", ipython_module) + monkeypatch.setattr( + sys, + "argv", + [relative_path, "--physics", "newton"], + ) + + original_path_entry = sys.path[0] + try: + runpy.run_path(_REPOSITORY_ROOT / relative_path, run_name="__main__") + finally: + sys.path[0] = original_path_entry + + assert isinstance(captured["physics_cfg"], NewtonPhysicsCfg) + assert captured["device"] is None + assert captured["physics_cfg"].device == "cuda:0" diff --git a/tests/sim/sensors/test_camera.py b/tests/sim/sensors/test_camera.py index 9ac69af2a..9ea0bf3ae 100644 --- a/tests/sim/sensors/test_camera.py +++ b/tests/sim/sensors/test_camera.py @@ -16,13 +16,13 @@ from __future__ import annotations -import pytest -import torch import os - +from types import SimpleNamespace from unittest.mock import MagicMock import numpy as np +import pytest +import torch from tensordict import TensorDict from embodichain.lab.sim import SimulationManager, SimulationManagerCfg @@ -38,6 +38,7 @@ from embodichain.lab.sim.objects import Articulation from embodichain.lab.sim.cfg import ArticulationCfg, RenderCfg from embodichain.data import get_data_path +from scripts.tutorials.sim.create_sensor import create_sensor as create_tutorial_sensor FULL_NUM_ENVS = 4 FULL_WIDTH = 640 @@ -61,7 +62,7 @@ def setup_simulation( # Setup SimulationManager config = SimulationManagerCfg( headless=True, - sim_device=sim_device, + device=sim_device, render_cfg=RenderCfg(renderer=renderer), num_envs=num_envs, ) @@ -78,6 +79,7 @@ def setup_simulation( } cfg = SensorCfg.from_dict(cfg_dict) self.camera: Camera = self.sim.add_sensor(cfg) + self.sim.prepare() def test_get_data(self): @@ -151,6 +153,7 @@ def test_attach_to_parent(self): self.art: Articulation = self.sim.add_articulation( cfg=ArticulationCfg.from_dict(cfg_dict) ) + self.sim.prepare() self.camera: Camera = self.sim.add_sensor( sensor_cfg=CameraCfg( uid="test", @@ -213,6 +216,30 @@ def setup_method(self): self.setup_simulation("cuda", renderer="hybrid") +def test_create_sensor_tutorial_preserves_attached_camera_view() -> None: + """Keep the wrist-camera view stable after the xyzw convention migration.""" + sim = MagicMock() + + create_tutorial_sensor(sim, SimpleNamespace(attach_sensor=True)) + + cfg = sim.add_sensor.call_args.kwargs["sensor_cfg"] + expected_rotation = torch.tensor( + [ + [0.579228, 0.573576, 0.579228], + [0.405580, -0.819152, 0.405580], + [0.707107, 0.0, -0.707107], + ], + dtype=torch.float32, + ) + assert cfg.extrinsics.parent == "ee_link" + torch.testing.assert_close( + cfg.extrinsics.transformation[:3, :3], + expected_rotation, + atol=1.0e-6, + rtol=1.0e-6, + ) + + @pytest.mark.parametrize( ("sim_device", "renderer"), [("cpu", "hybrid"), ("cpu", "fast-rt"), ("cuda", "fast-rt")], @@ -236,6 +263,23 @@ def test_camera_backend_smoke(sim_device, renderer): test.teardown_method() +def test_camera_parent_attachment_cpu() -> None: + """Attach a camera to a materialized articulation link on the CPU backend.""" + test = CameraTest() + test.setup_simulation( + "cpu", + renderer="hybrid", + num_envs=SMOKE_NUM_ENVS, + width=SMOKE_WIDTH, + height=SMOKE_HEIGHT, + enable_auxiliary_data=False, + ) + try: + test.test_attach_to_parent() + finally: + test.teardown_method() + + @pytest.mark.no_sim @pytest.mark.parametrize("stereo", [False, True]) def test_camera_attachment_reapplies_parent_relative_extrinsics(stereo: bool) -> None: diff --git a/tests/sim/sensors/test_contact.py b/tests/sim/sensors/test_contact.py index ef5e7f1b3..e3351fdf1 100644 --- a/tests/sim/sensors/test_contact.py +++ b/tests/sim/sensors/test_contact.py @@ -26,7 +26,7 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.cfg import ( RenderCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, ) from embodichain.lab.sim.sensors import ( ContactSensorCfg, @@ -43,14 +43,14 @@ class ContactTest: - def setup_simulation(self, sim_device, renderer="hybrid"): + def setup_simulation(self, device, renderer="hybrid"): sim_cfg = SimulationManagerCfg( width=CONTACT_TEST_WIDTH, height=CONTACT_TEST_HEIGHT, num_envs=2, headless=True, physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) - sim_device=sim_device, + device=device, render_cfg=RenderCfg(renderer=renderer), ) @@ -69,8 +69,10 @@ def setup_simulation(self, sim_device, renderer="hybrid"): contact_filter_cfg.articulation_cfg_list = [contact_filter_art_cfg] contact_filter_cfg.filter_need_both_actor = True + self.sim.prepare() self.to_grasp_pose(cube2) self.contact_sensor = self.sim.add_sensor(sensor_cfg=contact_filter_cfg) + self.sim.prepare() def create_cube(self, uid: str, position: list = (0.0, 0.0, 0)) -> RigidObject: """create cube @@ -89,12 +91,16 @@ def create_cube(self, uid: str, position: list = (0.0, 0.0, 0)) -> RigidObject: uid=uid, shape=CubeCfg(size=cube_size), body_type="dynamic", - attrs=RigidBodyAttributesCfg( - mass=0.1, - dynamic_friction=0.9, - static_friction=0.95, - restitution=0.01, - sleep_threshold=0.0, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 0.1}, + "rigid_props": {"sleep_threshold": 0.0}, + "material_props": { + "dynamic_friction": 0.9, + "static_friction": 0.95, + "restitution": 0.01, + }, + } ), init_pos=position, ) @@ -125,7 +131,7 @@ def create_robot(self, uid: str, position: list = (0.0, 0.0, 0)) -> Robot: }, "init_pos": position, "init_qpos": [0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], - "drive_pros": { + "joint_drive_props": { "stiffness": {"finger[1-2]_joint": 1e2}, "damping": {"finger[1-2]_joint": 1e1}, "max_effort": {"finger[1-2]_joint": 1e3}, @@ -224,17 +230,7 @@ def test_fetch_contact(self): # Remaining slots should be False assert not contact_report["is_valid"][env_id, num_contacts:].any() - cube2_user_ids = self.sim.get_rigid_object("cube2").get_user_ids() - finger1_user_ids = ( - self.sim.get_robot("UR10_PGI").get_user_ids("finger1_link").reshape(-1) - ) - filter_user_ids = torch.cat( - [ - cube2_user_ids, - self.sim.get_robot("UR10_PGI").get_user_ids("finger1_link").reshape(-1), - self.sim.get_robot("UR10_PGI").get_user_ids("finger2_link").reshape(-1), - ] - ) + filter_user_ids = self.contact_sensor.item_user_ids filter_contact_report = self.contact_sensor.filter_by_user_ids(filter_user_ids) n_filtered_contact = filter_contact_report["position"].shape[0] assert n_filtered_contact > 0, "No contact detected between gripper and cube." diff --git a/tests/sim/sensors/test_contact_default_e2e.py b/tests/sim/sensors/test_contact_default_e2e.py new file mode 100644 index 000000000..b5be0ee08 --- /dev/null +++ b/tests/sim/sensors/test_contact_default_e2e.py @@ -0,0 +1,115 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Default-PhysX contact-point multiplicity regression contract.""" + +from __future__ import annotations + +import pytest +import warp as wp + +from embodichain.lab.sim.cfg import ( + DefaultPhysicsCfg, + MassPropertiesCfg, + RigidBodyPhysicsCfg, + RigidObjectCfg, +) +from embodichain.lab.sim.sensors import ContactSensorCfg +from embodichain.lab.sim.shapes import CubeCfg +from embodichain.lab.sim.sim_manager import SimulationManager, SimulationManagerCfg + +pytestmark = pytest.mark.requires_sim + + +@pytest.mark.parametrize( + ("device", "expected_contacts_per_env"), + [ + pytest.param("cpu", 4, id="cpu"), + pytest.param("cuda:0", 4, marks=pytest.mark.gpu, id="direct-gpu"), + ], +) +def test_default_contact_sensor_preserves_physx_contact_multiplicity( + device: str, + expected_contacts_per_env: int, +) -> None: + wp.init() + if device.startswith("cuda") and not wp.is_cuda_available(): + pytest.skip("CUDA is required for the Direct-GPU contact E2E contract.") + + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + num_envs=2, + arena_space=2.0, + physics_cfg=DefaultPhysicsCfg(device=device), + ) + ) + try: + sim.add_rigid_object( + RigidObjectCfg( + uid="ground", + shape=CubeCfg(size=[1.0, 1.0, 0.1]), + attrs=RigidBodyPhysicsCfg(), + body_type="static", + ) + ) + sim.add_rigid_object( + RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=[0.2, 0.2, 0.2]), + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=1.0)), + body_type="dynamic", + init_pos=(0.0, 0.0, 0.4), + ) + ) + sensor = sim.add_sensor( + ContactSensorCfg( + uid="contacts", + rigid_uid_list=["cube"], + filter_need_both_actor=False, + max_contacts_per_env=16, + ) + ) + + counts = [0, 0] + for _ in range(80): + sim.update(step=1) + sensor.update() + counts = sensor._num_contacts_per_env.cpu().tolist() + if counts == [expected_contacts_per_env] * 2: + break + + assert not sensor.contact_capabilities.friction + assert counts == [expected_contacts_per_env] * 2 + data = sensor.get_data() + valid = data["is_valid"] + assert (data["impulse"][valid] > 1.0e-7).all() + positions = data["position"][valid] + assert positions[:, 0].abs().max().item() < 0.2 + assert (positions[:, 2] - 0.05).abs().max().item() < 0.02 + actor_ids = data["user_ids"][valid] + assert (actor_ids >= 0).any(dim=1).all() + cube_actor_ids = set(sensor.item_user_ids.cpu().tolist()) + assert all( + any(actor_id in cube_actor_ids for actor_id in pair) + for pair in actor_ids.cpu().tolist() + ) + assert all( + sensor.get_actor_info(actor_id).path.endswith("/cube") + for actor_id in cube_actor_ids + ) + finally: + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() diff --git a/tests/sim/sensors/test_contact_newton_e2e.py b/tests/sim/sensors/test_contact_newton_e2e.py new file mode 100644 index 000000000..94b1816d3 --- /dev/null +++ b/tests/sim/sensors/test_contact_newton_e2e.py @@ -0,0 +1,105 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Newton end-to-end contract for the backend-neutral contact sensor.""" + +from __future__ import annotations + +import pytest +import warp as wp + +from embodichain.lab.sim.cfg import ( + MassPropertiesCfg, + NewtonPhysicsCfg, + RigidBodyPhysicsCfg, + RigidObjectCfg, +) +from embodichain.lab.sim.sensors import ContactSensorCfg +from embodichain.lab.sim.shapes import CubeCfg +from embodichain.lab.sim.sim_manager import SimulationManager, SimulationManagerCfg + +pytestmark = [pytest.mark.requires_sim, pytest.mark.gpu] + + +def test_newton_contact_sensor_reports_each_arena() -> None: + wp.init() + if not wp.is_cuda_available(): + pytest.skip("CUDA is required for the Newton contact E2E contract.") + + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + num_envs=2, + arena_space=2.0, + physics_cfg=NewtonPhysicsCfg( + device="cuda:0", + num_substeps=1, + use_cuda_graph=False, + # Exercise a multi-point manifold so query-side reductions + # cannot satisfy this regression contract accidentally. + solver_cfg={ + "solver_type": "mujoco_warp", + "enable_multiccd": True, + }, + ), + ) + ) + try: + sim.add_rigid_object( + RigidObjectCfg( + uid="ground", + shape=CubeCfg(size=[1.0, 1.0, 0.1]), + attrs=RigidBodyPhysicsCfg(), + body_type="static", + ) + ) + sim.add_rigid_object( + RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=[0.2, 0.2, 0.2]), + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=1.0)), + body_type="dynamic", + init_pos=(0.0, 0.0, 0.4), + ) + ) + sensor = sim.add_sensor( + ContactSensorCfg( + uid="contacts", + rigid_uid_list=["ground", "cube"], + max_contacts_per_env=16, + ) + ) + + sim.update(step=120) + sensor.update() + + assert sensor.contact_capabilities.geometry + assert sensor.contact_capabilities.impulse + counts = sensor._num_contacts_per_env.cpu().tolist() + assert counts == [4, 4] + data = sensor.get_data() + assert data["is_valid"].sum(dim=1).cpu().tolist() == counts + assert (data["impulse"][data["is_valid"]] > 1.0e-7).all() + actor_ids = data["user_ids"][data["is_valid"]] + assert all( + sensor.get_actor_info(actor_id).path.endswith(("/ground", "/cube")) + for actor_id in set(actor_ids.flatten().cpu().tolist()) + ) + finally: + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + from dexsim.engine.newton_physics import teardown_newton_physics + + teardown_newton_physics() diff --git a/tests/sim/sensors/test_contact_query_sensor.py b/tests/sim/sensors/test_contact_query_sensor.py new file mode 100644 index 000000000..77fa60a87 --- /dev/null +++ b/tests/sim/sensors/test_contact_query_sensor.py @@ -0,0 +1,98 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from types import SimpleNamespace + +import torch +import warp as wp + +from dexsim.scene import ContactActorInfo, ContactBuffer, ContactQueryCapabilities +from embodichain.lab.sim.sensors import ContactSensor, ContactSensorCfg + + +class _FakeQuery: + def __init__(self) -> None: + self.capabilities = ContactQueryCapabilities(True, True, True) + self.selected_actor_ids = (4, 7) + self._actors = { + 4: ContactActorInfo(4, "arena_0/cube", None, "arena_0", 0), + 7: ContactActorInfo(7, "arena_1/cube", None, "arena_1", 1), + } + self.buffer = ContactBuffer.allocate(4, "cpu") + self.buffer.count = 2 + self.buffer.data[0] = torch.tensor( + [1.0, 2.0, 3.0, 0.0, 0.0, 1.0, 0.1, 0.2, 0.3, 0.4, -0.01] + ) + self.buffer.data[1] = torch.tensor( + [4.0, 5.0, 6.0, 0.0, 1.0, 0.0, 0.4, 0.5, 0.6, 0.7, -0.02] + ) + self.buffer.actor_ids[:2] = torch.tensor([[4, -1], [-1, 7]], dtype=torch.int32) + self.buffer.env_ids[:2] = torch.tensor([0, 1], dtype=torch.int32) + + def actor_info(self, actor_id: int) -> ContactActorInfo: + return self._actors[actor_id] + + def fetch(self) -> ContactBuffer: + return self.buffer + + +def test_contact_sensor_consumes_scene_query_and_explicit_env_ids() -> None: + wp.init() + query = _FakeQuery() + captured = {} + result = SimpleNamespace() + + def create_contact_query(targets, **kwargs): + captured["targets"] = tuple(targets) + captured.update(kwargs) + return query + + result.create_contact_query = create_contact_query + handles = ( + SimpleNamespace(path="arena_0/cube"), + SimpleNamespace(path="arena_1/cube"), + ) + owner = SimpleNamespace( + num_envs=2, + spawn_result=result, + _spawn_scene=SimpleNamespace(handles=lambda uid: handles), + arena_offsets=torch.zeros((2, 3)), + ) + cfg = ContactSensorCfg( + uid="contacts", + rigid_uid_list=["cube"], + filter_need_both_actor=False, + max_contacts_per_env=2, + ) + + sensor = ContactSensor(cfg, torch.device("cpu"), owner=owner) + sensor.update() + data = sensor.get_data() + + assert captured["targets"] == handles + assert captured["match"] == "any" + assert captured["frame"] == "arena" + assert captured["capacity"] == 4 + assert captured["capacity_per_env"] == 2 + assert sensor.total_current_contacts == 2 + assert data["is_valid"][:, 0].all() + assert data["position"][0, 0].tolist() == [1.0, 2.0, 3.0] + assert data["position"][1, 0].tolist() == [4.0, 5.0, 6.0] + assert data["user_ids"][1, 0].tolist() == [-1, 7] + assert sensor.get_actor_info(7).path == "arena_1/cube" + assert sensor.contact_capabilities.impulse diff --git a/tests/sim/sensors/test_stereo.py b/tests/sim/sensors/test_stereo.py index 156393ed7..704e00120 100644 --- a/tests/sim/sensors/test_stereo.py +++ b/tests/sim/sensors/test_stereo.py @@ -42,7 +42,7 @@ def setup_simulation( # Setup SimulationManager config = SimulationManagerCfg( headless=True, - sim_device=sim_device, + device=sim_device, num_envs=num_envs, render_cfg=RenderCfg(renderer=renderer), ) @@ -61,6 +61,7 @@ def setup_simulation( } cfg = SensorCfg.from_dict(cfg_dict) self.camera: StereoCamera = self.sim.add_sensor(cfg) + self.sim.prepare() def test_get_data(self): diff --git a/tests/sim/spawn/__init__.py b/tests/sim/spawn/__init__.py new file mode 100644 index 000000000..19567d22d --- /dev/null +++ b/tests/sim/spawn/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for EmbodiChain Spawn descriptor translation.""" + +from __future__ import annotations diff --git a/tests/sim/spawn/test_create_robot_integration.py b/tests/sim/spawn/test_create_robot_integration.py new file mode 100644 index 000000000..d2c71234d --- /dev/null +++ b/tests/sim/spawn/test_create_robot_integration.py @@ -0,0 +1,197 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Regression coverage for robots configured by simulation tutorials.""" + +from __future__ import annotations + +import numpy as np +import pytest + +import dexsim +from embodichain.lab.sim.cfg import NewtonPhysicsCfg +from embodichain.lab.sim.spawn.descriptors import ( + articulation_desc_from_cfg, + configure_articulation_desc, +) +from embodichain.lab.sim.spawn.scene import SpawnScene +from scripts.tutorials.sim.create_sensor import create_robot as create_sensor_robot +from scripts.tutorials.sim.create_robot import create_robot + +pytestmark = pytest.mark.requires_sim + +ARM_BASE_MASS = 3.167 # SR5 base_link inertial mass from the source URDF. +ARM_BASE_INERTIA = (5.677594, 30.912516, 31.167990) +ARM_STIFFNESS = 1.0e4 +ARM_DAMPING = 1.5e3 +ARM_MAX_EFFORT = 1.0e4 + + +class _ConfigCapture: + def add_robot(self, cfg): + return cfg + + +def _make_world(*, backend: str) -> dexsim.World: + """Build one headless world for a source-physics integration check.""" + config = dexsim.WorldConfig() + config.open_windows = False + config.renderer = dexsim.types.Renderer.HYBRID + if backend == "newton": + config.newton_cfg = NewtonPhysicsCfg(num_substeps=1).to_dexsim_cfg(gpu_id=0) + elif backend != "default": + raise ValueError(f"Unsupported test backend: {backend!r}.") + return dexsim.World(config) + + +def _resolve_tutorial_properties(world, cfg): + scene = SpawnScene(world, num_envs=1) + scene.builder.prepare_arenas() + descriptor = articulation_desc_from_cfg(cfg, per_env=False) + scene.declare( + "articulation", + "robot", + descriptor, + configure_source=lambda value: configure_articulation_desc(value, cfg), + ) + result = scene.commit() + descriptor = scene.handles("robot")[0].desc + + base = descriptor.get_link_desc("arm_base_link") + joint = descriptor.get_joint_desc("joint1") + properties = ( + base.rigid_body.mass, + base.rigid_body.inertia.copy(), + joint.dexsim.stiffness, + joint.dexsim.damping, + joint.dexsim.max_force, + joint.newton.target_ke, + joint.newton.target_kd, + joint.effort_limit, + ) + result.close() + return properties + + +def test_create_robot_preserves_source_inertia_and_arm_drive() -> None: + cfg = create_robot(_ConfigCapture()) + cfg.fpath = cfg.urdf_cfg.assemble_urdf() + + world = _make_world(backend="default") + + ( + mass, + inertia, + stiffness, + damping, + max_effort, + newton_ke, + newton_kd, + common_max_effort, + ) = _resolve_tutorial_properties(world, cfg) + + assert mass == pytest.approx(ARM_BASE_MASS) + np.testing.assert_allclose( + inertia, + ARM_BASE_INERTIA, + rtol=1.0e-5, + ) + assert stiffness == pytest.approx(ARM_STIFFNESS) + assert damping == pytest.approx(ARM_DAMPING) + assert max_effort == pytest.approx(ARM_MAX_EFFORT) + assert newton_ke == pytest.approx(ARM_STIFFNESS) + assert newton_kd == pytest.approx(ARM_DAMPING) + assert common_max_effort == pytest.approx(ARM_MAX_EFFORT) + + +def test_create_robot_newton_preserves_source_inertia_and_arm_drive() -> None: + """The deferred Newton build uses the same source property contract.""" + cfg = create_robot(_ConfigCapture()) + cfg.fpath = cfg.urdf_cfg.assemble_urdf() + + ( + mass, + inertia, + stiffness, + damping, + max_effort, + newton_ke, + newton_kd, + common_max_effort, + ) = _resolve_tutorial_properties(_make_world(backend="newton"), cfg) + + assert mass == pytest.approx(ARM_BASE_MASS) + np.testing.assert_allclose(inertia, ARM_BASE_INERTIA, rtol=1.0e-5) + assert ( + stiffness, + damping, + max_effort, + newton_ke, + newton_kd, + common_max_effort, + ) == pytest.approx( + ( + ARM_STIFFNESS, + ARM_DAMPING, + ARM_MAX_EFFORT, + ARM_STIFFNESS, + ARM_DAMPING, + ARM_MAX_EFFORT, + ) + ) + + +def test_create_sensor_uses_the_matched_arm_drive() -> None: + """Keep the sensor tutorial's arm controller aligned across backends.""" + cfg = create_sensor_robot(_ConfigCapture()) + + assert cfg.joint_drive_props is not None + assert cfg.joint_drive_props.max_effort == { + "joint[1-6]": ARM_MAX_EFFORT, + "LEFT_.*": ARM_MAX_EFFORT, + } + cfg.fpath = cfg.urdf_cfg.assemble_urdf() + + world = _make_world(backend="default") + + ( + _, + _, + stiffness, + damping, + max_effort, + newton_ke, + newton_kd, + common_max_effort, + ) = _resolve_tutorial_properties(world, cfg) + + assert ( + stiffness, + damping, + max_effort, + newton_ke, + newton_kd, + common_max_effort, + ) == pytest.approx( + ( + ARM_STIFFNESS, + ARM_DAMPING, + ARM_MAX_EFFORT, + ARM_STIFFNESS, + ARM_DAMPING, + ARM_MAX_EFFORT, + ) + ) diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py new file mode 100644 index 000000000..c184f5dd9 --- /dev/null +++ b/tests/sim/spawn/test_descriptors.py @@ -0,0 +1,2103 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for solver-aware Spawn descriptor translation.""" + +from __future__ import annotations + +import copy +import warnings +from dataclasses import fields, is_dataclass +from types import SimpleNamespace +from unittest.mock import Mock, patch + +import numpy as np +import pytest + +import dexsim +from dexsim.types import DriveType +from dexsim.spawn import ( + ArticulationDesc, + ClothDesc, + CollisionDesc, + CollisionApproximation, + DexsimCollisionDesc, + DexsimJointDesc, + DexsimPhysicsDesc, + JointDesc, + LinkDesc, + NewtonCollisionDesc, + NewtonJointDesc, + ObjectDesc, + RigidBodyPhysicsDesc, + SoftBodyDesc, +) + +from embodichain.lab.sim.cfg import ( + SurfaceElementPropertiesCfg, + ArticulationCfg, + ArticulationRootPropertiesCfg, + SurfaceDeformableObjectCfg, + SurfaceDeformablePhysicsCfg, + CollisionPropertiesCfg, + DefaultCollisionPropertiesCfg, + DefaultRigidBodyPropertiesCfg, + JointDrivePropertiesCfg, + LinkPhysicsOverrideCfg, + MassPropertiesCfg, + MeshCollisionCfg, + NewtonCollisionPropertiesCfg, + NewtonJointDrivePropertiesCfg, + NewtonRigidBodyMaterialCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, + RigidObjectCfg, + RobotCfg, + VolumeDeformablePhysicsCfg, + VolumeDeformableMeshingCfg, + VolumeDeformableObjectCfg, +) +from embodichain.lab.sim.shapes import CubeCfg, LoadOption, MeshCfg +from embodichain.lab.sim.objects import Articulation +from embodichain.lab.sim.spawn.descriptors import ( + articulation_desc_from_cfg, + surface_deformable_desc_from_cfg, + configure_articulation_desc, + rigid_desc_from_cfg, + volume_deformable_desc_from_cfg, +) +from embodichain.lab.sim.spawn.usd import ( + articulation_desc_from_usd, + rigid_desc_from_usd, +) + +pytestmark = pytest.mark.no_sim + +RESTITUTION = 0.25 +DEFORMABLE_MESH_PATH = "/assets/deformable.obj" + + +def test_soft_descriptor_uses_newton_particle_schema() -> None: + youngs = 1.0e5 + poissons = 0.4 + cfg = VolumeDeformableObjectCfg( + uid="soft", + shape=MeshCfg(fpath=DEFORMABLE_MESH_PATH), + particle_radius=0.02, + particle_flags=0, + validate_mesh=True, + meshing=VolumeDeformableMeshingCfg( + triangle_remesh_resolution=12, + triangle_simplify_target=40, + simulation_mesh_resolution=16, + voxel_num_relaxation_iters=7, + voxel_rel_min_tet_volume=0.08, + voxel_surface_dist_ratio=0.3, + embedding_impl="dexsim_exact_cpu", + ), + attrs=VolumeDeformablePhysicsCfg( + youngs=youngs, + poissons=poissons, + density=75.0, + elasticity_damping=0.2, + surface_props=SurfaceElementPropertiesCfg( + tri_ke=1.0, + edge_ke=2.0, + ), + ), + ) + + descriptor, materials = volume_deformable_desc_from_cfg(cfg, per_env=False) + + assert isinstance(descriptor, SoftBodyDesc) + assert descriptor.mesh.file_path == DEFORMABLE_MESH_PATH + assert descriptor.particle_radius == pytest.approx(0.02) + assert descriptor.particle_flags == 0 + assert descriptor.validate_mesh is True + assert descriptor.per_env is False + assert descriptor.physics.volume_density == pytest.approx(75.0) + assert descriptor.physics.k_mu == pytest.approx(youngs / (2.0 * (1.0 + poissons))) + assert descriptor.physics.k_lambda == pytest.approx( + youngs * poissons / ((1.0 + poissons) * (1.0 - 2.0 * poissons)) + ) + assert descriptor.physics.k_damp == pytest.approx(0.2) + assert descriptor.physics.surface_tri_ke == pytest.approx(1.0) + assert descriptor.physics.surface_edge_ke == pytest.approx(2.0) + assert descriptor.physics.dexsim is None + assert descriptor.meshing.proxy_simplify_target == 40 + assert descriptor.meshing.proxy_remesh_resolution == 12 + assert descriptor.meshing.voxel_resolution == 16 + assert descriptor.meshing.voxel_num_relaxation_iters == 7 + assert descriptor.meshing.voxel_rel_min_tet_volume == pytest.approx(0.08) + assert descriptor.meshing.voxel_surface_dist_ratio == pytest.approx(0.3) + assert descriptor.meshing.embedding_impl == "dexsim_exact_cpu" + assert materials == {} + + +def test_cloth_descriptor_uses_newton_particle_schema() -> None: + cfg = SurfaceDeformableObjectCfg( + uid="cloth", + shape=MeshCfg(fpath=DEFORMABLE_MESH_PATH), + particle_radius=0.01, + particle_flags=np.asarray([0, 1, 0], dtype=np.int32), + validate_mesh=True, + attrs=SurfaceDeformablePhysicsCfg( + density=2.5, + surface_props=SurfaceElementPropertiesCfg( + tri_ke=100.0, + tri_ka=90.0, + tri_kd=5.0, + edge_ke=20.0, + edge_kd=2.0, + ), + add_springs=True, + spring_ke=30.0, + spring_kd=3.0, + ), + ) + + descriptor, materials = surface_deformable_desc_from_cfg(cfg, per_env=False) + + assert isinstance(descriptor, ClothDesc) + assert descriptor.mesh.file_path == DEFORMABLE_MESH_PATH + assert descriptor.particle_radius == pytest.approx(0.01) + np.testing.assert_array_equal(descriptor.particle_flags, [0, 1, 0]) + assert descriptor.validate_mesh is True + assert descriptor.per_env is False + assert descriptor.physics.surface_density == pytest.approx(2.5) + assert descriptor.physics.tri_ke == pytest.approx(100.0) + assert descriptor.physics.tri_ka == pytest.approx(90.0) + assert descriptor.physics.tri_kd == pytest.approx(5.0) + assert descriptor.physics.edge_ke == pytest.approx(20.0) + assert descriptor.physics.edge_kd == pytest.approx(2.0) + assert descriptor.physics.add_springs is True + assert descriptor.physics.spring_ke == pytest.approx(30.0) + assert descriptor.physics.spring_kd == pytest.approx(3.0) + assert descriptor.physics.dexsim is None + assert materials == {} + + +def test_cloth_descriptor_preserves_array_mesh_vertex_order() -> None: + vertices = np.asarray( + [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], + dtype=np.float32, + ) + triangles = np.asarray([[2, 0, 1]], dtype=np.int32) + uv_coords = np.asarray( + [[0.0, 0.0], [1.0, 0.0], [0.5, 1.0]], + dtype=np.float32, + ) + cfg = SurfaceDeformableObjectCfg( + uid="cloth", + shape=MeshCfg( + vertices=vertices, + triangles=triangles, + uv_coords=uv_coords, + ), + particle_flags=[0, 1, 1], + ) + + cfg.validate() + descriptor, _ = surface_deformable_desc_from_cfg(cfg) + + assert descriptor.mesh.file_path is None + np.testing.assert_array_equal(descriptor.mesh.vertices, vertices) + np.testing.assert_array_equal(descriptor.mesh.triangles, triangles) + np.testing.assert_array_equal(descriptor.mesh.uv_coords, uv_coords) + np.testing.assert_array_equal(descriptor.particle_flags, [0, 1, 1]) + + +def test_cloth_descriptor_supports_independent_visual_mesh() -> None: + visual_mesh_path = "/assets/deformable_visual.obj" + cfg = SurfaceDeformableObjectCfg( + uid="cloth", + shape=MeshCfg( + vertices=np.asarray( + [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]], + dtype=np.float32, + ), + triangles=np.asarray([[0, 1, 2]], dtype=np.int32), + ), + visual_shape=MeshCfg(fpath=visual_mesh_path), + visual_binding_mode="nearest_vertex", + ) + + descriptor, materials = surface_deformable_desc_from_cfg(cfg) + + assert descriptor.mesh.file_path is None + assert descriptor.visual_mesh is not None + assert descriptor.visual_mesh.file_path == visual_mesh_path + assert descriptor.visual_binding_mode == "nearest_vertex" + assert materials == {} + + +def test_cloth_descriptor_rejects_unknown_visual_binding_mode() -> None: + cfg = SurfaceDeformableObjectCfg( + uid="cloth", + shape=MeshCfg(fpath=DEFORMABLE_MESH_PATH), + visual_binding_mode="unsupported", + ) + + with pytest.raises(ValueError, match="visual_binding_mode"): + surface_deformable_desc_from_cfg(cfg) + + +def test_cloth_descriptor_rejects_multiple_mesh_sources() -> None: + cfg = SurfaceDeformableObjectCfg( + uid="cloth", + shape=MeshCfg( + fpath=DEFORMABLE_MESH_PATH, + vertices=np.zeros((3, 3), dtype=np.float32), + triangles=np.asarray([[0, 1, 2]], dtype=np.int32), + ), + ) + + with pytest.raises(ValueError, match="either fpath or vertices/triangles"): + surface_deformable_desc_from_cfg(cfg) + + +def test_cloth_descriptor_rejects_missing_mesh_source_after_config_validation() -> None: + cfg = SurfaceDeformableObjectCfg(uid="cloth", shape=MeshCfg()) + + cfg.validate() + with pytest.raises(ValueError, match="non-empty fpath or vertices/triangles"): + surface_deformable_desc_from_cfg(cfg) + + +def test_soft_descriptor_rejects_invalid_poisson_ratio() -> None: + cfg = VolumeDeformableObjectCfg( + uid="soft", + shape=MeshCfg(fpath=DEFORMABLE_MESH_PATH), + attrs=VolumeDeformablePhysicsCfg(poissons=0.5), + ) + + with pytest.raises(ValueError, match="poissons"): + volume_deformable_desc_from_cfg(cfg) + + +@pytest.mark.parametrize("particle_radius", [0.0, float("nan")]) +def test_cloth_descriptor_rejects_invalid_particle_radius( + particle_radius: float, +) -> None: + cfg = SurfaceDeformableObjectCfg( + uid="cloth", + shape=MeshCfg(fpath=DEFORMABLE_MESH_PATH), + particle_radius=particle_radius, + ) + + with pytest.raises(ValueError, match="particle_radius"): + surface_deformable_desc_from_cfg(cfg) + + +@pytest.mark.parametrize( + "particle_flags", + [ + True, + np.iinfo(np.int32).max + 1, + [0.0, 1.0], + [-1, 1], + [np.iinfo(np.int32).max + 1], + np.zeros((1, 2), dtype=np.int32), + ], +) +def test_cloth_descriptor_rejects_invalid_particle_flags( + particle_flags: object, +) -> None: + cfg = SurfaceDeformableObjectCfg( + uid="cloth", + shape=MeshCfg(fpath=DEFORMABLE_MESH_PATH), + particle_flags=particle_flags, + ) + + with pytest.raises((TypeError, ValueError), match="particle_flags"): + surface_deformable_desc_from_cfg(cfg) + + +def _resolved_articulation_desc() -> ArticulationDesc: + source_inertia = np.ones(3, dtype=np.float32) + base = LinkDesc( + "base", + "", + np.eye(4, dtype=np.float32), + collisions=[CollisionDesc()], + rigid_body=RigidBodyPhysicsDesc.dynamic( + mass=0.5, + inertia=source_inertia, + ), + ) + finger = LinkDesc( + "finger_left", + "base", + np.eye(4, dtype=np.float32), + collisions=[CollisionDesc()], + rigid_body=RigidBodyPhysicsDesc.dynamic( + mass=0.25, + inertia=source_inertia, + ), + ) + return ArticulationDesc( + name="robot", + links=[base, finger], + joints=[ + JointDesc( + "arm_joint", + "base", + "finger_left", + dexsim.engine.JointType.REVOLUTE, + ) + ], + root_link_name="base", + ) + + +def _assert_property_tree_equal(actual: object, expected: object) -> None: + if isinstance(expected, np.ndarray): + np.testing.assert_array_equal(actual, expected) + elif is_dataclass(expected): + assert type(actual) is type(expected) + for field in fields(expected): + _assert_property_tree_equal( + getattr(actual, field.name), + getattr(expected, field.name), + ) + elif isinstance(expected, dict): + assert actual.keys() == expected.keys() + for key, value in expected.items(): + _assert_property_tree_equal(actual[key], value) + elif isinstance(expected, (list, tuple)): + assert type(actual) is type(expected) + assert len(actual) == len(expected) + for actual_item, expected_item in zip(actual, expected): + _assert_property_tree_equal(actual_item, expected_item) + else: + assert actual == expected + + +@pytest.mark.parametrize( + ("solver_type", "expected_restitution"), + [ + ("mujoco_warp", None), + ("semi_implicit", None), + ("featherstone", None), + ("xpbd", RESTITUTION), + (None, RESTITUTION), + ], +) +def test_rigid_descriptor_projects_restitution_only_to_supported_solvers( + solver_type: str | None, + expected_restitution: float | None, +) -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + material_props=RigidBodyMaterialCfg(restitution=RESTITUTION) + ), + ) + + descriptor, _ = rigid_desc_from_cfg( + cfg, + newton_solver_type=solver_type, + ) + + newton = descriptor.collisions[0].newton + assert newton is not None + assert newton.margin == pytest.approx(0.001) + assert newton.gap == pytest.approx(0.001) + assert newton.restitution == expected_restitution + + +def test_rigid_descriptor_preserves_default_backend_restitution() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + material_props=RigidBodyMaterialCfg(restitution=RESTITUTION) + ), + ) + + descriptor, _ = rigid_desc_from_cfg( + cfg, + newton_solver_type="mujoco_warp", + ) + + assert descriptor.collisions[0].dexsim.restitution == RESTITUTION + + +def test_flat_rigid_physics_is_rejected_at_config_boundary() -> None: + with pytest.raises(ValueError, match="Removed flat rigid-body attrs fields"): + RigidObjectCfg.from_dict( + { + "uid": "cube", + "shape": {"shape_type": "Cube", "size": [0.1, 0.1, 0.1]}, + "attrs": {"mass": 2.0}, + } + ) + + +def test_rigid_descriptor_authors_mass_or_density_exclusively() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg.from_dict( + {"mass_props": {"mass": 1.0, "density": 1.0}} + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + assert descriptor.physics.mass == 1.0 + assert descriptor.physics.density is None + + +def test_rigid_descriptor_forwards_explicit_mass_properties() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg( + mass=2.0, + inertia=[1.0, 2.0, 3.0], + com_position=[0.1, 0.2, 0.3], + com_quaternion=[1.0, 2.0, 3.0, 4.0], + ), + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + np.testing.assert_array_equal(descriptor.physics.inertia, [1.0, 2.0, 3.0]) + np.testing.assert_allclose( + descriptor.physics.com_position, + [0.1, 0.2, 0.3], + ) + np.testing.assert_allclose( + descriptor.physics.com_quaternion, + np.array([4.0, 1.0, 2.0, 3.0]) / np.sqrt(30.0), + ) + + +@pytest.mark.parametrize( + ("attrs", "error_match"), + [ + ( + RigidBodyPhysicsCfg.from_dict( + {"mass_props": {"mass": 0.0, "inertia": [1.0, 2.0, 3.0]}} + ), + "density is required when mass is zero", + ), + ( + RigidBodyPhysicsCfg.from_dict( + {"mass_props": {"mass": 1.0, "inertia": [1.0, 2.0]}} + ), + "inertia must contain", + ), + ( + RigidBodyPhysicsCfg.from_dict( + {"mass_props": {"mass": 1.0, "com_quaternion": [0.0, 0.0, 0.0, 0.0]}} + ), + "com_quaternion cannot be zero", + ), + ], + ids=["inertia-without-mass", "invalid-inertia-shape", "zero-com-quaternion"], +) +def test_rigid_descriptor_rejects_invalid_mass_properties( + attrs: RigidBodyPhysicsCfg, + error_match: str, +) -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=attrs, + ) + + with pytest.raises(ValueError, match=error_match): + rigid_desc_from_cfg(cfg) + + +def test_static_rigid_descriptor_omits_mass_properties() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + body_type="static", + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": { + "mass": 2.0, + "density": 3.0, + "inertia": [1.0, 2.0, 3.0], + "com_position": [0.1, 0.2, 0.3], + } + } + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + assert descriptor.physics.mass is None + assert descriptor.physics.density is None + assert descriptor.physics.inertia is None + assert descriptor.physics.com_position is None + + +def test_kinematic_rigid_descriptor_honors_mass_priority() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + body_type="kinematic", + attrs=RigidBodyPhysicsCfg.from_dict( + {"mass_props": {"mass": 2.0, "density": 3.0}} + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + assert descriptor.physics.mass == 2.0 + assert descriptor.physics.density is None + + +def test_grouped_rigid_physics_routes_common_and_backend_properties() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=2.0), + rigid_props=DefaultRigidBodyPropertiesCfg(linear_damping=0.2), + collision_props=NewtonCollisionPropertiesCfg( + collision_enabled=False, + margin=0.01, + ), + material_props=NewtonRigidBodyMaterialCfg( + dynamic_friction=0.4, + ke=1000.0, + torsional_friction=0.02, + ), + ), + ) + + descriptor, _ = rigid_desc_from_cfg( + cfg, + newton_solver_type="mujoco_warp", + ) + + assert descriptor.physics.mass == 2.0 + assert descriptor.physics.dexsim.linear_damping == 0.2 + assert descriptor.physics.dexsim.angular_damping is None + collision = descriptor.collisions[0] + assert collision.enable_collision is False + assert collision.dexsim.dynamic_friction == 0.4 + assert collision.dexsim.static_friction is None + assert collision.dexsim.contact_offset is None + assert collision.dexsim.rest_offset is None + assert collision.newton.margin == 0.01 + assert collision.newton.mu == 0.4 + assert collision.newton.ke == 1000.0 + assert collision.newton.mu_torsional == 0.02 + + +def test_portable_collision_envelope_compiles_to_both_backends() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + collision_props=CollisionPropertiesCfg( + contact_offset=0.015, + rest_offset=0.005, + ) + ), + ) + + descriptor, _ = rigid_desc_from_cfg( + cfg, + newton_solver_type="mujoco_warp", + ) + + collision = descriptor.collisions[0] + assert collision.dexsim.contact_offset == pytest.approx(0.015) + assert collision.dexsim.rest_offset == pytest.approx(0.005) + assert collision.newton.margin == pytest.approx(0.005) + assert collision.newton.gap == pytest.approx(0.01) + + +def test_procedural_rigid_collision_defaults_compile_to_both_backends() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + ) + + descriptor, _ = rigid_desc_from_cfg( + cfg, + newton_solver_type="mujoco_warp", + ) + + collision = descriptor.collisions[0] + assert collision.dexsim.contact_offset == pytest.approx(0.002) + assert collision.dexsim.rest_offset == pytest.approx(0.001) + assert collision.newton.margin == pytest.approx(0.001) + assert collision.newton.gap == pytest.approx(0.001) + + +def test_newton_native_collision_envelope_overrides_portable_translation() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + collision_props=NewtonCollisionPropertiesCfg( + contact_offset=0.015, + rest_offset=0.005, + margin=0.007, + gap=0.004, + ) + ), + ) + + descriptor, _ = rigid_desc_from_cfg( + cfg, + newton_solver_type="mujoco_warp", + ) + + collision = descriptor.collisions[0] + assert collision.dexsim.contact_offset == pytest.approx(0.015) + assert collision.dexsim.rest_offset == pytest.approx(0.005) + assert collision.newton.margin == pytest.approx(0.007) + assert collision.newton.gap == pytest.approx(0.004) + + +def test_portable_collision_envelope_rejects_invalid_ordering() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + collision_props=CollisionPropertiesCfg( + contact_offset=0.001, + rest_offset=0.002, + ) + ), + ) + + with pytest.raises(ValueError, match="no smaller than rest_offset"): + rigid_desc_from_cfg(cfg, newton_solver_type="mujoco_warp") + + +def test_newton_fills_missing_portable_rest_offset_from_the_default_profile() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + collision_props=CollisionPropertiesCfg(contact_offset=0.003) + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg, newton_solver_type="mujoco_warp") + + collision = descriptor.collisions[0] + assert collision.dexsim.contact_offset == pytest.approx(0.003) + assert collision.dexsim.rest_offset == pytest.approx(0.001) + assert collision.newton.margin == pytest.approx(0.001) + assert collision.newton.gap == pytest.approx(0.002) + + +def test_procedural_collision_defaults_apply_when_only_collision_is_enabled() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + collision_props=CollisionPropertiesCfg(collision_enabled=True) + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + assert descriptor.physics.dexsim is None + assert descriptor.physics.newton is None + assert descriptor.collisions[0].enable_collision is True + assert descriptor.collisions[0].dexsim.contact_offset == pytest.approx(0.002) + assert descriptor.collisions[0].dexsim.rest_offset == pytest.approx(0.001) + assert descriptor.collisions[0].newton.margin == pytest.approx(0.001) + assert descriptor.collisions[0].newton.gap == pytest.approx(0.001) + + +def test_grouped_rigid_physics_overlays_usd_without_erasing_source( + monkeypatch: pytest.MonkeyPatch, +) -> None: + source = ObjectDesc( + name="source", + physics=RigidBodyPhysicsDesc.dynamic( + mass=7.0, + inertia=np.array([1.0, 2.0, 3.0], dtype=np.float32), + dexsim=DexsimPhysicsDesc( + linear_damping=0.6, + angular_damping=0.8, + ), + ), + collisions=[ + CollisionDesc( + enable_collision=False, + dexsim=DexsimCollisionDesc( + dynamic_friction=0.9, + contact_offset=0.05, + ), + newton=NewtonCollisionDesc(margin=0.03, gap=0.07), + ) + ], + ) + scene = SimpleNamespace(materials={}) + + def parse_singleton(path, collection, label): + return scene, source + + monkeypatch.setattr( + "embodichain.lab.sim.spawn.usd._parse_singleton", + parse_singleton, + ) + cfg = RigidObjectCfg( + uid="cube", + shape=MeshCfg(fpath="cube.usd"), + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + rigid_props=DefaultRigidBodyPropertiesCfg(linear_damping=0.2), + collision_props=NewtonCollisionPropertiesCfg(margin=0.01), + material_props=RigidBodyMaterialCfg(dynamic_friction=0.4), + ), + ) + + descriptor, _ = rigid_desc_from_usd(cfg) + + assert descriptor.physics.mass == 7.0 + np.testing.assert_array_equal(descriptor.physics.inertia, [1.0, 2.0, 3.0]) + assert descriptor.physics.dexsim.linear_damping == 0.2 + assert descriptor.physics.dexsim.angular_damping == 0.8 + collision = descriptor.collisions[0] + assert collision.enable_collision is False + assert collision.dexsim.dynamic_friction == 0.4 + assert collision.dexsim.contact_offset == 0.05 + assert collision.newton.margin == 0.01 + assert collision.newton.gap == 0.07 + + +def test_rigid_usd_can_recompute_source_inertia( + monkeypatch: pytest.MonkeyPatch, +) -> None: + source = ObjectDesc( + name="source", + physics=RigidBodyPhysicsDesc.dynamic( + mass=7.0, + inertia=np.array([1.0, 2.0, 3.0], dtype=np.float32), + ), + collisions=[CollisionDesc()], + ) + monkeypatch.setattr( + "embodichain.lab.sim.spawn.usd._parse_singleton", + lambda path, collection, label: (SimpleNamespace(materials={}), source), + ) + cfg = RigidObjectCfg( + uid="cube", + shape=MeshCfg(fpath="cube.usd"), + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg( + mass=2.0, + recompute_inertia=True, + ) + ), + ) + + descriptor, _ = rigid_desc_from_usd(cfg) + + assert descriptor.physics.mass == 2.0 + assert descriptor.physics.inertia is None + + +def test_rigid_usd_preserves_asset_physics_by_default( + monkeypatch: pytest.MonkeyPatch, +) -> None: + source_mass = 7.0 + source_scale = np.array([2.0, 3.0, 4.0], dtype=np.float32) + source = ObjectDesc( + name="source", + physics=RigidBodyPhysicsDesc.dynamic(mass=source_mass), + collisions=[CollisionDesc(enable_collision=False)], + body_scale=source_scale, + ) + monkeypatch.setattr( + "embodichain.lab.sim.spawn.usd._parse_singleton", + lambda path, collection, label: (SimpleNamespace(materials={}), source), + ) + cfg = RigidObjectCfg( + uid="cube", + shape=MeshCfg(fpath="cube.usd"), + body_type="static", + body_scale=(1.0, 1.0, 1.0), + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=1.0)), + ) + + descriptor, _ = rigid_desc_from_usd(cfg) + + assert descriptor.physics.mass == source_mass + assert descriptor.physics.actor_type == dexsim.types.ActorType.DYNAMIC + np.testing.assert_array_equal(descriptor.body_scale, source_scale) + assert descriptor.collisions[0].enable_collision is False + assert cfg.body_type == "dynamic" + assert cfg.body_scale == tuple(source_scale) + + +def test_rigid_descriptor_forwards_newton_sdf_options() -> None: + cfg = RigidObjectCfg( + uid="mesh", + shape=MeshCfg( + fpath="mesh.glb", + collision=MeshCollisionCfg( + approximation="sdf", + sdf_padding=0.02, + ), + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + assert descriptor.collisions[0].newton.force_sdf is True + assert descriptor.collisions[0].newton.sdf_padding == pytest.approx(0.02) + + +def test_mesh_collision_and_backend_property_slots_compile_independently() -> None: + cfg = RigidObjectCfg( + uid="mesh", + shape=MeshCfg( + fpath="mesh.glb", + collision=MeshCollisionCfg( + approximation="sdf", + sdf_target_voxel_size=0.005, + sdf_padding=0.02, + ), + ), + attrs=RigidBodyPhysicsCfg( + rigid_props=DefaultRigidBodyPropertiesCfg(linear_damping=0.2), + collision_props=NewtonCollisionPropertiesCfg(margin=0.04), + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + collision = descriptor.collisions[0] + + assert descriptor.physics.dexsim.linear_damping == pytest.approx(0.2) + assert collision.approximation == CollisionApproximation.SDF + assert collision.decomp_max_hulls == 1 + assert collision.newton.margin == pytest.approx(0.04) + assert collision.newton.sdf_target_voxel_size == pytest.approx(0.005) + assert collision.newton.sdf_max_resolution is None + assert collision.newton.sdf_padding == pytest.approx(0.02) + + +def test_mesh_cfg_legacy_collision_fields_normalize_before_compilation() -> None: + with pytest.warns(DeprecationWarning): + cfg = RigidObjectCfg.from_dict( + { + "uid": "mesh", + "shape": { + "shape_type": "Mesh", + "fpath": "mesh.glb", + "max_convex_hull_num": 3, + "acd_method": "coacd", + }, + } + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + assert ( + descriptor.collisions[0].approximation + == CollisionApproximation.CONVEX_DECOMPOSITION + ) + assert descriptor.collisions[0].decomp_max_hulls == 3 + + +@pytest.mark.parametrize("enabled", [True, False]) +def test_newton_particle_collision_uses_the_collision_property_slot( + enabled: bool, +) -> None: + """Preserve deformable contact toggles through the current config schema.""" + cfg = RigidObjectCfg.from_dict( + { + "uid": "cube", + "shape": {"shape_type": "Cube", "size": [0.1, 0.1, 0.1]}, + "attrs": { + "collision_props": { + "backend": "newton", + "has_particle_collision": enabled, + }, + }, + } + ) + + descriptor, _ = rigid_desc_from_cfg(cfg, newton_solver_type="vbd") + + assert descriptor.collisions[0].newton.has_particle_collision is enabled + + +def test_rigid_descriptor_preserves_array_mesh_data() -> None: + vertices = np.asarray( + [[2.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 2.0]], + dtype=np.float32, + ) + triangles = np.asarray([[2, 0, 1]], dtype=np.int32) + cfg = RigidObjectCfg( + uid="mesh", + shape=MeshCfg(vertices=vertices, triangles=triangles), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + np.testing.assert_array_equal(descriptor.renders[0].vertices, vertices) + np.testing.assert_array_equal(descriptor.renders[0].triangles, triangles) + np.testing.assert_array_equal(descriptor.collisions[0].vertices, vertices) + np.testing.assert_array_equal(descriptor.collisions[0].triangles, triangles) + + +def test_static_triangle_mesh_collision_compiles_without_convex_cooking() -> None: + cfg = RigidObjectCfg( + uid="mesh", + body_type="static", + shape=MeshCfg( + fpath="mesh.glb", + collision=MeshCollisionCfg(approximation="triangle_mesh"), + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + assert descriptor.collisions[0].approximation == CollisionApproximation.NONE + + +def test_dynamic_triangle_mesh_collision_is_rejected_before_spawn() -> None: + cfg = RigidObjectCfg( + uid="mesh", + shape=MeshCfg( + fpath="mesh.glb", + collision=MeshCollisionCfg(approximation="triangle_mesh"), + ), + ) + + with pytest.raises(ValueError, match="only for static"): + rigid_desc_from_cfg(cfg) + + +def test_spawn_rejects_unsupported_convex_decomposition_method() -> None: + cfg = RigidObjectCfg( + uid="mesh", + shape=MeshCfg( + fpath="mesh.glb", + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=4, + acd_method="vhacd", + ), + ), + ) + + with pytest.raises(ValueError, match="only acd_method='coacd'"): + rigid_desc_from_cfg(cfg) + + +def test_default_collision_solver_fields_compile_from_collision_slot() -> None: + cfg = RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=(0.1, 0.1, 0.1)), + attrs=RigidBodyPhysicsCfg( + collision_props=DefaultCollisionPropertiesCfg( + contact_offset=0.01, + torsional_patch_radius=0.02, + min_torsional_patch_radius=0.005, + disable_strong_friction=True, + ) + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + default_collision = descriptor.collisions[0].dexsim + assert default_collision.contact_offset == pytest.approx(0.01) + assert default_collision.torsional_patch_radius == pytest.approx(0.02) + assert default_collision.min_torsional_patch_radius == pytest.approx(0.005) + assert default_collision.disable_strong_friction is True + + +def test_mesh_descriptor_passes_load_options_to_spawn() -> None: + cfg = RigidObjectCfg( + uid="mesh", + shape=MeshCfg( + fpath="mesh.glb", + load_option=LoadOption( + rebuild_normals=True, + rebuild_tangent=True, + rebuild_3rdnormal=False, + rebuild_3rdtangent=False, + smooth=45.0, + ), + ), + ) + + descriptor, _ = rigid_desc_from_cfg(cfg) + + option = descriptor.renders[0].load_option + assert option is not None + assert option.rebuild_normals is True + assert option.rebuild_tangent is True + assert option.rebuild_3rdnormal is False + assert option.rebuild_3rdtangent is False + assert option.smooth == 45.0 + + +def test_articulation_constructor_defers_newton_properties_until_configure() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + material_props=RigidBodyMaterialCfg(restitution=RESTITUTION) + ), + ) + + descriptor = articulation_desc_from_cfg( + cfg, + newton_solver_type="mujoco_warp", + ) + + assert descriptor.newton_collision is None + assert descriptor.newton_drive is None + assert descriptor.urdf_read_inertia is True + + descriptor.links = _resolved_articulation_desc().links + descriptor.joints = _resolved_articulation_desc().joints + configure_articulation_desc( + descriptor, + cfg, + newton_solver_type="mujoco_warp", + ) + assert descriptor.links[0].collisions[0].newton is None + + +def test_flat_articulation_physics_is_rejected_at_config_boundary() -> None: + with pytest.raises(ValueError, match="Removed flat rigid-body attrs fields"): + ArticulationCfg.from_dict( + { + "uid": "robot", + "fpath": "robot.urdf", + "attrs": {"mass": 2.0}, + } + ) + + +def test_articulation_root_properties_compile_to_common_descriptor() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + root_props=ArticulationRootPropertiesCfg( + fixed_base=False, + self_collision_enabled=True, + ), + ) + + descriptor = articulation_desc_from_cfg(cfg) + + assert descriptor.fixed_base is False + assert descriptor.urdf_fix_root_link is False + assert descriptor.enable_self_collision is True + + +def test_articulation_root_defaults_are_resolved_at_import_boundary() -> None: + descriptor = articulation_desc_from_cfg( + ArticulationCfg(uid="robot", fpath="robot.urdf") + ) + + assert descriptor.fixed_base is True + assert descriptor.urdf_fix_root_link is True + assert descriptor.enable_self_collision is False + + +def test_explicit_root_properties_override_usd_in_preserve_mode() -> None: + source = ArticulationDesc( + name="source", + fixed_base=False, + enable_self_collision=True, + ) + cfg = ArticulationCfg( + uid="robot", + fpath="robot.usd", + asset_physics_mode="preserve", + root_props=ArticulationRootPropertiesCfg( + fixed_base=True, + self_collision_enabled=False, + ), + ) + + with patch( + "embodichain.lab.sim.spawn.usd._parse_singleton", + return_value=(SimpleNamespace(materials={}), source), + ): + descriptor, _ = articulation_desc_from_usd(cfg) + + assert descriptor.fixed_base is True + assert descriptor.enable_self_collision is False + + +def test_default_root_properties_override_usd_values() -> None: + source = ArticulationDesc( + name="source", + fixed_base=False, + enable_self_collision=True, + ) + cfg = ArticulationCfg( + uid="robot", + fpath="robot.usd", + asset_physics_mode="preserve", + ) + + with patch( + "embodichain.lab.sim.spawn.usd._parse_singleton", + return_value=(SimpleNamespace(materials={}), source), + ): + descriptor, _ = articulation_desc_from_usd(cfg) + + assert descriptor.fixed_base is True + assert descriptor.enable_self_collision is False + + +def test_explicit_none_root_properties_preserve_usd_values() -> None: + source = ArticulationDesc( + name="source", + fixed_base=False, + enable_self_collision=True, + ) + cfg = ArticulationCfg( + uid="robot", + fpath="robot.usd", + asset_physics_mode="preserve", + root_props=ArticulationRootPropertiesCfg( + fixed_base=None, + self_collision_enabled=None, + ), + ) + + with patch( + "embodichain.lab.sim.spawn.usd._parse_singleton", + return_value=(SimpleNamespace(materials={}), source), + ): + descriptor, _ = articulation_desc_from_usd(cfg) + + assert descriptor.fixed_base is False + assert descriptor.enable_self_collision is True + + +def test_articulation_descriptor_rejects_newton_acceleration_drive() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + drive_type="acceleration", + ), + ) + + descriptor = articulation_desc_from_cfg(cfg, newton_solver_type="mujoco_warp") + descriptor.links = _resolved_articulation_desc().links + descriptor.joints = _resolved_articulation_desc().joints + + with pytest.raises(NotImplementedError, match="acceleration-drive"): + configure_articulation_desc( + descriptor, + cfg, + newton_solver_type="mujoco_warp", + ) + + +@pytest.mark.parametrize( + ( + "target_mode", + "expected_default_mode", + "expected_newton_mode", + "expected_stiffness", + "expected_damping", + ), + [ + ("none", DriveType.NONE, 0, 0.0, 0.0), + ("position", DriveType.FORCE, 1, 12.0, 4.0), + ("velocity", DriveType.FORCE, 2, 0.0, 4.0), + ("position_velocity", DriveType.FORCE, 3, 12.0, 4.0), + ("effort", DriveType.NONE, 4, 0.0, 0.0), + ], +) +def test_portable_joint_target_modes_compile_for_both_backends( + target_mode: str, + expected_default_mode: DriveType, + expected_newton_mode: int, + expected_stiffness: float, + expected_damping: float, +) -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", + target_mode=target_mode, # type: ignore[arg-type] + stiffness=12.0, + damping=4.0, + ), + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc( + descriptor, + cfg, + newton_solver_type="mujoco_warp", + ) + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.dexsim.drive_mode == expected_default_mode + assert joint.newton.target_mode == expected_newton_mode + assert joint.dexsim.stiffness == pytest.approx(expected_stiffness) + assert joint.dexsim.damping == pytest.approx(expected_damping) + assert joint.newton.target_ke == pytest.approx(expected_stiffness) + assert joint.newton.target_kd == pytest.approx(expected_damping) + + +def test_force_drive_defaults_newton_target_to_position_velocity() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(drive_type="force"), + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc( + descriptor, + cfg, + newton_solver_type="mujoco_warp", + ) + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.dexsim.drive_mode == DriveType.FORCE + assert joint.newton.target_mode == 3 + + +@pytest.mark.parametrize( + ("target_mode", "expected_ke", "expected_kd"), + [ + ("none", 0.0, 0.0), + ("velocity", 0.0, 4.0), + ("effort", 0.0, 0.0), + ], +) +def test_non_mode_aware_newton_solver_uses_gain_fallbacks( + target_mode: str, + expected_ke: float, + expected_kd: float, +) -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + target_mode=target_mode, # type: ignore[arg-type] + stiffness=12.0, + damping=4.0, + ), + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg, newton_solver_type="xpbd") + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.newton.target_ke == pytest.approx(expected_ke) + assert joint.newton.target_kd == pytest.approx(expected_kd) + + +def test_non_mode_aware_newton_position_fallback_is_explicit() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + target_mode="position", + stiffness=12.0, + damping=4.0, + ), + ) + descriptor = _resolved_articulation_desc() + + with pytest.warns(UserWarning, match="POSITION is emulated"): + configure_articulation_desc(descriptor, cfg, newton_solver_type="xpbd") + + +def test_auto_solver_defers_position_mode_compatibility_warning() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + target_mode="position", + stiffness=12.0, + damping=4.0, + ), + ) + descriptor = _resolved_articulation_desc() + + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + configure_articulation_desc(descriptor, cfg, newton_solver_type="auto") + + assert not caught + + +def test_default_articulation_body_properties_compile_per_link() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + rigid_props=DefaultRigidBodyPropertiesCfg( + sleep_threshold=0.002, + min_position_iters=8, + min_velocity_iters=2, + ) + ), + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc( + descriptor, + cfg, + newton_solver_type="mujoco_warp", + ) + + for link in descriptor.links: + assert link.rigid_body.dexsim.sleep_threshold == pytest.approx(0.002) + assert link.rigid_body.dexsim.min_position_iters == 8 + assert link.rigid_body.dexsim.min_velocity_iters == 2 + assert link.rigid_body.newton is None + + +def test_articulation_config_applies_to_exact_source_resolved_names() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), + material_props=RigidBodyMaterialCfg(dynamic_friction=0.4), + ), + link_attrs={ + "fingers": LinkPhysicsOverrideCfg( + link_names_expr=["finger_.*"], + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg( + mass=2.0, + recompute_inertia=True, + ), + material_props=RigidBodyMaterialCfg(dynamic_friction=0.8), + ), + ) + }, + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", + stiffness={"arm_.*": 10.0}, + damping=3.0, + max_effort=20.0, + max_velocity=4.0, + friction=0.1, + armature=0.2, + ), + qpos_limits={"arm_.*": [-1.0, 1.0]}, + ) + + descriptor = articulation_desc_from_cfg(cfg) + assert descriptor.links == [] + assert descriptor.joints == [] + + resolved = _resolved_articulation_desc() + descriptor.links = resolved.links + descriptor.joints = resolved.joints + descriptor.root_link_name = resolved.root_link_name + + with ( + patch.object( + descriptor, + "set_link_properties", + wraps=descriptor.set_link_properties, + ) as set_link_properties, + patch.object( + descriptor, + "set_joint_properties", + wraps=descriptor.set_joint_properties, + ) as set_joint_properties, + ): + configure_articulation_desc(descriptor, cfg) + + assert set_link_properties.call_count == len(descriptor.links) + assert set_joint_properties.call_count == len(descriptor.joints) + + base = descriptor.get_link_desc("base") + finger = descriptor.get_link_desc("finger_left") + assert base.rigid_body.mass == 1.0 + assert base.collisions[0].dexsim.dynamic_friction == 0.4 + np.testing.assert_array_equal( + base.rigid_body.inertia, + np.ones(3, dtype=np.float32), + ) + assert finger.rigid_body.mass == 2.0 + assert finger.collisions[0].newton.mu == 0.8 + assert finger.rigid_body.inertia is None + assert finger.replace_inertial + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.dexsim.damping == 3.0 + assert joint.newton.target_kd == 3.0 + assert joint.armature == 0.2 + assert joint.dexsim.stiffness == 10.0 + assert joint.newton.target_ke == 10.0 + assert joint.effort_limit == 20.0 + assert joint.velocity_limit == 4.0 + assert joint.lower_limit == -1.0 + assert joint.upper_limit == 1.0 + + +def test_joint_drive_properties_compile_joint_dynamics() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + stiffness=10.0, + max_effort=20.0, + max_velocity=2.0, + friction=0.4, + armature=0.7, + ), + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg) + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.dexsim.stiffness == pytest.approx(10.0) + assert joint.effort_limit == pytest.approx(20.0) + assert joint.velocity_limit == pytest.approx(2.0) + assert joint.dexsim.joint_friction == pytest.approx(0.4) + assert joint.newton.friction == pytest.approx(0.4) + assert joint.armature == pytest.approx(0.7) + + +def test_articulation_array_qpos_limits_compile_before_backend_build() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + qpos_limits=np.array([[-0.5, 0.75]], dtype=np.float32), + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg) + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.lower_limit == pytest.approx(-0.5) + assert joint.upper_limit == pytest.approx(0.75) + + +def test_robot_control_part_drive_rule_expands_before_spawn() -> None: + cfg = RobotCfg( + uid="robot", + fpath="robot.urdf", + control_parts={"arm": ["arm_joint"]}, + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", + stiffness={"arm": 10.0, "arm_joint": 20.0}, + ), + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg) + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.dexsim.stiffness == 20.0 + assert joint.newton.target_ke == 20.0 + + +def test_newton_joint_compatibility_subclass_uses_portable_target_mode() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=NewtonJointDrivePropertiesCfg( + drive_type="force", + stiffness={"arm_.*": 12.0}, + damping=4.0, + friction=0.5, + armature=0.7, + target_mode={"arm_.*": "velocity"}, + ), + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg) + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.dexsim.stiffness == 0.0 + assert joint.dexsim.damping == 4.0 + assert joint.dexsim.joint_friction == 0.5 + assert joint.armature == 0.7 + assert joint.newton.target_ke == 0.0 + assert joint.newton.target_kd == 4.0 + assert joint.newton.friction == 0.5 + assert joint.newton.armature is None + assert joint.newton.target_mode == 2 + + +def test_grouped_link_physics_overrides_compose_after_source_resolution() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0), + material_props=RigidBodyMaterialCfg(dynamic_friction=0.4), + ), + link_attrs={ + "fingers": LinkPhysicsOverrideCfg( + link_names_expr=["finger_.*"], + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg( + mass=2.0, + recompute_inertia=True, + ), + material_props=RigidBodyMaterialCfg(dynamic_friction=0.8), + ), + ) + }, + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg) + + base = descriptor.get_link_desc("base") + finger = descriptor.get_link_desc("finger_left") + assert base.rigid_body.mass == 1.0 + assert base.collisions[0].newton.mu == 0.4 + assert finger.rigid_body.mass == 2.0 + assert finger.collisions[0].newton.mu == 0.8 + assert finger.rigid_body.inertia is None + + +def test_global_articulation_mass_properties_can_recompute_inertia() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(recompute_inertia=True)), + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg) + + for link in descriptor.links: + assert link.rigid_body.inertia is None + assert link.replace_inertial is True + assert link._embodichain_apply_physics + + +def test_invalid_source_inertia_uses_geometry_fallback_without_an_overlay() -> None: + """All-zero asset inertia is not a physical value to preserve.""" + descriptor = _resolved_articulation_desc() + for link in descriptor.links: + link._embodichain_source_inertia_valid = False + link._embodichain_has_collision_geometry = True + link.rigid_body.inertia = None + link.rigid_body.com_position = None + link.rigid_body.com_quaternion = None + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + ) + + configure_articulation_desc(descriptor, cfg) + + for link in descriptor.links: + assert link._embodichain_apply_physics + assert link.replace_inertial is True + assert link.rigid_body.inertia is None + + +def test_joint_only_overlay_does_not_author_link_physics() -> None: + """A robot drive overlay must not trigger native inertia derivation.""" + descriptor = _resolved_articulation_desc() + before = copy.deepcopy(descriptor) + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(stiffness=10.0), + ) + + configure_articulation_desc(descriptor, cfg) + + for link, source_link in zip(descriptor.links, before.links, strict=True): + assert not link._embodichain_apply_physics + _assert_property_tree_equal(link.rigid_body, source_link.rigid_body) + + +def test_density_override_requires_explicit_source_inertia_recomputation() -> None: + descriptor = _resolved_articulation_desc() + for link in descriptor.links: + link._embodichain_source_inertia_valid = True + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(density=1000.0)), + ) + + with pytest.raises(ValueError, match="Density override.*recompute_inertia=True"): + configure_articulation_desc(descriptor, cfg) + + +def test_per_link_mass_properties_can_preserve_global_source_inertia() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(recompute_inertia=True)), + link_attrs={ + "fingers": LinkPhysicsOverrideCfg( + link_names_expr=["finger_.*"], + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(recompute_inertia=False) + ), + ) + }, + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg) + + base = descriptor.get_link_desc("base") + finger = descriptor.get_link_desc("finger_left") + assert base.rigid_body.inertia is None + assert base.replace_inertial is True + np.testing.assert_array_equal( + finger.rigid_body.inertia, + np.ones(3, dtype=np.float32), + ) + assert finger.replace_inertial is False + + +def test_explicit_and_recomputed_inertia_are_mutually_exclusive() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg( + mass=2.0, + inertia=[1.0, 2.0, 3.0], + recompute_inertia=True, + ) + ), + ) + + with pytest.raises(ValueError, match="recompute_inertia"): + configure_articulation_desc(_resolved_articulation_desc(), cfg) + + +def test_recompute_inertia_rejects_non_boolean_values() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg( + recompute_inertia="yes", # type: ignore[arg-type] + ) + ), + ) + + with pytest.raises(TypeError, match="recompute_inertia"): + configure_articulation_desc(_resolved_articulation_desc(), cfg) + + +def test_grouped_link_zero_mass_falls_back_to_inherited_density() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=1.0, density=500.0) + ), + link_attrs={ + "fingers": LinkPhysicsOverrideCfg( + link_names_expr=["finger_.*"], + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=0.0)), + ) + }, + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg) + + assert descriptor.get_link_desc("base").rigid_body.mass == 1.0 + finger_physics = descriptor.get_link_desc("finger_left").rigid_body + assert finger_physics.mass is None + assert finger_physics.density == 500.0 + + +@pytest.mark.parametrize("source_path", ["robot.urdf", "robot.usd"]) +def test_articulation_preserve_mode_keeps_source_physics(source_path: str) -> None: + descriptor = _resolved_articulation_desc() + source_joint = descriptor.get_joint_desc("arm_joint") + source_joint.lower_limit = -2.0 + source_joint.upper_limit = 2.0 + source_joint.effort_limit = 321.0 + source_joint.dexsim = DexsimJointDesc(stiffness=123.0, damping=456.0) + before = copy.deepcopy(descriptor) + cfg = ArticulationCfg( + uid="robot", + fpath=source_path, + asset_physics_mode="preserve", + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=9.0)), + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", + stiffness=10.0, + damping=20.0, + ), + qpos_limits={"arm_.*": [-1.0, 1.0]}, + ) + + with pytest.warns( + UserWarning, + match="preserve.*attrs, joint_drive_props, qpos_limits", + ): + configure_articulation_desc(descriptor, cfg) + + _assert_property_tree_equal(descriptor, before) + + +def test_articulation_drive_overlay_preserves_unspecified_source_fields() -> None: + source_stiffness = 123.0 + source_damping = 456.0 + configured_stiffness = 10.0 + descriptor = _resolved_articulation_desc() + joint = descriptor.get_joint_desc("arm_joint") + joint.effort_limit = 321.0 + joint.dexsim = DexsimJointDesc( + stiffness=source_stiffness, + damping=source_damping, + drive_mode=DriveType.FORCE, + ) + joint.newton = NewtonJointDesc( + target_ke=source_stiffness, + target_kd=source_damping, + target_mode=2, + ) + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(stiffness=configured_stiffness), + ) + + configure_articulation_desc(descriptor, cfg) + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.dexsim.stiffness == configured_stiffness + assert joint.dexsim.damping == source_damping + assert joint.dexsim.drive_mode == DriveType.FORCE + assert joint.newton.target_ke == configured_stiffness + assert joint.newton.target_kd == source_damping + assert joint.newton.target_mode == 2 + assert joint.effort_limit == 321.0 + + +def test_articulation_overlay_does_not_invent_collision_geometry() -> None: + descriptor = _resolved_articulation_desc() + collisionless_link = LinkDesc( + "imu_link", + "base", + np.eye(4, dtype=np.float32), + rigid_body=RigidBodyPhysicsDesc.dynamic(mass=0.1), + ) + descriptor.links.append(collisionless_link) + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + material_props=RigidBodyMaterialCfg(dynamic_friction=0.5) + ), + ) + + configure_articulation_desc(descriptor, cfg) + + assert descriptor.get_link_desc("imu_link").collisions == [] + + +@pytest.mark.parametrize( + ("cfg", "error_type"), + [ + ( + ArticulationCfg( + uid="robot", + fpath="robot.urdf", + link_attrs={ + "missing": LinkPhysicsOverrideCfg( + link_names_expr=["missing_.*"], + ) + }, + ), + ValueError, + ), + ( + ArticulationCfg( + uid="robot", + fpath="robot.urdf", + link_attrs={ + "first": LinkPhysicsOverrideCfg( + link_names_expr=["finger_.*"], + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=2.0) + ), + ), + "second": LinkPhysicsOverrideCfg( + link_names_expr=["finger_left"], + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=3.0) + ), + ), + }, + ), + ValueError, + ), + ( + ArticulationCfg( + uid="robot", + fpath="robot.urdf", + joint_drive_props=JointDrivePropertiesCfg( + stiffness={"missing_.*": 10.0} + ), + ), + ValueError, + ), + ( + ArticulationCfg( + uid="robot", + fpath="robot.urdf", + joint_drive_props=JointDrivePropertiesCfg( + stiffness={"arm_.*": "not-a-number"} + ), + ), + TypeError, + ), + ( + ArticulationCfg( + uid="robot", + fpath="robot.urdf", + qpos_limits={"arm_.*": [1.0, -1.0]}, + ), + ValueError, + ), + ( + ArticulationCfg( + uid="robot", + fpath="robot.urdf", + qpos_limits=np.zeros((2, 2), dtype=np.float32), + ), + ValueError, + ), + ( + ArticulationCfg( + uid="robot", + fpath="robot.urdf", + joint_drive_props=NewtonJointDrivePropertiesCfg( + target_mode={"arm_.*": "servo"} + ), + ), + ValueError, + ), + ], + ids=[ + "unmatched-link", + "overlapping-link-groups", + "unmatched-joint", + "non-numeric-joint-property", + "invalid-qpos-limit", + "invalid-array-qpos-shape", + "invalid-newton-target-mode", + ], +) +def test_articulation_config_validation_failure_is_atomic( + cfg: ArticulationCfg, + error_type: type[Exception], +) -> None: + cfg.asset_physics_mode = "overlay" + descriptor = _resolved_articulation_desc() + before = copy.deepcopy(descriptor) + + with pytest.raises(error_type): + configure_articulation_desc(descriptor, cfg) + + _assert_property_tree_equal(descriptor, before) + finger = descriptor.get_link_desc("finger_left") + np.testing.assert_array_equal( + finger.rigid_body.inertia, + np.ones(3, dtype=np.float32), + ) + + +def test_usd_articulation_uses_the_same_exact_name_configuration() -> None: + cfg = ArticulationCfg( + uid="robot", + fpath="robot.usd", + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=1.0)), + link_attrs={ + "fingers": LinkPhysicsOverrideCfg( + link_names_expr=["finger_.*"], + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=2.0)), + ) + }, + joint_drive_props=JointDrivePropertiesCfg(stiffness={"arm_.*": 10.0}), + ) + source = ArticulationDesc( + name="source", + links=[ + LinkDesc( + "finger_left", + "", + np.eye(4, dtype=np.float32), + collisions=[CollisionDesc()], + rigid_body=RigidBodyPhysicsDesc.dynamic(mass=0.5), + ) + ], + joints=[ + JointDesc( + "arm_joint", + "finger_left", + "tip", + dexsim.engine.JointType.REVOLUTE, + ) + ], + ) + + with patch( + "embodichain.lab.sim.spawn.usd._parse_singleton", + return_value=(SimpleNamespace(materials={}), source), + ): + descriptor, _ = articulation_desc_from_usd( + cfg, + newton_solver_type="mujoco_warp", + ) + + configure_articulation_desc( + descriptor, + cfg, + newton_solver_type="mujoco_warp", + ) + + assert descriptor.get_link_desc("finger_left").rigid_body.mass == 2.0 + joint = descriptor.get_joint_desc("arm_joint") + assert joint.dexsim.stiffness == 10.0 + assert joint.newton.target_ke == 10.0 + + +def test_spawn_post_config_only_applies_render_uv() -> None: + render_body = Mock() + entity = Mock() + entity.joint_dof_layout = [] + entity.get_render_body.return_value = render_body + articulation = object.__new__(Articulation) + articulation.cfg = SimpleNamespace(compute_uv=True) + articulation._entities = [entity] + articulation.__dict__["link_names"] = ["base"] + articulation._prepared_default_root_topology_revision = -1 + articulation._mimic_info = SimpleNamespace( + mimic_id=np.array([], dtype=np.int32), + mimic_parent=np.array([], dtype=np.int32), + ) + articulation._set_default_joint_drive = Mock() + + articulation._apply_spawn_config() + + articulation._set_default_joint_drive.assert_not_called() + entity.get_render_body.assert_called_once_with("base") + render_body.set_projective_uv.assert_called_once_with() + + +def test_spawn_post_config_applies_default_only_root_properties() -> None: + native_articulation = Mock() + entity = SimpleNamespace( + _physics_binding=native_articulation, + joint_dof_layout=[], + ) + articulation = object.__new__(Articulation) + articulation.cfg = ArticulationCfg( + root_props=ArticulationRootPropertiesCfg( + sleep_threshold=0.005, + min_position_iters=8, + min_velocity_iters=2, + ) + ) + articulation._spawn_result = SimpleNamespace(backend="dexsim", topology_revision=0) + articulation._entities = [entity] + articulation._prepared_default_root_topology_revision = -1 + articulation._mimic_info = SimpleNamespace( + mimic_id=np.array([], dtype=np.int32), + mimic_parent=np.array([], dtype=np.int32), + ) + + articulation._apply_spawn_config() + + native_articulation.set_sleep_threshold.assert_called_once_with(0.005) + native_articulation.set_solver_iteration_counts.assert_called_once_with( + min_position_iters=8, + min_velocity_iters=2, + ) + + +def test_newton_skips_default_only_articulation_root_properties() -> None: + native_articulation = Mock() + entity = SimpleNamespace( + _physics_binding=native_articulation, + joint_dof_layout=[], + ) + articulation = object.__new__(Articulation) + articulation.cfg = ArticulationCfg( + root_props=ArticulationRootPropertiesCfg( + sleep_threshold=0.005, + min_position_iters=8, + min_velocity_iters=2, + ) + ) + articulation._spawn_result = SimpleNamespace(backend="newton", topology_revision=0) + articulation._entities = [entity] + articulation._prepared_default_root_topology_revision = -1 + articulation._mimic_info = SimpleNamespace( + mimic_id=np.array([], dtype=np.int32), + mimic_parent=np.array([], dtype=np.int32), + ) + + articulation._apply_spawn_config() + + native_articulation.set_sleep_threshold.assert_not_called() + native_articulation.set_solver_iteration_counts.assert_not_called() diff --git a/tests/sim/spawn/test_scene.py b/tests/sim/spawn/test_scene.py new file mode 100644 index 000000000..d204ac328 --- /dev/null +++ b/tests/sim/spawn/test_scene.py @@ -0,0 +1,475 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import inspect +from types import SimpleNamespace +from unittest.mock import MagicMock + +import pytest + +from embodichain.lab.sim.cfg import ArticulationRootPropertiesCfg, RigidObjectCfg +from embodichain.lab.sim.objects import ( + Articulation, + DeformableObject, + RigidObject, + RigidObjectGroup, + Robot, +) +from embodichain.lab.sim.spawn.scene import SpawnScene + +pytestmark = pytest.mark.no_sim + + +def _make_scene(handles: dict[str, object]) -> SpawnScene: + scene = object.__new__(SpawnScene) + scene._num_envs = 1 + scene.builder = SimpleNamespace( + is_finalized=True, + result=SimpleNamespace(handles=handles), + ) + scene._assets = {} + return scene + + +class _RetryableFacade: + def __init__(self, *, fail_first: bool = False) -> None: + self._entities: list[object] = [] + self.is_declared = True + self.declared_num_instances: int | None = None + self.fail_first = fail_first + self.bind_attempts = 0 + + def _initialize_spawn_declaration(self, num_instances: int) -> None: + self.declared_num_instances = num_instances + + def attach_spawn_handles(self, entities: tuple[object, ...]) -> None: + self._entities = list(entities) + + def bind_spawn(self, _result: object) -> None: + self.bind_attempts += 1 + if self.fail_first and self.bind_attempts == 1: + raise RuntimeError("bind failed") + self.is_declared = False + + +class _RuntimeConfigFacade(_RetryableFacade): + def __init__(self, events: list[str]) -> None: + super().__init__() + self.events = events + + def attach_spawn_handles(self, entities: tuple[object, ...]) -> None: + self.events.append("attach") + super().attach_spawn_handles(entities) + + def _prepare_spawn_runtime_config(self, _result: object) -> None: + self.events.append("runtime_config") + + +def test_bind_retries_only_incomplete_declarations() -> None: + first_handle = object() + second_handle = object() + scene = _make_scene({"first": first_handle, "second": second_handle}) + first = _RetryableFacade() + second = _RetryableFacade(fail_first=True) + + scene.track( + "rigid_object", + "first", + SimpleNamespace(name="first", per_env=False), + facade=first, + ) + scene.track( + "rigid_object", + "second", + SimpleNamespace(name="second", per_env=False), + facade=second, + ) + + with pytest.raises(RuntimeError, match="bind failed"): + scene.bind() + scene.bind() + scene.bind() + + assert first._entities == [first_handle] + assert second._entities == [second_handle] + assert first.declared_num_instances == 1 + assert second.declared_num_instances == 1 + assert first.bind_attempts == 1 + assert second.bind_attempts == 2 + + +def test_declare_initializes_facade_with_spawn_instance_count() -> None: + scene = object.__new__(SpawnScene) + scene._num_envs = 3 + scene.builder = SimpleNamespace( + is_finalized=False, + result=None, + add_object=lambda descriptor: descriptor, + ) + scene._assets = {} + facade = _RetryableFacade() + + scene.declare( + "rigid_object", + "cube", + SimpleNamespace(name="cube", per_env=True), + facade=facade, + ) + + assert facade.declared_num_instances == 3 + + +def test_rigid_object_receives_instance_count_only_when_declared() -> None: + scene = object.__new__(SpawnScene) + scene._num_envs = 3 + scene.builder = SimpleNamespace( + is_finalized=False, + result=None, + add_object=lambda descriptor: descriptor, + ) + scene._assets = {} + facade = RigidObject(RigidObjectCfg(uid="cube")) + + with pytest.raises(RuntimeError, match="registered through SpawnScene"): + _ = facade.num_instances + + scene.declare( + "rigid_object", + "cube", + SimpleNamespace(name="cube", per_env=True), + facade=facade, + ) + + assert facade.num_instances == 3 + assert facade._all_indices == [0, 1, 2] + + +@pytest.mark.parametrize( + "facade_type", + [RigidObject, Articulation, Robot, RigidObjectGroup, DeformableObject], +) +def test_object_constructors_hide_spawn_lifecycle_parameters( + facade_type: type[object], +) -> None: + parameters = inspect.signature(facade_type.__init__).parameters + + assert "declared_num_instances" not in parameters + assert "spawn_result" not in parameters + + +def test_runtime_config_attaches_articulation_before_preparing_it() -> None: + scene = _make_scene({}) + events: list[str] = [] + facade = _RuntimeConfigFacade(events) + scene.track( + "articulation", + "robot", + SimpleNamespace(name="robot", per_env=False), + facade=facade, + ) + handle = object() + scene.builder.result.handles["robot"] = handle + + scene.prepare_runtime_config(scene.builder.result) + + assert facade._entities == [handle] + assert events == ["attach", "runtime_config"] + + +def test_default_root_properties_prepare_once_per_topology_revision() -> None: + native_articulation = MagicMock() + articulation = object.__new__(Articulation) + articulation.cfg = SimpleNamespace( + root_props=ArticulationRootPropertiesCfg( + min_position_iters=32, + min_velocity_iters=8, + ) + ) + articulation._entities = [SimpleNamespace(_physics_binding=native_articulation)] + articulation._prepared_default_root_topology_revision = -1 + result = SimpleNamespace(backend="dexsim", topology_revision=3) + + articulation._prepare_spawn_runtime_config(result) + articulation._prepare_spawn_runtime_config(result) + + native_articulation.set_solver_iteration_counts.assert_called_once_with( + min_position_iters=32, + min_velocity_iters=8, + ) + + result.topology_revision = 4 + articulation._prepare_spawn_runtime_config(result) + assert native_articulation.set_solver_iteration_counts.call_count == 2 + + +def test_newton_skips_default_root_runtime_properties() -> None: + native_articulation = MagicMock() + articulation = object.__new__(Articulation) + articulation.cfg = SimpleNamespace( + root_props=ArticulationRootPropertiesCfg( + min_position_iters=32, + min_velocity_iters=8, + ) + ) + articulation._entities = [SimpleNamespace(_physics_binding=native_articulation)] + articulation._prepared_default_root_topology_revision = -1 + + articulation._prepare_spawn_runtime_config( + SimpleNamespace(backend="newton", topology_revision=3) + ) + + native_articulation.set_solver_iteration_counts.assert_not_called() + + +def test_commit_resolves_and_configures_before_finalize(monkeypatch) -> None: + events: list[str] = [] + descriptor = SimpleNamespace(name="robot", per_env=True, links=[]) + result = object() + builder = SimpleNamespace( + backend="newton", + is_finalized=False, + result=None, + replicate_plan=SimpleNamespace(env_names=lambda: ["arena_0"]), + add_articulation=lambda value: value, + ) + + def resolve_source(_builder: object, value: object) -> None: + events.append("resolve") + value.links = [SimpleNamespace(name="base")] + + def finalize() -> object: + events.append("finalize") + builder.is_finalized = True + builder.result = result + return result + + builder.finalize = finalize + monkeypatch.setattr( + "embodichain.lab.sim.spawn.source.resolve_articulation_source", + resolve_source, + ) + scene = object.__new__(SpawnScene) + scene.builder = builder + scene._assets = {} + + def configure(value: object) -> None: + assert value.links[0].name == "base" + events.append("configure") + + scene.declare( + "articulation", + "robot", + descriptor, + configure_source=configure, + ) + + assert scene.commit() is result + assert events == ["resolve", "configure", "finalize"] + + +@pytest.mark.parametrize("is_finalized", [False, True]) +def test_materialized_articulation_is_configured_before_backend_add( + is_finalized: bool, +) -> None: + events: list[str] = [] + descriptor = SimpleNamespace(name="robot", per_env=True, links=[]) + result = SimpleNamespace(handles={}) + builder = SimpleNamespace( + is_finalized=is_finalized, + result=result, + replicate_plan=SimpleNamespace(env_names=lambda: ["arena_0"]), + ) + + def resolve_source(value: object) -> None: + events.append("resolve") + value.links = [SimpleNamespace(name="base")] + + def configure(value: object) -> None: + assert value.links[0].name == "base" + events.append("configure") + + def add_articulation(value: object) -> object: + assert value.links[0].name == "base" + events.append("add") + return value + + builder.resolve_articulation_source = resolve_source + builder.add_articulation = add_articulation + scene = object.__new__(SpawnScene) + scene.builder = builder + scene._assets = {} + + scene.declare( + "articulation", + "robot", + descriptor, + configure_source=configure, + ) + + assert events == ["resolve", "configure", "add"] + + +@pytest.mark.parametrize("is_finalized", [False, True]) +def test_default_eager_articulation_is_configured_after_native_add( + is_finalized: bool, +) -> None: + events: list[str] = [] + descriptor = SimpleNamespace(name="robot", per_env=True, links=[]) + result = SimpleNamespace(backend="dexsim", handles={}) + builder = SimpleNamespace( + backend="dexsim", + is_finalized=is_finalized, + result=result, + replicate_plan=SimpleNamespace(env_names=lambda: ["arena_0"]), + ) + + def configure(value: object) -> None: + assert value.links[0].name == "base" + events.append("configure") + + def add_articulation(value: object) -> object: + events.append("add") + value.links = [SimpleNamespace(name="base")] + result.handles["arena_0/robot"] = SimpleNamespace( + articulation_desc=value, + apply_dexsim_properties=lambda source: events.append("apply"), + ) + return value + + builder.add_articulation = add_articulation + scene = object.__new__(SpawnScene) + scene.builder = builder + scene._assets = {} + + scene.declare( + "articulation", + "robot", + descriptor, + configure_source=configure, + ) + + assert events == ["add", "configure", "apply"] + + +def test_source_configuration_retries_failure_then_runs_only_once() -> None: + events: list[str] = [] + descriptor = SimpleNamespace(name="robot", per_env=True, links=[]) + builder = SimpleNamespace( + is_finalized=False, + result=None, + replicate_plan=SimpleNamespace(env_names=lambda: ["arena_0"]), + add_articulation=lambda value: value, + ) + + def resolve_sources() -> None: + events.append("resolve") + descriptor.links = [SimpleNamespace(name="base")] + + attempts = 0 + + def configure(_value: object) -> None: + nonlocal attempts + attempts += 1 + events.append("configure") + if attempts == 1: + raise RuntimeError("configuration failed") + + builder.resolve_sources = resolve_sources + scene = object.__new__(SpawnScene) + scene.builder = builder + scene._assets = {} + + scene.declare( + "articulation", + "robot", + descriptor, + configure_source=configure, + ) + + with pytest.raises(RuntimeError, match="configuration failed"): + scene.resolve_sources() + scene.resolve_sources() + scene.resolve_sources() + + assert attempts == 2 + assert events == [ + "resolve", + "configure", + "resolve", + "configure", + "resolve", + ] + + +class _RetryableArticulation(Articulation): + bind_attempts = 0 + reset_attempts = 0 + + def __init__( + self, + cfg: object, + device: object = "cpu", + ) -> None: + self.cfg = cfg + self.uid = cfg.uid + self.device = device + self._entities: list[object] = [] + self._spawn_result = None + self._world = None + self._declared_num_instances: int | None = None + + def _initialize_spawn_declaration(self, num_instances: int) -> None: + self._declared_num_instances = num_instances + + def _initialize_spawn_bound(self, result: object) -> None: + self._spawn_result = result + self._world = object() + + def attach_spawn_handles(self, entities: list[object]) -> None: + self._entities = list(entities) + + def _apply_spawn_config(self) -> None: + type(self).bind_attempts += 1 + if type(self).bind_attempts == 1: + raise RuntimeError("configuration failed") + + def reset(self, env_ids: object | None = None) -> None: + del env_ids + type(self).reset_attempts += 1 + + +def test_articulation_binding_is_atomic_and_retryable() -> None: + _RetryableArticulation.bind_attempts = 0 + _RetryableArticulation.reset_attempts = 0 + facade = _RetryableArticulation( + SimpleNamespace(uid="robot"), + ) + facade._initialize_spawn_declaration(1) + result = object() + handles = [object()] + facade.attach_spawn_handles(handles) + + with pytest.raises(RuntimeError, match="configuration failed"): + facade.bind_spawn(result) + + assert facade.is_declared + assert _RetryableArticulation.reset_attempts == 0 + facade.bind_spawn(result) + assert facade.is_spawn_bound + assert facade._entities == handles + assert _RetryableArticulation.reset_attempts == 1 diff --git a/tests/sim/spawn/test_source.py b/tests/sim/spawn/test_source.py new file mode 100644 index 000000000..dbe0a96f7 --- /dev/null +++ b/tests/sim/spawn/test_source.py @@ -0,0 +1,209 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for backend-neutral URDF source mass-property handling.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import numpy as np +import pytest +from dexsim.spawn import ArticulationDesc, LinkDesc + +from embodichain.lab.sim.spawn.source import ( + _apply_dexsim_source_overlay, + _capture_dexsim_source_physics, + _clear_invalid_source_com, +) + +pytestmark = pytest.mark.no_sim + + +def _link(name: str) -> LinkDesc: + return LinkDesc(name, "", np.eye(4, dtype=np.float32)) + + +def _physical_attr( + mass: float, + inertia: tuple[float, float, float], + com_position: tuple[float, float, float] = (0.1, 0.2, 0.3), +) -> SimpleNamespace: + return SimpleNamespace( + mass=mass, + inertia=np.asarray(inertia, dtype=np.float32), + com_position=np.asarray(com_position, dtype=np.float32), + com_quaternion=np.asarray((1.0, 0.0, 0.0, 0.0), dtype=np.float32), + ) + + +def test_default_source_capture_keeps_valid_inertia_and_discards_invalid_com( + tmp_path, +) -> None: + urdf_path = tmp_path / "source.urdf" + urdf_path.write_text( + """ + + + + + + + + + + + + + + + + """, + encoding="utf-8", + ) + links = [_link("valid"), _link("invalid"), _link("unowned")] + desc = ArticulationDesc( + name="source", + links=links, + urdf_path=str(urdf_path), + ) + attrs = { + "valid": _physical_attr(2.0, (1.0, 2.0, 3.0)), + # The native loader exposes an epsilon tensor for the zero source + # inertia. The source XML, not this fallback, controls provenance. + "invalid": _physical_attr(0.5, (1.0e-6, 1.0e-6, 1.0e-6)), + "unowned": _physical_attr(1.0, (0.2, 0.3, 0.4)), + } + handle = SimpleNamespace(get_physical_attr=lambda name: attrs[name]) + + _capture_dexsim_source_physics(handle, desc) + + valid = desc.get_link_desc("valid") + assert valid.rigid_body is not None + assert valid.rigid_body.mass == pytest.approx(2.0) + np.testing.assert_array_equal(valid.rigid_body.inertia, (1.0, 2.0, 3.0)) + np.testing.assert_allclose(valid.rigid_body.com_position, (0.1, 0.2, 0.3)) + assert valid._inertia_from_source + + invalid = desc.get_link_desc("invalid") + assert invalid.rigid_body is not None + assert invalid.rigid_body.mass == pytest.approx(0.5) + assert invalid.rigid_body.inertia is None + assert invalid.rigid_body.com_position is None + assert invalid.rigid_body.com_quaternion is None + assert not invalid._inertia_from_source + + unowned = desc.get_link_desc("unowned") + assert unowned.rigid_body is not None + assert unowned.rigid_body.mass is None + assert unowned.rigid_body.inertia is None + assert not unowned._embodichain_source_inertia_valid + + +def test_invalid_source_com_normalization_does_not_touch_authored_inertia() -> None: + source = _link("source") + source.rigid_body = source_body = _physical_body( + inertia=None, + com_position=np.asarray((1.0, 2.0, 3.0), dtype=np.float32), + ) + authored = _link("authored") + authored.rigid_body = authored_body = _physical_body( + inertia=np.asarray((1.0, 2.0, 3.0), dtype=np.float32), + com_position=np.asarray((4.0, 5.0, 6.0), dtype=np.float32), + ) + authored._inertia_from_source = True + desc = ArticulationDesc(name="robot", links=[source, authored]) + source._embodichain_source_inertia_valid = False + source._embodichain_has_collision_geometry = True + + _clear_invalid_source_com(desc) + + assert source_body.com_position is None + np.testing.assert_array_equal(authored_body.com_position, (4.0, 5.0, 6.0)) + + +def test_default_overlay_writes_only_explicitly_marked_link_physics() -> None: + skipped = _link("skipped") + applied = _link("applied") + for link in (skipped, applied): + link.rigid_body = _physical_body( + inertia=np.asarray((1.0, 2.0, 3.0), dtype=np.float32), + com_position=np.asarray((0.1, 0.2, 0.3), dtype=np.float32), + ) + skipped._embodichain_apply_physics = False + applied._embodichain_apply_physics = True + applied._embodichain_mass_override = True + applied.rigid_body.mass = 2.0 + desc = ArticulationDesc(name="robot", links=[skipped, applied]) + + raw = _RawBody() + binding = SimpleNamespace( + get_physical_body=lambda name: raw if name == "applied" else None, + set_physical_attr=lambda _attr, name, _replace: raw.calls.append( + ("attr", name) + ), + ) + handle = SimpleNamespace( + _physics_binding=binding, + articulation_desc=ArticulationDesc(name="robot", links=[]), + _desc_shared=True, + ) + + _apply_dexsim_source_overlay(handle, desc) + + assert raw.calls[0] == ("attr", "applied") + assert raw.mass == pytest.approx(2.0) + np.testing.assert_array_equal(raw.inertia, (1.0, 2.0, 3.0)) + np.testing.assert_allclose(raw.com_position, (0.1, 0.2, 0.3)) + assert handle.articulation_desc.get_link_desc("skipped").rigid_body is not None + + +class _RawBody: + def __init__(self) -> None: + self.calls: list[tuple[str, str]] = [] + self.mass = 1.0 + self.inertia = np.ones(3, dtype=np.float32) + self.com_position = np.zeros(3, dtype=np.float32) + self.com_quaternion = np.asarray((1.0, 0.0, 0.0, 0.0), dtype=np.float32) + + def set_mass(self, mass: float) -> None: + self.mass = mass + + def set_mass_space_inertia_tensor(self, inertia: np.ndarray) -> None: + self.inertia = np.asarray(inertia, dtype=np.float32) + + def get_cmass_local_pose(self) -> tuple[np.ndarray, np.ndarray]: + return self.com_position, self.com_quaternion + + def set_cmass_local_pose( + self, + position: np.ndarray, + quaternion: np.ndarray, + ) -> None: + self.com_position = np.asarray(position, dtype=np.float32) + self.com_quaternion = np.asarray(quaternion, dtype=np.float32) + + +def _physical_body(*, inertia, com_position): + from dexsim.spawn import RigidBodyPhysicsDesc + + return RigidBodyPhysicsDesc.dynamic( + mass=1.0, + inertia=inertia, + com_position=com_position, + ) diff --git a/tests/sim/test_backend_parity.py b/tests/sim/test_backend_parity.py new file mode 100644 index 000000000..c6a1cf72a --- /dev/null +++ b/tests/sim/test_backend_parity.py @@ -0,0 +1,216 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Backend capability parity matrix. + +This is the single source of truth for which simulation features each physics +backend supports. It pins the capability contract so that: + +- flipping a ``supports_*`` flag (or adding a backend) fails loudly, and +- every ``SimulationManager.add_*`` capability guard maps 1:1 to its flag. + +Headless (no GPU / no dexsim world): backends are constructed with a minimal +fake owning-manager back-ref, and the ``add_*`` guard mapping is exercised by +binding a fake ``physics`` onto a bare ``SimulationManager`` via +``object.__new__`` (mirroring the lifecycle-test pattern). +""" + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from embodichain.lab.sim.physics import ( + DefaultPhysicsBackend, + NewtonPhysicsBackend, + PhysicsBackend, +) +from embodichain.lab.sim.sim_manager import SimulationManager + +# --------------------------------------------------------------------------- +# The parity matrix — edit this table when a backend gains/loses a feature. +# --------------------------------------------------------------------------- +# feature -> {backend -> supported} +BACKEND_CAPABILITIES: dict[str, dict[str, bool]] = { + "robot": {"default": True, "newton": True}, + "volume_deformables": {"default": False, "newton": True}, + "surface_deformables": {"default": False, "newton": True}, + "soft_bodies": {"default": False, "newton": True}, + "cloth": {"default": False, "newton": True}, + "rigid_object_group": {"default": True, "newton": True}, + "rigid_constraints": {"default": True, "newton": False}, + "contact_sensor": {"default": True, "newton": False}, + "can_disable_manual_update": {"default": True, "newton": False}, +} + +BACKENDS: dict[str, type[PhysicsBackend]] = { + "default": DefaultPhysicsBackend, + "newton": NewtonPhysicsBackend, +} + +# Map each capability flag to the SimulationManager.add_* method whose +# NotImplementedError guard consults it. ``None`` means the flag is consulted +# elsewhere (e.g. set_manual_update) rather than an add_* guard. +CAPABILITY_TO_ADD_METHOD: dict[str, str | None] = { + "robot": "add_robot", + "volume_deformables": "add_deformable_object", + "surface_deformables": "add_deformable_object", + "soft_bodies": None, + "cloth": None, + "rigid_object_group": "add_rigid_object_group", + "rigid_constraints": None, + "contact_sensor": None, + "can_disable_manual_update": None, +} + + +def _make_backend(name: str) -> PhysicsBackend: + """Construct a backend with a minimal fake owning-manager back-ref.""" + return BACKENDS[name](SimpleNamespace()) + + +@pytest.mark.parametrize("backend_name", list(BACKENDS)) +def test_backend_name_matches(backend_name: str) -> None: + backend = _make_backend(backend_name) + assert backend.name == backend_name + + +@pytest.mark.parametrize("backend_name", list(BACKENDS)) +@pytest.mark.parametrize( + "feature", [f for f in BACKEND_CAPABILITIES if f != "can_disable_manual_update"] +) +def test_supports_flags_match_matrix(backend_name: str, feature: str) -> None: + """Each backend's supports_* property matches the parity matrix.""" + backend = _make_backend(backend_name) + expected = BACKEND_CAPABILITIES[feature][backend_name] + actual = getattr(backend, f"supports_{feature}") + assert ( + actual is expected + ), f"{backend_name}.supports_{feature} = {actual}, matrix says {expected}" + + +@pytest.mark.parametrize("backend_name", list(BACKENDS)) +def test_can_disable_manual_update_matches_matrix(backend_name: str) -> None: + backend = _make_backend(backend_name) + expected = BACKEND_CAPABILITIES["can_disable_manual_update"][backend_name] + assert backend.can_disable_manual_update is expected + + +def test_rigid_constraint_guard_uses_backend_capability() -> None: + """Rigid constraints fail at the capability boundary before world access.""" + backend = _make_backend("newton") + sim = _make_sim_with_backend(backend) + + with pytest.raises(NotImplementedError, match="rigid constraints"): + sim.create_rigid_constraint(cfg=SimpleNamespace()) + + +def test_contact_sensor_guard_uses_backend_capability() -> None: + """Contact sensors fail at the capability boundary before preparation.""" + backend = _make_backend("newton") + sim = _make_sim_with_backend(backend) + sim._sensors = {} + sim.SUPPORTED_SENSOR_TYPES = {"ContactSensor": object()} + sensor_cfg = SimpleNamespace(sensor_type="ContactSensor", uid="contact") + + with pytest.raises(NotImplementedError, match="ContactSensor"): + sim.add_sensor(sensor_cfg) + + +def _make_sim_with_backend(backend: PhysicsBackend) -> SimulationManager: + """Build a bare SimulationManager whose ``physics`` is the given backend. + + The add_* capability guards consult only ``self.physics.supports_*`` (plus a + few uid/existence checks that run after the guard), so a bare instance with + ``physics`` + the registries set is enough to assert the guard fires. + """ + sim = object.__new__(SimulationManager) + sim.physics = backend + sim._deformable_objects = {} + sim._rigid_object_groups = {} + sim._robots = {} + sim._rigid_objects = {} + sim._articulations = {} + return sim + + +@pytest.mark.parametrize( + "feature,add_method", + [(f, m) for f, m in CAPABILITY_TO_ADD_METHOD.items() if m is not None], +) +@pytest.mark.parametrize("backend_name", list(BACKENDS)) +def test_add_method_guard_maps_to_capability( + backend_name: str, feature: str, add_method: str +) -> None: + """add_ raises NotImplementedError iff the backend lacks the flag. + + For unsupported features the guard must fire before any world access; for + supported features the method proceeds past the guard (and is expected to + fail later on the missing world — we only assert it does NOT raise + NotImplementedError at the guard). + """ + backend = _make_backend(backend_name) + sim = _make_sim_with_backend(backend) + supported = BACKEND_CAPABILITIES[feature][backend_name] + method = getattr(sim, add_method) + + # Deformable dispatch needs its topology discriminator before the guard. + deformable_types = { + "volume_deformables": "volume", + "surface_deformables": "surface", + } + cfg = SimpleNamespace(uid=None) + if feature in deformable_types: + cfg.deformable_type = deformable_types[feature] + + if supported: + # Past the guard it will hit missing-world attrs; assert the failure is + # NOT the capability NotImplementedError. + with pytest.raises(Exception) as exc_info: + method(cfg=cfg) + assert not isinstance(exc_info.value, NotImplementedError), ( + f"{add_method} raised NotImplementedError on the {backend_name} " + f"backend despite supports_{feature}=True" + ) + assert "not enabled" not in str(exc_info.value) + else: + with pytest.raises(NotImplementedError): + method(cfg=cfg) + + +def test_matrix_covers_all_capability_flags() -> None: + """Every supports_* / can_disable_manual_update flag is in the matrix.""" + flag_names = { + name[len("supports_") :] if name.startswith("supports_") else name + for name in dir(PhysicsBackend) + if name.startswith("supports_") or name == "can_disable_manual_update" + } + matrix_features = set(BACKEND_CAPABILITIES) + assert ( + flag_names == matrix_features + ), f"capability flags {flag_names} != matrix features {matrix_features}" + + +def test_matrix_covers_all_backends() -> None: + """Every concrete backend class is in the matrix.""" + # Discover concrete (non-abstract) backends by instantiation. + concrete = set(BACKENDS) + matrix_backends = {b for feats in BACKEND_CAPABILITIES.values() for b in feats} + assert concrete == matrix_backends + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/tests/sim/test_batch_entity.py b/tests/sim/test_batch_entity.py new file mode 100644 index 000000000..78bd7cd0c --- /dev/null +++ b/tests/sim/test_batch_entity.py @@ -0,0 +1,55 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +from __future__ import annotations + +from types import SimpleNamespace + +import torch + +from embodichain.lab.sim.common import BatchEntity + + +class _BatchEntityForTest(BatchEntity): + def __init__(self) -> None: + self.reset_calls = 0 + cfg = SimpleNamespace(uid="test_entity") + super().__init__( + cfg=cfg, + entities=[object()], + device=torch.device("cpu"), + ) + + def set_local_pose(self, pose, env_ids=None) -> None: + pass + + def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: + return torch.empty(0) + + def reset(self, env_ids=None) -> None: + self.reset_calls += 1 + + +def test_batch_entity_does_not_reset_in_constructor() -> None: + entity = _BatchEntityForTest() + + assert entity.reset_calls == 0 + + +def test_batch_entity_reset_is_explicit() -> None: + entity = _BatchEntityForTest() + entity.reset() + + assert entity.reset_calls == 1 diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index 9b379fa28..856676518 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -16,73 +16,878 @@ from __future__ import annotations +from dataclasses import fields + import dexsim import pytest +import embodichain.lab.sim.cfg as sim_cfg + +from dexsim.engine.newton_physics import ( + NewtonCollisionPipelineCfg as SpawnNewtonCollisionPipelineCfg, +) +from dexsim.spawn import DexsimCollisionDesc, DexsimPhysicsDesc, NewtonCollisionDesc from dexsim.types import DenoiserType, Renderer, ToneMappingType from embodichain.lab.sim.cfg import ( ArticulationCfg, + ArticulationRootPropertiesCfg, + CollisionPropertiesCfg, + DefaultCollisionPropertiesCfg, + DefaultPhysicsCfg, + DefaultRigidBodyPropertiesCfg, DLSSCfg, - PhysicsCfg, + JointDrivePropertiesCfg, + LinkPhysicsOverrideCfg, + MassPropertiesCfg, + MeshCollisionCfg, + NewtonCollisionPipelineCfg, + NewtonCollisionPropertiesCfg, + NewtonJointDrivePropertiesCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + PhysicsBackendCfg, RenderCfg, + RigidBodyMaterialCfg, + RigidBodyPhysicsCfg, + RigidObjectCfg, RobotCfg, + RobotPresetCfg, + physics_cfg_for_backend, ) +from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg +from embodichain.utils import configclass + + +def test_cfg_package_preserves_the_public_facade() -> None: + from embodichain.lab.sim.cfg.rigid import ( + RigidBodyPhysicsCfg as LeafRigidBodyPhysicsCfg, + ) + from embodichain.lab.sim.cfg.robot import RobotCfg as LeafRobotCfg + assert hasattr(sim_cfg, "__path__") + assert not hasattr(sim_cfg, "PhysicsCfg") + assert sim_cfg.RigidBodyPhysicsCfg is LeafRigidBodyPhysicsCfg + assert sim_cfg.RobotCfg is LeafRobotCfg -def test_articulation_cfg_defaults_to_no_joint_drive() -> None: - """Generic articulations are passive unless a drive is requested.""" + +def test_articulation_cfg_defaults_to_preserving_asset_physics() -> None: + """Generic articulations do not author source drive properties.""" articulation_cfg = ArticulationCfg() - assert articulation_cfg.drive_pros.drive_type == "none" + assert articulation_cfg.joint_drive_props is None + assert articulation_cfg.resolve_asset_physics_mode() == "preserve" + + +def test_articulation_cfg_uses_grouped_physics_fields_only() -> None: + field_names = {item.name for item in fields(ArticulationCfg)} + root_props = ArticulationCfg().root_props + assert { + "fix_base", + "disable_self_collision", + "sleep_threshold", + "min_position_iters", + "min_velocity_iters", + "articulation_props", + "drive_pros", + "joint_props", + }.isdisjoint(field_names) + assert root_props == ArticulationRootPropertiesCfg() + assert root_props.fixed_base is True + assert root_props.self_collision_enabled is False -def test_articulation_cfg_partial_drive_properties_preserve_no_drive() -> None: - """Partial articulation drive overrides retain the passive default.""" + +@pytest.mark.parametrize( + "field_name", + [ + "fix_base", + "disable_self_collision", + "sleep_threshold", + "min_position_iters", + "min_velocity_iters", + "articulation_props", + "drive_pros", + "joint_props", + ], +) +def test_removed_articulation_fields_fail_with_migration_target( + field_name: str, +) -> None: + with pytest.raises(ValueError, match=f"{field_name} ->"): + ArticulationCfg.from_dict({field_name: True}) + + with pytest.raises(ValueError, match=f"{field_name} ->"): + merge_robot_cfg(RobotCfg(), {field_name: True}) + + +def test_physics_cfg_factory_rejects_noncanonical_backend_names() -> None: + with pytest.raises(ValueError, match="expected 'default' or 'newton'"): + physics_cfg_for_backend("alternate") # type: ignore[arg-type] + + +def test_articulation_cfg_parses_sparse_drive_overrides() -> None: + """Unspecified drive fields remain source-owned.""" articulation_cfg = ArticulationCfg.from_dict( - {"drive_pros": {"stiffness": 0.0, "damping": 0.0}} + {"joint_drive_props": {"stiffness": 0.0, "damping": 0.0}} + ) + + assert articulation_cfg.joint_drive_props.drive_type is None + assert articulation_cfg.joint_drive_props.stiffness == 0.0 + assert articulation_cfg.joint_drive_props.damping == 0.0 + assert articulation_cfg.joint_drive_props.max_effort is None + + +def test_robot_cfg_defaults_to_portable_position_velocity_drive() -> None: + """The original force drive resolves to position+velocity targets.""" + robot_cfg = RobotCfg() + + assert robot_cfg.joint_drive_props.drive_type == "force" + assert robot_cfg.joint_drive_props.target_mode is None + assert robot_cfg.joint_drive_props._resolve_modes() == ( + "position_velocity", + "force", + ) + assert robot_cfg.resolve_asset_physics_mode() == "overlay" + + +def test_robot_cfg_partial_drive_properties_preserve_portable_drive() -> None: + """Partial robot drive overrides retain the original force mode.""" + robot_cfg = RobotCfg.from_dict( + {"joint_drive_props": {"stiffness": 0.0, "damping": 0.0}} + ) + + assert robot_cfg.joint_drive_props.drive_type == "force" + assert robot_cfg.joint_drive_props.target_mode is None + assert robot_cfg.joint_drive_props._resolve_modes() == ( + "position_velocity", + "force", ) - assert articulation_cfg.drive_pros.drive_type == "none" +def test_drive_type_override_replaces_robot_force_default() -> None: + override = {"joint_drive_props": {"drive_type": "none"}} + robot_cfg = RobotCfg.from_dict(override) + merged_cfg = merge_robot_cfg(RobotCfg(), override) -def test_articulation_cfg_enables_gravity_by_default() -> None: - """Articulations opt into gravity unless explicitly configured otherwise.""" - assert ArticulationCfg().enable_gravity is True + for cfg in (robot_cfg, merged_cfg): + assert cfg.joint_drive_props.target_mode is None + assert cfg.joint_drive_props.drive_type == "none" + assert cfg.joint_drive_props._resolve_modes() == ("none", "none") -def test_articulation_cfg_parses_disabled_gravity() -> None: - """Dictionary configuration can disable articulation gravity.""" - articulation_cfg = ArticulationCfg.from_dict({"enable_gravity": False}) +def test_common_target_mode_does_not_require_newton_subclass() -> None: + articulation_cfg = ArticulationCfg.from_dict( + { + "joint_drive_props": { + "target_mode": "effort", + "drive_type": "force", + } + } + ) - assert articulation_cfg.enable_gravity is False + assert type(articulation_cfg.joint_drive_props) is JointDrivePropertiesCfg + assert articulation_cfg.joint_drive_props.target_mode == "effort" + assert articulation_cfg.joint_drive_props.drive_type == "force" -def test_robot_cfg_defaults_to_force_joint_drive() -> None: - """Robots retain force-based joint drives by default.""" +def test_asset_physics_policy_uses_explicit_modes() -> None: + rigid_cfg = RigidObjectCfg() + articulation_cfg = ArticulationCfg() robot_cfg = RobotCfg() + overlay_cfg = ArticulationCfg(asset_physics_mode="overlay") + + assert rigid_cfg.asset_physics_mode == "preserve" + assert rigid_cfg.resolve_asset_physics_mode() == "preserve" + assert articulation_cfg.asset_physics_mode == "preserve" + assert articulation_cfg.resolve_asset_physics_mode() == "preserve" + assert robot_cfg.asset_physics_mode == "overlay" + assert robot_cfg.resolve_asset_physics_mode() == "overlay" + assert overlay_cfg.resolve_asset_physics_mode() == "overlay" + + invalid_cfg = RigidObjectCfg(asset_physics_mode="replace") # type: ignore[arg-type] + with pytest.raises(ValueError, match="must be 'preserve' or 'overlay'"): + invalid_cfg.resolve_asset_physics_mode() + + +def test_articulation_cfg_parses_polymorphic_newton_joint_drive() -> None: + articulation_cfg = ArticulationCfg.from_dict( + { + "joint_drive_props": { + "backend": "newton", + "stiffness": {"arm_.*": 25.0}, + "target_mode": "position", + } + } + ) + + assert articulation_cfg.joint_drive_props.drive_type is None + assert isinstance(articulation_cfg.joint_drive_props, NewtonJointDrivePropertiesCfg) + assert articulation_cfg.joint_drive_props.stiffness == {"arm_.*": 25.0} + assert articulation_cfg.joint_drive_props.target_mode == "position" + + +def test_joint_drive_from_dict_preserves_newton_subclass_defaults() -> None: + defaults = NewtonJointDrivePropertiesCfg( + stiffness=10.0, + target_mode="position", + ) + + cfg = JointDrivePropertiesCfg.from_dict( + {"damping": 4.0}, + defaults=defaults, + ) + + assert isinstance(cfg, NewtonJointDrivePropertiesCfg) + assert cfg.stiffness == 10.0 + assert cfg.damping == 4.0 + assert cfg.target_mode == "position" + + +def test_robot_cfg_merge_preserves_typed_backend_property_configs() -> None: + base = RobotCfg( + joint_drive_props=NewtonJointDrivePropertiesCfg( + stiffness=10.0, + target_mode="position", + ), + attrs=RigidBodyPhysicsCfg( + collision_props=NewtonCollisionPropertiesCfg(margin=0.01), + material_props=NewtonRigidBodyMaterialCfg(ke=1000.0), + ), + ) + + merged = merge_robot_cfg( + base, + { + "joint_drive_props": {"backend": "newton", "damping": 4.0}, + "attrs": {"material_props": {"backend": "newton", "kd": 50.0}}, + }, + ) + + assert isinstance(merged.joint_drive_props, NewtonJointDrivePropertiesCfg) + assert merged.joint_drive_props.stiffness == 10.0 + assert merged.joint_drive_props.damping == 4.0 + assert merged.joint_drive_props.target_mode == "position" + assert isinstance(merged.attrs, RigidBodyPhysicsCfg) + assert isinstance(merged.attrs.material_props, NewtonRigidBodyMaterialCfg) + assert merged.attrs.material_props.ke == 1000.0 + assert merged.attrs.material_props.kd == 50.0 + + +def test_rigid_physics_uses_one_slot_per_physical_concept() -> None: + """Backend blocks and geometry cooking are not parallel physics owners.""" + assert {item.name for item in fields(RigidBodyPhysicsCfg)} == { + "mass_props", + "rigid_props", + "collision_props", + "material_props", + } + assert issubclass(DefaultCollisionPropertiesCfg, CollisionPropertiesCfg) + assert issubclass(NewtonCollisionPropertiesCfg, CollisionPropertiesCfg) + assert issubclass(NewtonRigidBodyMaterialCfg, RigidBodyMaterialCfg) + assert issubclass(NewtonJointDrivePropertiesCfg, JointDrivePropertiesCfg) + for removed_name in ( + "DefaultRigidBodyPhysicsCfg", + "NewtonRigidBodyPhysicsCfg", + "MeshCollisionPropertiesCfg", + "NewtonMeshCollisionPropertiesCfg", + ): + assert not hasattr(sim_cfg, removed_name) + + +def test_backend_property_groups_track_dexsim_spawn_descriptors() -> None: + def names(config_type: type) -> set[str]: + return {item.name for item in fields(config_type)} + + assert names(DefaultRigidBodyPropertiesCfg) == names(DexsimPhysicsDesc) + default_collision_fields = ( + (names(CollisionPropertiesCfg) - {"collision_enabled"}) + | (names(DefaultCollisionPropertiesCfg) - names(CollisionPropertiesCfg)) + | names(RigidBodyMaterialCfg) + ) + assert default_collision_fields == names(DexsimCollisionDesc) + + newton_fields = ( + names(NewtonCollisionPropertiesCfg) - names(CollisionPropertiesCfg) + ) | (names(NewtonRigidBodyMaterialCfg) - names(RigidBodyMaterialCfg)) + newton_fields.remove("torsional_friction") + newton_fields.remove("rolling_friction") + newton_fields.update( + { + "mu", + "restitution", + "mu_torsional", + "mu_rolling", + "is_hydroelastic", + "sdf_narrow_band_range", + "sdf_target_voxel_size", + "sdf_max_resolution", + "sdf_texture_format", + "force_sdf", + "sdf_padding", + } + ) + intentionally_unowned_shape_fields = { + "is_solid", + "collision_group", + "collision_filter_parent", + "has_particle_collision", + "is_visible", + "is_site", + } + assert ( + newton_fields == names(NewtonCollisionDesc) - intentionally_unowned_shape_fields + ) + + assert names(NewtonCollisionPipelineCfg) == names( + SpawnNewtonCollisionPipelineCfg + ) - {"requires_grad"} + + +def test_rigid_physics_from_dict_selects_backend_subclasses() -> None: + cfg = RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 2.0}, + "rigid_props": {"backend": "default", "has_gravity": False}, + "collision_props": {"backend": "newton", "margin": 0.01}, + "material_props": { + "backend": "newton", + "dynamic_friction": 0.4, + "ke": 1000.0, + }, + } + ) + + assert isinstance(cfg.mass_props, MassPropertiesCfg) + assert isinstance(cfg.rigid_props, DefaultRigidBodyPropertiesCfg) + assert isinstance(cfg.collision_props, NewtonCollisionPropertiesCfg) + assert isinstance(cfg.material_props, NewtonRigidBodyMaterialCfg) + + +def test_recompute_inertia_is_a_mass_property() -> None: + cfg = RigidBodyPhysicsCfg.from_dict( + {"mass_props": {"mass": 2.0, "recompute_inertia": True}} + ) + + assert "replace_inertial" not in { + item.name for item in fields(LinkPhysicsOverrideCfg) + } + assert cfg.mass_props.recompute_inertia is True + assert cfg.to_dict()["mass_props"]["recompute_inertia"] is True + + link_cfg = LinkPhysicsOverrideCfg.from_dict( + { + "link_names_expr": ["finger_.*"], + "attrs": {"mass_props": {"recompute_inertia": True}}, + } + ) + assert link_cfg.attrs.mass_props.recompute_inertia is True + + with pytest.raises(ValueError, match="attrs.mass_props.recompute_inertia"): + LinkPhysicsOverrideCfg.from_dict( + { + "link_names_expr": ["finger_.*"], + "replace_inertial": True, + } + ) + + +@pytest.mark.parametrize( + ("removed_field", "replacement"), + [ + ("default_props", "polymorphic property slot"), + ("newton_props", "polymorphic property slot"), + ("mesh_collision_props", "MeshCfg.collision"), + ], +) +def test_rigid_physics_rejects_removed_parallel_owners( + removed_field: str, + replacement: str, +) -> None: + with pytest.raises(ValueError, match=replacement): + RigidBodyPhysicsCfg.from_dict({removed_field: {}}) + + +def test_articulation_cfg_parses_joint_drive_and_dynamics() -> None: + cfg = ArticulationCfg.from_dict( + { + "joint_drive_props": { + "stiffness": 12.0, + "max_effort": 20.0, + "friction": {"arm_.*": 0.2}, + }, + } + ) + + assert cfg.joint_drive_props.stiffness == pytest.approx(12.0) + assert cfg.joint_drive_props.max_effort == pytest.approx(20.0) + assert cfg.joint_drive_props.friction == {"arm_.*": 0.2} + + +def test_robot_cfg_merge_composes_single_slot_and_joint_drive_properties() -> None: + base = RobotCfg( + attrs=RigidBodyPhysicsCfg( + rigid_props=DefaultRigidBodyPropertiesCfg(linear_damping=0.1), + ), + joint_drive_props=JointDrivePropertiesCfg( + max_effort={"arm": 10.0}, + friction=0.1, + ), + ) + + merged = merge_robot_cfg( + base, + { + "attrs": { + "rigid_props": { + "backend": "default", + "angular_damping": 0.2, + }, + }, + "joint_drive_props": { + "max_effort": {"wrist": 20.0}, + "armature": 0.3, + }, + }, + ) + + assert merged.attrs.rigid_props.linear_damping == pytest.approx(0.1) + assert merged.attrs.rigid_props.angular_damping == pytest.approx(0.2) + assert merged.joint_drive_props.max_effort == {"arm": 10.0, "wrist": 20.0} + assert merged.joint_drive_props.friction == pytest.approx(0.1) + assert merged.joint_drive_props.armature == pytest.approx(0.3) + + +def test_portable_collision_envelope_round_trips_as_common_config() -> None: + cfg = RigidBodyPhysicsCfg.from_dict( + { + "collision_props": { + "collision_enabled": True, + "contact_offset": 0.01, + "rest_offset": 0.002, + } + } + ) + + assert type(cfg.collision_props) is CollisionPropertiesCfg + assert cfg.to_dict()["collision_props"] == { + "collision_enabled": True, + "contact_offset": 0.01, + "rest_offset": 0.002, + } + + +def test_portable_collision_envelope_uses_the_shared_default_profile() -> None: + common = CollisionPropertiesCfg() + + assert common.contact_offset == pytest.approx(0.002) + assert common.rest_offset == pytest.approx(0.001) + assert DefaultCollisionPropertiesCfg().contact_offset is None + assert DefaultCollisionPropertiesCfg().rest_offset is None + assert NewtonCollisionPropertiesCfg().contact_offset is None + assert NewtonCollisionPropertiesCfg().rest_offset is None + + +@configclass +class _RobotPhysicsPresetCfg(RobotPresetCfg): + default: RobotCfg = RobotCfg(uid="default") + newton: RobotCfg = RobotCfg(uid="newton") + newton_xpbd: RobotCfg = RobotCfg(uid="newton_xpbd") + + +def test_robot_preset_selects_complete_backend_and_solver_variants() -> None: + preset = _RobotPhysicsPresetCfg() + + default_cfg = preset.resolve(DefaultPhysicsCfg()) + newton_cfg = preset.resolve(NewtonPhysicsCfg()) + xpbd_cfg = preset.resolve(NewtonPhysicsCfg(solver_cfg={"solver_type": "xpbd"})) + + assert default_cfg.uid == "default" + assert newton_cfg.uid == "newton" + assert xpbd_cfg.uid == "newton_xpbd" + assert default_cfg is not preset.default + + +@configclass +class _CommonRobotPresetCfg(RobotPresetCfg): + default: RobotCfg = RobotCfg(uid="portable") + + +def test_robot_preset_falls_back_to_one_portable_definition() -> None: + preset = _CommonRobotPresetCfg() + + assert preset.resolve(DefaultPhysicsCfg()).uid == "portable" + assert preset.resolve(NewtonPhysicsCfg()).uid == "portable" + + +@configclass +class _NewtonSolverAliasRobotPresetCfg(RobotPresetCfg): + default: RobotCfg = RobotCfg(uid="fallback") + newton_mjwarp: RobotCfg = RobotCfg(uid="mjwarp") + + +def test_robot_preset_accepts_newton_solver_alias() -> None: + preset = _NewtonSolverAliasRobotPresetCfg() + + assert preset.resolve(DefaultPhysicsCfg()).uid == "fallback" + assert preset.resolve(NewtonPhysicsCfg()).uid == "fallback" + assert ( + preset.resolve(NewtonPhysicsCfg(solver_cfg={"solver_type": "mjwarp"})).uid + == "mjwarp" + ) + + +@configclass +class _UnsupportedRobotPresetCfg(RobotPresetCfg): + default: RobotCfg = RobotCfg(uid="default") + alternate: RobotCfg = RobotCfg(uid="alternate") + + +def test_robot_preset_rejects_noncanonical_backend_names() -> None: + with pytest.raises(TypeError, match="unsupported preset name"): + _UnsupportedRobotPresetCfg().resolve(DefaultPhysicsCfg()) + + +def test_backend_property_configs_round_trip_without_losing_subclasses() -> None: + cfg = RigidBodyPhysicsCfg( + collision_props=NewtonCollisionPropertiesCfg(margin=0.01), + material_props=NewtonRigidBodyMaterialCfg(ke=1000.0), + ) + + serialized = cfg.to_dict() + restored = RigidBodyPhysicsCfg.from_dict(serialized) + + assert serialized["rigid_props"] is None + assert serialized["collision_props"]["backend"] == "newton" + assert serialized["material_props"]["backend"] == "newton" + assert isinstance(restored.collision_props, NewtonCollisionPropertiesCfg) + assert isinstance(restored.material_props, NewtonRigidBodyMaterialCfg) + + +def test_default_property_configs_use_the_default_discriminator() -> None: + cfg = RigidBodyPhysicsCfg( + rigid_props=DefaultRigidBodyPropertiesCfg(linear_damping=0.2), + collision_props=DefaultCollisionPropertiesCfg( + contact_offset=0.01, + disable_strong_friction=True, + ), + material_props=RigidBodyMaterialCfg(dynamic_friction=0.5), + ) + + serialized = cfg.to_dict() + restored = RigidBodyPhysicsCfg.from_dict(serialized) + + assert serialized["rigid_props"]["backend"] == "default" + assert serialized["collision_props"]["backend"] == "default" + assert "backend" not in serialized["material_props"] + assert isinstance(restored.rigid_props, DefaultRigidBodyPropertiesCfg) + assert isinstance(restored.collision_props, DefaultCollisionPropertiesCfg) + assert type(restored.material_props) is RigidBodyMaterialCfg + + +def test_backend_property_parser_infers_unique_fields_without_discriminator() -> None: + cfg = RigidBodyPhysicsCfg.from_dict( + { + "rigid_props": {"linear_damping": 0.2}, + "collision_props": {"margin": 0.01}, + "material_props": {"rolling_friction": 0.03}, + } + ) + + assert isinstance(cfg.rigid_props, DefaultRigidBodyPropertiesCfg) + assert isinstance(cfg.collision_props, NewtonCollisionPropertiesCfg) + assert isinstance(cfg.material_props, NewtonRigidBodyMaterialCfg) + + +def test_mesh_collision_cfg_requires_explicit_strategy_fields() -> None: + collision = MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=8, + acd_method="coacd", + ) + + assert collision.max_hulls == 8 + with pytest.raises(ValueError, match="valid only for convex_decomposition"): + MeshCollisionCfg(approximation="convex_hull", acd_method="coacd") + with pytest.raises(ValueError, match="only one"): + MeshCollisionCfg( + approximation="sdf", + sdf_resolution=64, + sdf_target_voxel_size=0.005, + ) + + +@pytest.mark.parametrize( + ("legacy_max_hulls", "expected_approximation", "expected_max_hulls"), + [ + (1, "convex_hull", None), + (4, "convex_decomposition", 4), + ], +) +def test_mesh_collision_cfg_accepts_deprecated_hull_count_alias( + legacy_max_hulls: int, + expected_approximation: str, + expected_max_hulls: int | None, +) -> None: + with pytest.warns(DeprecationWarning): + collision = MeshCollisionCfg(max_convex_hull_num=legacy_max_hulls) + + assert collision.approximation == expected_approximation + assert collision.max_hulls == expected_max_hulls + assert "max_convex_hull_num" not in collision.to_dict() + + +def test_mesh_collision_cfg_deprecated_hull_count_view_uses_canonical_value() -> None: + collision = MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=4, + ) + + assert collision.max_convex_hull_num == 4 + + +def test_mesh_collision_cfg_rejects_both_hull_count_names() -> None: + with ( + pytest.warns(DeprecationWarning), + pytest.raises(ValueError, match="cannot both be configured"), + ): + MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=4, + max_convex_hull_num=8, + ) + + +@pytest.mark.parametrize( + "collision_kwargs", + [ + {"approximation": "convex_decomposition", "max_hulls": 2.5}, + {"approximation": "sdf", "sdf_resolution": 64.5}, + {"approximation": "sdf", "sdf_padding": float("nan")}, + {"approximation": "sdf", "sdf_texture_format": "invalid"}, + ], +) +def test_mesh_collision_cfg_rejects_invalid_numeric_types_and_values( + collision_kwargs: dict[str, object], +) -> None: + with pytest.raises(ValueError): + MeshCollisionCfg(**collision_kwargs) + + +def test_mesh_cfg_legacy_collision_fields_normalize_to_nested_config() -> None: + with pytest.warns(DeprecationWarning): + cfg = RigidObjectCfg.from_dict( + { + "uid": "mesh", + "shape": { + "shape_type": "Mesh", + "fpath": "mesh.obj", + "max_convex_hull_num": 4, + "acd_method": "coacd", + }, + } + ) + + assert cfg.shape.collision == MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=4, + acd_method="coacd", + ) + serialized_shape = cfg.shape.to_dict() + assert "max_convex_hull_num" not in serialized_shape + assert serialized_shape["collision"]["approximation"] == "convex_decomposition" + + +def test_rigid_object_legacy_physics_mesh_collision_moves_to_shape() -> None: + with pytest.warns(DeprecationWarning): + cfg = RigidObjectCfg.from_dict( + { + "uid": "mesh", + "shape": {"shape_type": "Mesh", "fpath": "mesh.obj"}, + "attrs": { + "mesh_collision_props": {"max_convex_hull_num": 4}, + }, + } + ) + + assert cfg.shape.collision.approximation == "convex_decomposition" + assert cfg.shape.collision.max_hulls == 4 + assert "mesh_collision_props" not in cfg.attrs.to_dict() + + +def test_legacy_mesh_collision_physics_rejects_non_mesh_shape() -> None: + with pytest.raises(ValueError, match="only to a MeshCfg"): + RigidObjectCfg.from_dict( + { + "uid": "cube", + "shape": {"shape_type": "Cube", "size": [1.0, 1.0, 1.0]}, + "attrs": { + "mesh_collision_props": {"max_convex_hull_num": 4}, + }, + } + ) + + +def test_backend_joint_and_articulation_configs_round_trip() -> None: + drive = NewtonJointDrivePropertiesCfg(target_mode=None) + root = ArticulationRootPropertiesCfg(fixed_base=False) + + restored_drive = JointDrivePropertiesCfg.from_dict(drive.to_dict()) + restored_root = ArticulationRootPropertiesCfg.from_dict(root.to_dict()) + + assert isinstance(restored_drive, NewtonJointDrivePropertiesCfg) + assert root.to_dict() == { + "fixed_base": False, + "self_collision_enabled": False, + "sleep_threshold": None, + "min_position_iters": None, + "min_velocity_iters": None, + } + assert type(restored_root) is ArticulationRootPropertiesCfg + + +def test_articulation_root_config_rejects_backend_discriminator() -> None: + with pytest.raises(TypeError, match="backend"): + ArticulationRootPropertiesCfg.from_dict( + {"backend": "newton", "fixed_base": False} + ) + + +def test_articulation_root_config_round_trip() -> None: + root = ArticulationRootPropertiesCfg( + fixed_base=True, + sleep_threshold=0.005, + min_position_iters=8, + min_velocity_iters=2, + ) + + restored = ArticulationRootPropertiesCfg.from_dict(root.to_dict()) + + assert type(restored) is ArticulationRootPropertiesCfg + assert restored == root + assert "backend" not in root.to_dict() + + +def test_articulation_root_requires_both_solver_iteration_counts() -> None: + with pytest.raises(ValueError, match="must be configured together"): + ArticulationRootPropertiesCfg(min_position_iters=8) + + +def test_robot_cfg_round_trip_preserves_grouped_backend_types() -> None: + cfg = RobotCfg( + attrs=RigidBodyPhysicsCfg( + collision_props=NewtonCollisionPropertiesCfg(margin=0.01), + material_props=NewtonRigidBodyMaterialCfg(ke=1000.0), + ), + joint_drive_props=NewtonJointDrivePropertiesCfg(target_mode="position"), + root_props=ArticulationRootPropertiesCfg(fixed_base=False), + ) + + restored = RobotCfg.from_dict(cfg.to_dict()) + + assert isinstance(restored.attrs, RigidBodyPhysicsCfg) + assert isinstance(restored.attrs.collision_props, NewtonCollisionPropertiesCfg) + assert isinstance(restored.attrs.material_props, NewtonRigidBodyMaterialCfg) + assert isinstance(restored.joint_drive_props, NewtonJointDrivePropertiesCfg) + assert type(restored.root_props) is ArticulationRootPropertiesCfg + + +def test_rigid_physics_from_dict_rejects_unknown_fields() -> None: + with pytest.raises((KeyError, TypeError)): + RigidBodyPhysicsCfg.from_dict({"collision_props": {"margn": 0.01}}) + + +def test_robot_cfg_merge_preserves_grouped_overrides() -> None: + base = RobotCfg( + attrs=RigidBodyPhysicsCfg( + material_props=RigidBodyMaterialCfg(dynamic_friction=0.8) + ) + ) + + merged = merge_robot_cfg(base, {"attrs": {"mass_props": {"mass": 2.0}}}) + + assert isinstance(merged.attrs, RigidBodyPhysicsCfg) + assert merged.attrs.mass_props.mass == 2.0 + assert merged.attrs.material_props.dynamic_friction == 0.8 + + +def test_newton_physics_inherits_common_gravity_and_collision_config() -> None: + cfg = NewtonPhysicsCfg( + gravity=[0.0, 0.0, -1.5], + collision_cfg=NewtonCollisionPipelineCfg( + broad_phase="sap", + rigid_contact_max=1234, + update_interval=4, + ), + ) + + assert isinstance(cfg, PhysicsBackendCfg) + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) + assert dexsim_cfg.gravity == [0.0, 0.0, -1.5] + assert dexsim_cfg.collision_pipeline_cfg.broad_phase == "sap" + assert dexsim_cfg.collision_pipeline_cfg.rigid_contact_max == 1234 + assert dexsim_cfg.collision_pipeline_cfg.update_interval == 4 + + +def test_newton_physics_normalizes_mapping_collision_config() -> None: + cfg = NewtonPhysicsCfg( + collision_cfg={"broad_phase": "sap", "rigid_contact_max": 12} + ) + + assert isinstance(cfg.collision_cfg, NewtonCollisionPipelineCfg) + assert cfg.collision_cfg.broad_phase == "sap" + assert cfg.collision_cfg.rigid_contact_max == 12 + + +@pytest.mark.parametrize("update_interval", [0, -1, True, 1.5]) +def test_newton_collision_pipeline_rejects_invalid_update_interval( + update_interval: int | float | bool, +) -> None: + with pytest.raises(ValueError, match="update_interval must be a positive integer"): + NewtonCollisionPipelineCfg(update_interval=update_interval) + + +def test_newton_physics_can_disable_the_external_collision_pipeline() -> None: + cfg = NewtonPhysicsCfg(collision_cfg=None) + + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) + + assert dexsim_cfg.collision_pipeline_cfg is None + assert not hasattr(cfg, "enable_collision_pipeline") + assert not hasattr(cfg, "collision_pipeline_update_interval") + + +def test_newton_physics_rejects_broad_phase_without_a_collision_pipeline() -> None: + with pytest.raises(ValueError, match="broad_phase requires collision_cfg"): + NewtonPhysicsCfg(collision_cfg=None, broad_phase="sap") + + cfg = NewtonPhysicsCfg(collision_cfg=None) + cfg.broad_phase = "sap" + with pytest.raises(ValueError, match="broad_phase requires collision_cfg"): + cfg.to_dexsim_cfg(gpu_id=0) - assert robot_cfg.drive_pros.drive_type == "force" +def test_default_physics_accepts_the_same_gravity_input_shape() -> None: + cfg = DefaultPhysicsCfg(gravity=[0.0, 0.0, -1.5]) -def test_robot_cfg_partial_drive_properties_preserve_force_drive() -> None: - """Partial robot drive overrides retain the force-drive default.""" - robot_cfg = RobotCfg.from_dict({"drive_pros": {"stiffness": 0.0, "damping": 0.0}}) + assert cfg.to_dexsim_args()["gravity"] == [0.0, 0.0, -1.5] + assert DefaultPhysicsCfg().to_dexsim_args()["gravity"] == [0.0, 0.0, -9.81] - assert robot_cfg.drive_pros.drive_type == "force" + with pytest.raises(ValueError, match="three finite values"): + DefaultPhysicsCfg(gravity=[0.0, -9.81]).to_dexsim_args() -def test_physics_cfg_does_not_expose_fixed_solver_options() -> None: +def test_default_physics_cfg_does_not_expose_fixed_solver_options() -> None: """Fixed solver implementation details are not part of the public config.""" - physics_cfg = PhysicsCfg() + physics_cfg = DefaultPhysicsCfg() assert not hasattr(physics_cfg, "enable_enhanced_determinism") assert not hasattr(physics_cfg, "enable_friction_every_iteration") -def test_physics_cfg_applies_fixed_solver_defaults() -> None: - """Removed solver options retain their established DexSim defaults.""" - physics_args = PhysicsCfg(enable_ccd=True).to_dexsim_args() +def test_default_physics_cfg_applies_fixed_solver_defaults() -> None: + """Removed solver options retain the Default backend's established values.""" + physics_args = DefaultPhysicsCfg(enable_ccd=True).to_dexsim_args() assert physics_args["enable_ccd"] is True assert physics_args["enable_enhanced_determinism"] is False diff --git a/tests/sim/test_deformable_cfg.py b/tests/sim/test_deformable_cfg.py new file mode 100644 index 000000000..a834f3050 --- /dev/null +++ b/tests/sim/test_deformable_cfg.py @@ -0,0 +1,153 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Typed deformable configuration and descriptor contracts.""" + +from __future__ import annotations + +import pickle +from dataclasses import fields + +import pytest + +from embodichain.lab.sim import cfg as sim_cfg +from embodichain.lab.sim.cfg import ( + SurfaceDeformableObjectCfg, + VolumeDeformableObjectCfg, +) +from embodichain.lab.sim.spawn.descriptors import ( + surface_deformable_desc_from_cfg, + volume_deformable_desc_from_cfg, +) + +pytestmark = pytest.mark.no_sim + + +@pytest.mark.parametrize("topology", ["volume", "surface"]) +def test_new_schema_preserves_defaults_and_round_trips(topology): + cls = ( + VolumeDeformableObjectCfg + if topology == "volume" + else SurfaceDeformableObjectCfg + ) + cfg = cls.from_dict( + {"uid": "body", "shape": {"shape_type": "Mesh", "fpath": "mesh.obj"}} + ) + data = cfg.to_dict() + assert "attrs" in data and "physical_attr" not in data + assert "surface_props" in data["attrs"] + assert {f.name for f in fields(cfg.attrs.surface_props)} == { + "tri_ke", + "tri_ka", + "tri_kd", + "tri_drag", + "tri_lift", + "edge_ke", + "edge_kd", + } + expected = 0.0 if topology == "volume" else None + assert all(v == expected for v in data["attrs"]["surface_props"].values()) + restored = cls.from_dict(data) + assert restored.attrs.to_dict() == cfg.attrs.to_dict() + assert restored.shape.fpath == "mesh.obj" + copied = cfg.copy() + copied.attrs.surface_props.tri_ke = 12.0 + assert cfg.attrs.surface_props.tri_ke == expected + assert pickle.loads(pickle.dumps(cfg)).attrs.to_dict() == cfg.attrs.to_dict() + + +def test_volume_partial_surface_group_keeps_zero_descriptor_defaults(): + cfg = VolumeDeformableObjectCfg.from_dict( + { + "uid": "body", + "shape": {"shape_type": "Mesh", "fpath": "mesh.obj"}, + "attrs": {"surface_props": {"tri_ke": 12.0}}, + } + ) + desc, _ = volume_deformable_desc_from_cfg(cfg) + assert desc.physics.surface_tri_ke == 12.0 + assert desc.physics.surface_edge_ke == 0.0 + + +def test_unknown_nested_fields_are_rejected(): + with pytest.raises(TypeError, match="tri_kee"): + SurfaceDeformableObjectCfg.from_dict( + {"attrs": {"surface_props": {"tri_kee": 1.0}}} + ) + + +@pytest.mark.parametrize("topology", ["volume", "surface"]) +def test_surface_coefficients_compile_without_changing_values(topology: str) -> None: + cls = ( + VolumeDeformableObjectCfg + if topology == "volume" + else SurfaceDeformableObjectCfg + ) + compile_cfg = ( + volume_deformable_desc_from_cfg + if topology == "volume" + else surface_deformable_desc_from_cfg + ) + values = { + "tri_ke": 12.0, + "tri_ka": 13.0, + "tri_kd": 14.0, + "tri_drag": 15.0, + "tri_lift": 16.0, + "edge_ke": 17.0, + "edge_kd": 18.0, + } + cfg = cls.from_dict( + { + "uid": "body", + "shape": {"fpath": "mesh.obj"}, + "attrs": {"surface_props": values}, + } + ) + desc, _ = compile_cfg(cfg) + for field, value in values.items(): + native_field = f"surface_{field}" if topology == "volume" else field + assert getattr(desc.physics, native_field) == value + + +def test_volume_elasticity_uses_short_names() -> None: + cfg = VolumeDeformableObjectCfg.from_dict( + { + "uid": "body", + "shape": {"fpath": "mesh.obj"}, + "attrs": {"youngs": 1000.0, "poissons": 0.3}, + } + ) + desc, _ = volume_deformable_desc_from_cfg(cfg) + assert desc.physics.k_mu == pytest.approx(1000.0 / (2.0 * 1.3)) + assert desc.physics.k_lambda == pytest.approx(1000.0 * 0.3 / (1.3 * 0.4)) + assert "youngs" in cfg.attrs.to_dict() and "poissons" in cfg.attrs.to_dict() + + +@pytest.mark.parametrize( + "cfg_type, values", + [ + (VolumeDeformableObjectCfg, {"physical_attr": {}}), + (VolumeDeformableObjectCfg, {"voxel_attr": {}}), + (sim_cfg.VolumeDeformablePhysicsCfg, {"youngs_modulus": 1.0}), + (sim_cfg.VolumeDeformablePhysicsCfg, {"poissons_ratio": 0.3}), + (sim_cfg.VolumeDeformablePhysicsCfg, {"surface_tri_ke": 1.0}), + (sim_cfg.SurfaceDeformablePhysicsCfg, {"tri_ke": 1.0}), + ], +) +def test_only_current_config_keywords_are_accepted(cfg_type, values) -> None: + with pytest.raises(TypeError): + cfg_type(**values) diff --git a/tests/sim/test_grasp_cup_to_caffe_demo.py b/tests/sim/test_grasp_cup_to_caffe_demo.py new file mode 100644 index 000000000..190dd414b --- /dev/null +++ b/tests/sim/test_grasp_cup_to_caffe_demo.py @@ -0,0 +1,143 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import importlib.util +from pathlib import Path +from types import ModuleType, SimpleNamespace + +import pytest +import torch + +pytestmark = pytest.mark.no_sim + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[2] +_DEMO_PATH = _REPOSITORY_ROOT / "examples/sim/demo/grasp_cup_to_caffe.py" +_INITIAL_PHYSICS_STEPS = 1 +_IDLE_LOOP_PHYSICS_STEPS = 10 + + +def _load_demo_module() -> ModuleType: + spec = importlib.util.spec_from_file_location("grasp_cup_to_caffe_demo", _DEMO_PATH) + assert spec is not None and spec.loader is not None + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def test_scene_perturbations_precede_first_physics_step(monkeypatch) -> None: + demo = _load_demo_module() + events: list[str] = [] + + class FakeSimulation: + def prepare(self) -> None: + events.append("prepare") + + def update(self, step: int) -> None: + events.append(f"update:{step}") + if step == _IDLE_LOOP_PHYSICS_STEPS: + raise KeyboardInterrupt + + def open_window(self) -> None: + events.append("open_window") + + sim = FakeSimulation() + robot = object() + cup = object() + caffe = object() + monkeypatch.setattr( + demo, + "parse_arguments", + lambda: SimpleNamespace(headless=True, seed=0), + ) + monkeypatch.setattr(demo, "initialize_simulation", lambda _args: sim) + monkeypatch.setattr(demo, "create_robot", lambda _sim: robot) + monkeypatch.setattr(demo, "create_table", lambda _sim: object()) + monkeypatch.setattr(demo, "create_caffe", lambda _sim: caffe) + monkeypatch.setattr(demo, "create_cup", lambda _sim: cup) + monkeypatch.setattr( + demo, + "apply_random_xy_perturbation", + lambda item, **_kwargs: events.append( + "perturb:cup" if item is cup else "perturb:caffe" + ), + ) + monkeypatch.setattr( + demo, + "run_simulation", + lambda *_args: events.append("run_simulation"), + ) + monkeypatch.setattr( + demo.np.random, + "seed", + lambda seed: events.append(f"seed:{seed}"), + ) + + demo.main() + + assert events[:5] == [ + "prepare", + "seed:0", + "perturb:cup", + "perturb:caffe", + f"update:{_INITIAL_PHYSICS_STEPS}", + ] + + +def test_trajectory_uses_authored_hold_target_as_ik_seed(monkeypatch) -> None: + demo = _load_demo_module() + target_reads: list[bool] = [] + + class FakeRobot: + def get_joint_ids(self, name: str) -> list[int]: + assert name == "right_arm" + return [0, 1] + + def get_qpos(self, target: bool = False) -> torch.Tensor: + target_reads.append(target) + return torch.tensor([[0.25, -0.5]], dtype=torch.float32) + + def compute_fk(self, **_kwargs) -> torch.Tensor: + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + def compute_ik( + self, *, joint_seed: torch.Tensor, **_kwargs + ) -> tuple[torch.Tensor, torch.Tensor]: + return torch.ones(1, dtype=torch.bool), joint_seed.clone() + + class FakeItem: + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + monkeypatch.setattr( + demo, + "interpolate_with_distance", + lambda trajectory, **_kwargs: trajectory, + ) + + trajectory = demo.create_trajectory( + SimpleNamespace( + device=torch.device("cpu"), num_envs=1, is_newton_backend=False + ), + FakeRobot(), + FakeItem(), + FakeItem(), + ) + + assert target_reads == [True] + assert trajectory.shape == (1, 10, 8) diff --git a/tests/sim/test_open_drawer_tutorial.py b/tests/sim/test_open_drawer_tutorial.py new file mode 100644 index 000000000..4366c2668 --- /dev/null +++ b/tests/sim/test_open_drawer_tutorial.py @@ -0,0 +1,177 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import importlib.util +from pathlib import Path +from types import ModuleType, SimpleNamespace + +import pytest + +pytestmark = pytest.mark.no_sim + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[2] +_TUTORIAL_PATH = _REPOSITORY_ROOT / "scripts/tutorials/sim/open_drawer.py" + + +def _load_tutorial_module() -> ModuleType: + spec = importlib.util.spec_from_file_location( + "open_drawer_tutorial", _TUTORIAL_PATH + ) + assert spec is not None and spec.loader is not None + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +@pytest.mark.parametrize("is_newton_backend", [False, True]) +def test_create_scene_configures_newton_grasp_material_only_for_newton( + monkeypatch, + is_newton_backend, +) -> None: + tutorial = _load_tutorial_module() + robot = object() + drawer = object() + captured: dict[str, object] = {} + + class FakeSimulation: + def __init__(self): + self.is_newton_backend = is_newton_backend + + def add_robot(self, cfg): + captured["robot_cfg"] = cfg + return robot + + def add_articulation(self, cfg): + captured["drawer_cfg"] = cfg + return drawer + + monkeypatch.setattr( + tutorial.FrankaPandaCfg, + "from_dict", + lambda _config: SimpleNamespace( + joint_drive_props=SimpleNamespace(damping={}), + link_attrs=None, + ), + ) + monkeypatch.setattr(tutorial, "get_data_path", lambda asset: asset) + + tutorial.create_scene(FakeSimulation()) + + drawer_cfg = captured["drawer_cfg"] + robot_cfg = captured["robot_cfg"] + assert robot_cfg.joint_drive_props.damping == {} + assert drawer_cfg.root_props.fixed_base is True + if is_newton_backend: + robot_material = robot_cfg.link_attrs[ + "newton_gripper_contacts" + ].attrs.material_props + drawer_override = drawer_cfg.link_attrs["newton_handle_contacts"] + drawer_material = drawer_override.attrs.material_props + assert robot_material.ke == pytest.approx( + tutorial.NEWTON_GRASP_CONTACT_STIFFNESS + ) + assert robot_material.kd == pytest.approx(tutorial.NEWTON_GRASP_CONTACT_DAMPING) + assert drawer_override.link_names_expr == [tutorial.DRAWER_CONTACT_LINK_NAME] + assert drawer_material.ke == pytest.approx( + tutorial.NEWTON_GRASP_CONTACT_STIFFNESS + ) + assert drawer_material.kd == pytest.approx( + tutorial.NEWTON_GRASP_CONTACT_DAMPING + ) + else: + assert robot_cfg.link_attrs is None + assert drawer_cfg.link_attrs is None + + +def test_tutorial_newton_physics_cfg_enables_multiccd_with_auto_sized_buffers() -> None: + tutorial = _load_tutorial_module() + + cfg = tutorial._tutorial_physics_cfg("newton") + + assert cfg.num_substeps == 20 + assert cfg.solver_cfg == { + "solver_type": "mujoco_warp", + "cone": "elliptic", + "enable_multiccd": True, + } + + +def test_main_opens_native_window_after_spawn_prepare(monkeypatch) -> None: + tutorial = _load_tutorial_module() + events: list[str] = [] + captured_cfg: dict[str, object] = {} + args = SimpleNamespace( + num_envs=1, + hold_steps=0, + record_fps=30, + record_save_path=None, + headless=False, + viser=False, + auto_start=True, + physics="default", + device="cpu", + arena_space=2.0, + renderer="hybrid", + ) + + class FakeSimulation: + num_envs = 1 + + def prepare(self) -> None: + events.append("prepare") + + def open_window(self) -> None: + events.append("open_window") + + def update(self, *, step: int) -> None: + pass + + def is_window_recording(self) -> bool: + return False + + def wait_window_record_saves(self) -> None: + pass + + def destroy(self) -> None: + pass + + monkeypatch.setattr( + tutorial.argparse.ArgumentParser, + "parse_args", + lambda _parser: args, + ) + monkeypatch.setattr( + tutorial, + "SimulationManagerCfg", + lambda **kwargs: captured_cfg.update(kwargs) or kwargs, + ) + monkeypatch.setattr(tutorial, "SimulationManager", lambda _cfg: FakeSimulation()) + monkeypatch.setattr( + tutorial, + "create_scene", + lambda _sim: events.append("create_scene") + or (SimpleNamespace(uid="robot"), object()), + ) + monkeypatch.setattr(tutorial, "MotionGenerator", lambda *, cfg: object()) + monkeypatch.setattr(tutorial, "open_drawer", lambda *_args, **_kwargs: None) + monkeypatch.setattr(tutorial, "visualization_cfg_from_args", lambda _args: None) + + tutorial.main() + + assert captured_cfg["headless"] is True + assert events == ["create_scene", "prepare", "open_window"] diff --git a/tests/sim/test_rigid_constraint_integration.py b/tests/sim/test_rigid_constraint_integration.py index 5e6aaf5df..baac80f33 100644 --- a/tests/sim/test_rigid_constraint_integration.py +++ b/tests/sim/test_rigid_constraint_integration.py @@ -38,7 +38,7 @@ from embodichain.lab.sim.cfg import ( RigidObjectCfg, RigidConstraintCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, ) from embodichain.lab.sim.shapes import MeshCfg @@ -64,21 +64,19 @@ def _delta_z(self) -> float: pose_b = self.duck_b.get_local_pose(to_matrix=True) return float(pose_b[0, 2, 3] - pose_a[0, 2, 3]) - def setup_simulation(self, sim_device: str) -> None: - if not _can_run_sim(sim_device): - pytest.skip( - f"Cannot run rigid-constraint integration test on {sim_device}." - ) - config = SimulationManagerCfg(headless=True, sim_device=sim_device, num_envs=1) + def setup_simulation(self, device: str) -> None: + if not _can_run_sim(device): + pytest.skip(f"Cannot run rigid-constraint integration test on {device}.") + config = SimulationManagerCfg(headless=True, device=device, num_envs=1) self.sim = SimulationManager(config) self.sim.enable_physics(False) duck_path = get_data_path(DUCK_PATH) # Two dynamic ducks at different heights; with default (None) local # frames the constraint welds them at their current relative pose. - attrs_a = RigidBodyAttributesCfg() + attrs_a = RigidBodyPhysicsCfg() attrs_a.mass = 0.2 - attrs_b = RigidBodyAttributesCfg() + attrs_b = RigidBodyPhysicsCfg() attrs_b.mass = 0.1 self.duck_a = self.sim.add_rigid_object( cfg=RigidObjectCfg( @@ -99,8 +97,7 @@ def setup_simulation(self, sim_device: str) -> None: ), ) - if sim_device == "cuda" and getattr(self.sim, "is_use_gpu_physics", False): - self.sim.init_gpu_physics() + self.sim.prepare() self.sim.enable_physics(True) def teardown_method(self): diff --git a/tests/sim/test_rigid_physics_cfg.py b/tests/sim/test_rigid_physics_cfg.py new file mode 100644 index 000000000..829a55cca --- /dev/null +++ b/tests/sim/test_rigid_physics_cfg.py @@ -0,0 +1,77 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Tests for the grouped rigid-body physics configuration boundary.""" + +from __future__ import annotations + +import numpy as np +import pytest + +import embodichain.lab.sim as sim +import embodichain.lab.sim.cfg as sim_cfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + RigidBodyPhysicsCfg, + RigidObjectCfg, +) + + +def test_public_cfg_facade_no_longer_exports_flat_rigid_attribute_types() -> None: + for facade in (sim, sim_cfg): + assert not hasattr(facade, "RigidBodyAttributesCfg") + assert not hasattr(facade, "RigidBodyAttributesOverrideCfg") + + +def test_grouped_cfg_converts_com_quaternion_only_at_dexsim_boundary() -> None: + input_quaternion_xyzw = [1.0, 2.0, 3.0, 4.0] + cfg = RigidBodyPhysicsCfg.from_dict( + { + "mass_props": { + "mass": 2.0, + "inertia": [1.0, 2.0, 3.0], + "com_position": [0.1, 0.2, 0.3], + "com_quaternion": input_quaternion_xyzw, + }, + "material_props": {"dynamic_friction": 0.4}, + } + ) + + native = cfg.to_dexsim_physical_attr() + restored = RigidBodyPhysicsCfg.from_dexsim_physical_attr(native) + + np.testing.assert_allclose(native.com_quaternion, [4.0, 1.0, 2.0, 3.0]) + np.testing.assert_allclose( + restored.mass_props.com_quaternion, + input_quaternion_xyzw, + ) + assert restored.mass_props.mass == pytest.approx(2.0) + assert restored.material_props.dynamic_friction == pytest.approx(0.4) + + +@pytest.mark.parametrize("config_type", [RigidObjectCfg, ArticulationCfg]) +def test_asset_config_rejects_removed_flat_rigid_attributes(config_type: type) -> None: + with pytest.raises(ValueError, match="Removed flat rigid-body attrs fields"): + config_type.from_dict({"attrs": {"mass": 2.0}}) + + +def test_grouped_attrs_parse_for_rigid_and_articulation_configs() -> None: + rigid = RigidObjectCfg.from_dict({"attrs": {"mass_props": {"mass": 2.0}}}) + articulation = ArticulationCfg.from_dict( + {"attrs": {"material_props": {"static_friction": 0.8}}} + ) + + assert rigid.attrs.mass_props.mass == pytest.approx(2.0) + assert articulation.attrs.material_props.static_friction == pytest.approx(0.8) diff --git a/tests/sim/test_runtime_controls.py b/tests/sim/test_runtime_controls.py new file mode 100644 index 000000000..531fba080 --- /dev/null +++ b/tests/sim/test_runtime_controls.py @@ -0,0 +1,137 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for manager-owned Newton runtime-control adapters.""" + +from __future__ import annotations + +from types import SimpleNamespace +from unittest.mock import MagicMock + +import numpy as np +import pytest + +from embodichain.lab.sim._runtime_controls import ( + _KinematicNodalTrajectoryControl, +) + +pytestmark = pytest.mark.no_sim + + +class _ArrayView: + """Host-array stand-in for a borrowed Warp particle view.""" + + def __init__(self, values: np.ndarray) -> None: + self.values = values + + def numpy(self) -> np.ndarray: + """Return a host snapshot matching Warp's ``numpy`` method.""" + return self.values.copy() + + +class _ParticleSet: + """Minimal particle-set facade used by the runtime-control tests.""" + + def __init__(self) -> None: + self.positions = np.asarray( + [[0.0, 0.0, 0.0], [2.0, 0.0, 0.0]], + dtype=np.float32, + ) + self.fixed_indices: np.ndarray | None = None + + @property + def particle_count(self) -> int: + """Return the number of test particles.""" + return len(self.positions) + + def get_particle_positions(self) -> _ArrayView: + """Return the current particle positions.""" + return _ArrayView(self.positions) + + def set_particle_positions(self, positions: np.ndarray) -> None: + """Store one complete position snapshot.""" + self.positions = np.asarray(positions, dtype=np.float32).copy() + + def fix_particles(self, particle_indices: np.ndarray) -> None: + """Record which particles were made kinematic.""" + self.fixed_indices = np.asarray(particle_indices, dtype=np.int32).copy() + + +def test_kinematic_nodal_control_interpolates_offsets_per_substep() -> None: + particle_set = _ParticleSet() + solver = SimpleNamespace(rebuild_bvh=MagicMock()) + current_state = object() + context = SimpleNamespace( + result=SimpleNamespace(get_particle_set=lambda _target: particle_set), + solver=solver, + current_state=current_state, + ) + offsets = np.asarray( + [ + [[0.0, 0.0, 0.0]], + [[0.0, 2.0, 0.0]], + ], + dtype=np.float32, + ) + control = _KinematicNodalTrajectoryControl( + "arena_0/cloth", + np.asarray([1], dtype=np.int32), + offsets, + fps=10.0, + rebuild_self_contact_bvh=True, + ) + + control.initialize(context) + control(context, substep_index=0, substep_count=2, substep_dt=0.05) + np.testing.assert_allclose(particle_set.positions[1], [2.0, 0.0, 0.0]) + control(context, substep_index=1, substep_count=2, substep_dt=0.05) + + assert particle_set.fixed_indices is None + np.testing.assert_allclose(particle_set.positions[0], [0.0, 0.0, 0.0]) + np.testing.assert_allclose(particle_set.positions[1], [2.0, 1.0, 0.0]) + solver.rebuild_bvh.assert_called_once_with(current_state) + assert control.exclusive_resource_claims() == ( + ("kinematic_nodal_trajectory", "arena_0/cloth"), + ) + + +def test_kinematic_nodal_control_holds_last_unrated_sample() -> None: + particle_set = _ParticleSet() + context = SimpleNamespace( + result=SimpleNamespace(get_particle_set=lambda _target: particle_set), + solver=object(), + current_state=object(), + ) + offsets = np.asarray( + [ + [[0.0, 0.0, 0.0]], + [[1.0, 0.0, 0.0]], + ], + dtype=np.float32, + ) + control = _KinematicNodalTrajectoryControl( + "arena_0/cloth", + np.asarray([1], dtype=np.int32), + offsets, + fps=None, + rebuild_self_contact_bvh=False, + ) + control.initialize(context) + + for _ in range(3): + control(context, substep_index=0, substep_count=1, substep_dt=0.01) + + np.testing.assert_allclose(particle_set.positions[1], [3.0, 0.0, 0.0]) diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index fd4af648f..870657604 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -22,7 +22,7 @@ from pathlib import Path from types import SimpleNamespace -from unittest.mock import MagicMock +from unittest.mock import MagicMock, call import dexsim import numpy as np @@ -30,9 +30,17 @@ import torch import embodichain.lab.sim.sim_manager as sim_manager_module -from embodichain.lab.sim.cfg import MarkerCfg +from embodichain.lab.sim.cfg import ( + DefaultPhysicsCfg, + DLSSCfg, + MarkerCfg, + RenderCfg, + RobotCfg, + RobotPresetCfg, + SurfaceDeformableObjectCfg, +) from embodichain.lab.sim.profiler import Profiler -from embodichain.lab.sim.cfg import DLSSCfg, RenderCfg +from embodichain.lab.sim.physics import DefaultPhysicsBackend, NewtonPhysicsBackend from embodichain.lab.sim.sim_manager import ( SimulationManager, SimulationManagerCfg, @@ -46,6 +54,7 @@ SceneOverlays, VisualizationCfg, ) +from embodichain.utils import configclass DEFAULT_LOOK_AT = ( (2.6, -2.2, 1.6), @@ -90,7 +99,10 @@ def test_convert_sim_config_applies_dlss_for_all_renderers_and_camera_modes( ), ), ) - manager = SimpleNamespace(_material_cache_dir=tmp_path) + manager = SimpleNamespace( + _material_cache_dir=tmp_path, + physics=SimpleNamespace(configure_world=lambda *_: None), + ) world = SimulationManager._convert_sim_config(manager, config) @@ -311,7 +323,8 @@ def _make_sim_manager( sim._window_camera_pose_input_control = None sim._env = FakeEnv() sim._world = FakeWorld() - sim._default_plane = object() + sim._native_default_plane = object() + sim._default_plane = SimpleNamespace(native=lambda: sim._native_default_plane) sim._visualization_runtime = None sim.is_window_opened = window is not None return sim @@ -325,12 +338,14 @@ def _make_visualization_sim_manager() -> ( runtime = FakeVisualizationRuntime() sim.sim_config = SimpleNamespace( physics_dt=0.01, + physics_cfg=DefaultPhysicsCfg(), visualization=SimpleNamespace(backend="viser"), ) sim.device = SimpleNamespace(type="cpu") sim.profiler = Profiler(None, torch.device("cpu")) sim._is_initialized_gpu_physics = False sim._world = FakeWorld() + sim.prepare = MagicMock() sim._window_record_state = None sim._visualization_runtime = runtime sim._visualization_overlays = None @@ -342,6 +357,30 @@ def _make_visualization_sim_manager() -> ( return sim, runtime +def _make_runtime_control_sim_manager( + *, + num_envs: int = 2, + backend: str = "newton", +) -> tuple[SimulationManager, MagicMock]: + """Create a manager stub at the pre-prepare runtime-control boundary.""" + sim = object.__new__(SimulationManager) + sim.sim_config = SimpleNamespace(num_envs=num_envs) + sim.physics = SimpleNamespace(name=backend) + sim._robots = {"robot": object()} + sim._articulations = {} + sim._rigid_objects = {"table": object()} + sim._deformable_objects = { + "cloth": SimpleNamespace( + cfg=SurfaceDeformableObjectCfg(uid="cloth", particle_flags=[0, 1, 0]) + ) + } + spawn_scene = MagicMock() + spawn_scene.arena_names = tuple(f"arena_{index}" for index in range(num_envs)) + spawn_scene.builder.is_finalized = False + sim._spawn_scene = spawn_scene + return sim, spawn_scene.builder + + def test_flush_cleanup_queue_returns_immediately_when_no_destroy_is_pending( monkeypatch: pytest.MonkeyPatch, ) -> None: @@ -381,6 +420,55 @@ def test_flush_cleanup_queue_waits_after_running_pending_destroy( wait_scene_destruction.assert_called_once_with() +def test_deferred_destroy_prepares_backend_before_releasing_world( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Backend-owned views are released before Spawn and World resources.""" + events: list[str] = [] + sim = object.__new__(SimulationManager) + spawn_scene = MagicMock() + spawn_scene.close.side_effect = lambda: events.append("spawn_close") + sim.physics = SimpleNamespace( + prepare_for_teardown=lambda: events.append("backend_prepare") + ) + sim._gizmos = {} + sim._markers = {} + sim._rigid_objects = {} + sim._constraints = {} + sim._rigid_object_groups = {} + sim._deformable_objects = {} + sim._articulations = {} + sim._robots = {} + sim._sensors = {} + sim._lights = {} + sim._visual_materials = {} + sim._texture_cache = {} + sim._arenas = [] + sim._spawn_scene = spawn_scene + sim._default_plane = object() + sim._sensors = {} + sim._env = SimpleNamespace(clean=lambda: events.append("env_clean")) + sim._world = SimpleNamespace(quit=lambda: events.append("world_quit")) + sim.instance_id = 0 + sim.is_window_recording = lambda: False + sim.wait_window_record_saves = lambda: events.append("record_wait") + sim.clean_materials = lambda: events.append("material_clean") + sim.is_window_opened = False + + monkeypatch.setattr( + SimulationManager, + "reset", + lambda _instance_id: events.append("manager_reset"), + ) + monkeypatch.setattr(gc, "collect", lambda: events.append("gc_collect")) + + sim._deferred_destroy() + + assert events.index("backend_prepare") < events.index("gc_collect") + assert events.index("backend_prepare") < events.index("spawn_close") + assert events.index("backend_prepare") < events.index("world_quit") + + def test_sim_update_refreshes_dirty_visualization_and_captures_current_state() -> None: sim, runtime = _make_visualization_sim_manager() @@ -848,7 +936,7 @@ def test_entity_gizmo_delegates_to_dexsim_and_excludes_default_plane() -> None: ( SimulationManager._DEFAULT_PLANE_GIZMO_TARGET_ID, dexsim.interaction.EntityGizmoTargetType.RIGID_BODY, - sim._default_plane, + sim._native_default_plane, dexsim.types.ActorType.STATIC, ) ] @@ -862,7 +950,10 @@ def test_open_window_enables_entity_gizmo_by_default() -> None: assert sim.is_window_opened is True assert sim._world.window_open_count == 1 assert sim._world.entity_gizmo_configs == [None] - assert sim._world.get_entity_gizmo().external_targets[0][2] is sim._default_plane + assert ( + sim._world.get_entity_gizmo().external_targets[0][2] + is sim._native_default_plane + ) def test_entity_gizmo_can_be_disabled_in_startup_configuration() -> None: @@ -970,6 +1061,10 @@ def test_constructor_starts_visualization_after_default_scene( expected_gizmo: bool, ) -> None: lifecycle: list[str] = [] + spawn_scene = MagicMock() + spawn_scene.builder.prepare_arenas.side_effect = ( + lambda: lifecycle.append("arenas") or [] + ) world = MagicMock() world.set_manual_update.side_effect = lambda _enable: lifecycle.append( "explicit_physics" @@ -982,6 +1077,11 @@ def test_constructor_starts_visualization_after_default_scene( ) monkeypatch.setattr(sim_manager_module.wp, "init", lambda: None) monkeypatch.setattr(sim_manager_module.dexsim, "World", lambda _cfg: world) + monkeypatch.setattr( + sim_manager_module, + "SpawnScene", + lambda *_args, **_kwargs: spawn_scene, + ) monkeypatch.setattr( sim_manager_module.dexsim, "set_physics_config", lambda **_kwargs: None ) @@ -1005,7 +1105,7 @@ def test_constructor_starts_visualization_after_default_scene( ) monkeypatch.setattr( SimulationManager, - "_create_default_plane", + "_declare_spawn_default_plane", lambda _self: lifecycle.append("plane"), ) monkeypatch.setattr( @@ -1019,14 +1119,9 @@ def test_constructor_starts_visualization_after_default_scene( lambda _self: lifecycle.append("lighting"), ) - def build_arenas(sim: SimulationManager, num: int) -> None: - lifecycle.append("arenas") - sim._arenas.extend([object() for _ in range(num)]) - def start_visualization(sim: SimulationManager) -> None: lifecycle.append(f"visualization:{sim.num_envs}") - monkeypatch.setattr(SimulationManager, "_build_multiple_arenas", build_arenas) monkeypatch.setattr( SimulationManager, "start_visualization", @@ -1042,6 +1137,7 @@ def start_visualization(sim: SimulationManager) -> None: SimulationManager.__init__( sim, SimulationManagerCfg( + startup_summary="off", num_envs=3, headless=headless, enable_entity_gizmo=entity_gizmo, @@ -1053,23 +1149,606 @@ def start_visualization(sim: SimulationManager) -> None: assert lifecycle == [ "explicit_physics", "enable_physics", + "arenas", "resources", - "plane", "background", + "plane", "lighting", - "arenas", "visualization:3", ] + (["entity_gizmo"] if expected_gizmo else []) + assert sim._spawn_scene is spawn_scene + assert sim._arenas == [] + + +def test_register_kinematic_joint_trajectory_expands_each_arena() -> None: + sim, builder = _make_runtime_control_sim_manager() + frame_count = 3 + dof_count = 2 + positions = torch.arange( + sim.num_envs * frame_count * dof_count, + dtype=torch.float32, + ).reshape(sim.num_envs, frame_count, dof_count) + root_poses = np.tile( + np.eye(4, dtype=np.float32), + (sim.num_envs, frame_count, 1, 1), + ) + + sim.register_kinematic_joint_trajectory( + "robot", + positions, + fps=50.0, + root_poses=root_poses, + ) + + assert builder.add_runtime_control.call_count == sim.num_envs + for env_index, control_call in enumerate( + builder.add_runtime_control.call_args_list + ): + control = control_call.args[0] + assert control.target == f"arena_{env_index}/robot" + assert control.fps == 50.0 + np.testing.assert_array_equal( + control.joint_positions, + positions[env_index].numpy(), + ) + np.testing.assert_array_equal( + control.root_poses, + root_poses[env_index], + ) + + +def test_register_kinematic_joint_trajectory_rejects_non_newton_backend() -> None: + sim, builder = _make_runtime_control_sim_manager(backend="default") + positions = np.zeros((sim.num_envs, 2, 1), dtype=np.float32) + + with pytest.raises(RuntimeError, match="require the Newton backend"): + sim.register_kinematic_joint_trajectory("robot", positions) + + builder.add_runtime_control.assert_not_called() + + +def test_register_kinematic_joint_trajectory_rejects_unknown_asset() -> None: + sim, builder = _make_runtime_control_sim_manager() + positions = np.zeros((sim.num_envs, 2, 1), dtype=np.float32) + + with pytest.raises(KeyError, match="missing"): + sim.register_kinematic_joint_trajectory("missing", positions) + + builder.add_runtime_control.assert_not_called() + + +def test_register_kinematic_joint_trajectory_rejects_wrong_arena_batch() -> None: + sim, builder = _make_runtime_control_sim_manager(num_envs=2) + positions = np.zeros((1, 2, 1), dtype=np.float32) + + with pytest.raises(ValueError, match=r"\(2, frames, dof\)"): + sim.register_kinematic_joint_trajectory("robot", positions) + + builder.add_runtime_control.assert_not_called() + + +def test_register_kinematic_joint_trajectory_rejects_finalized_scene() -> None: + sim, builder = _make_runtime_control_sim_manager() + builder.is_finalized = True + positions = np.zeros((sim.num_envs, 2, 1), dtype=np.float32) + + with pytest.raises(RuntimeError, match=r"before SimulationManager\.prepare"): + sim.register_kinematic_joint_trajectory("robot", positions) + + builder.add_runtime_control.assert_not_called() + + +def test_register_contact_material_schedule_expands_each_arena() -> None: + sim, builder = _make_runtime_control_sim_manager() + friction_track = ((0.0, 0.5), (1.0, 0.1)) + + sim.register_contact_material_schedule( + "robot", + {"dynamic_friction": friction_track}, + link_names=("left_finger", "right_finger"), + ) + + assert builder.add_runtime_control.call_count == sim.num_envs + for env_index, control_call in enumerate( + builder.add_runtime_control.call_args_list + ): + control = control_call.args[0] + assert control.target == f"arena_{env_index}/robot" + assert control.link_names == ("left_finger", "right_finger") + assert control.property_names == ("dynamic_friction",) + np.testing.assert_allclose(control.keyframe_times, (0.0, 1.0)) + np.testing.assert_allclose(control.keyframe_values, (0.5, 0.1)) + + +def test_register_contact_material_schedule_rejects_unknown_asset() -> None: + sim, builder = _make_runtime_control_sim_manager() + + with pytest.raises(KeyError, match="missing"): + sim.register_contact_material_schedule( + "missing", + {"dynamic_friction": ((0.0, 0.5),)}, + ) + + builder.add_runtime_control.assert_not_called() + + +def test_register_particle_contact_material_schedule_adds_global_control() -> None: + sim, builder = _make_runtime_control_sim_manager() + friction_track = ((0.0, 0.5), (1.0, 1.2)) + + sim.register_particle_contact_material_schedule( + {"dynamic_friction": friction_track} + ) + + builder.add_runtime_control.assert_called_once() + control = builder.add_runtime_control.call_args.args[0] + np.testing.assert_allclose( + control.tracks["dynamic_friction"], + friction_track, + ) + + +def test_contact_material_schedules_reject_finalized_scene() -> None: + sim, builder = _make_runtime_control_sim_manager() + builder.is_finalized = True + keyframes = {"dynamic_friction": ((0.0, 0.5),)} + + with pytest.raises(RuntimeError, match=r"before SimulationManager\.prepare"): + sim.register_contact_material_schedule("table", keyframes) + with pytest.raises(RuntimeError, match=r"before SimulationManager\.prepare"): + sim.register_particle_contact_material_schedule(keyframes) + + builder.add_runtime_control.assert_not_called() + + +def test_register_kinematic_nodal_trajectory_expands_each_arena() -> None: + sim, builder = _make_runtime_control_sim_manager() + node_indices = np.asarray([0, 2], dtype=np.int32) + sample_count = 3 + offsets = np.arange( + sim.num_envs * sample_count * len(node_indices) * 3, + dtype=np.float32, + ).reshape(sim.num_envs, sample_count, len(node_indices), 3) + + sim.register_kinematic_nodal_trajectory( + "cloth", + node_indices, + offsets, + fps=60.0, + rebuild_self_contact_bvh=True, + ) + + assert builder.add_runtime_control.call_count == sim.num_envs + for env_index, control_call in enumerate( + builder.add_runtime_control.call_args_list + ): + control = control_call.args[0] + assert control.target == f"arena_{env_index}/cloth" + assert control.fps == pytest.approx(60.0) + assert control.rebuild_self_contact_bvh is True + np.testing.assert_array_equal(control.node_indices, node_indices) + np.testing.assert_array_equal(control.position_offsets, offsets[env_index]) + + +def test_register_kinematic_nodal_trajectory_rejects_active_nodes() -> None: + sim, builder = _make_runtime_control_sim_manager() + offsets = np.zeros((sim.num_envs, 2, 1, 3), dtype=np.float32) + + with pytest.raises(ValueError, match="ACTIVE particle flag"): + sim.register_kinematic_nodal_trajectory("cloth", [1], offsets) + + builder.add_runtime_control.assert_not_called() + + +def test_register_kinematic_nodal_trajectory_rejects_wrong_shape() -> None: + sim, builder = _make_runtime_control_sim_manager() + offsets = np.zeros((sim.num_envs, 2, 3), dtype=np.float32) + + with pytest.raises(ValueError, match="position_offsets"): + sim.register_kinematic_nodal_trajectory("cloth", [0], offsets) + + builder.add_runtime_control.assert_not_called() + + +def test_register_kinematic_nodal_trajectory_rejects_finalized_scene() -> None: + sim, builder = _make_runtime_control_sim_manager() + builder.is_finalized = True + offsets = np.zeros((sim.num_envs, 2, 1, 3), dtype=np.float32) + + with pytest.raises(RuntimeError, match=r"before SimulationManager\.prepare"): + sim.register_kinematic_nodal_trajectory("cloth", [0], offsets) + + builder.add_runtime_control.assert_not_called() + + +def test_add_robot_resolves_backend_preset_before_declaration() -> None: + @configclass + class TestRobotPresetCfg(RobotPresetCfg): + default: RobotCfg = RobotCfg(uid="selected", fpath="selected.urdf") + + sim = object.__new__(SimulationManager) + sim.physics = SimpleNamespace(name="default", supports_robot=True, solver_type=None) + sim.sim_config = SimpleNamespace(physics_cfg=DefaultPhysicsCfg()) + sim._robots = {} + sim._declare_spawn_articulation = MagicMock(return_value="robot-handle") + + robot = sim.add_robot(TestRobotPresetCfg()) + + assert robot == "robot-handle" + resolved_cfg = sim._declare_spawn_articulation.call_args.args[0] + assert isinstance(resolved_cfg, RobotCfg) + assert resolved_cfg.uid == "selected" + + +def test_default_plane_authors_repeated_uv_before_spawn() -> None: + sim = object.__new__(SimulationManager) + sim._spawn_scene = MagicMock() + sim._spawn_scene.handles.return_value = [] + sim._spawn_default_plane_material = object() + + sim._declare_spawn_default_plane() + + descriptor = sim._spawn_scene.declare.call_args.args[2] + expected_repeat = 500.0 # One two-metre texture tile across a 1000 m plane. + np.testing.assert_array_equal( + descriptor.renders[0].uv_coords, + np.asarray( + [ + [0.0, 0.0], + [expected_repeat, 0.0], + [expected_repeat, expected_repeat], + [0.0, expected_repeat], + ], + dtype=np.float32, + ), + ) + + +@pytest.mark.parametrize( + ("backend", "device", "initializes_direct_gpu"), + [ + pytest.param("default", torch.device("cpu"), False, id="default-host"), + pytest.param("default", torch.device("cuda"), True, id="default-accelerator"), + pytest.param("newton", torch.device("cpu"), False, id="newton-host"), + pytest.param("newton", torch.device("cuda"), False, id="newton-accelerator"), + ], +) +def test_prepare_initializes_runtime_for_backend_device_matrix( + backend: str, + device: torch.device, + initializes_direct_gpu: bool, +) -> None: + result = MagicMock() + result.topology_revision = 3 + spawn_scene = MagicMock() + spawn_scene.builder.is_finalized = False + spawn_scene.builder.result = None + spawn_scene.commit.return_value = result + spawn_scene.arena_names = ["arena_0"] + events: list[str] = [] + spawn_scene.prepare_runtime_config.side_effect = lambda _result: events.append( + "runtime_config" + ) + spawn_scene.bind.side_effect = lambda: events.append("bind") + + sim = object.__new__(SimulationManager) + sync_render_state = MagicMock() + sim.device = device + sim._world = MagicMock() + backend_cls = ( + DefaultPhysicsBackend if backend == "default" else NewtonPhysicsBackend + ) + sim.physics = backend_cls(sim) + sim.physics.sync_render_state = sync_render_state + sim._world.init_gpu_physics.side_effect = lambda: events.append("gpu_init") + sim._spawn_scene = spawn_scene + sim._default_plane = object() + sim._sensors = {} + sim._prepared_spawn_topology_revision = -1 + sim._synced_spawn_render_topology_revision = -1 + sim._camera_attachment_topology_revision = -1 + + sim.prepare() + + spawn_scene.prepare_runtime_config.assert_called_once_with(result) + spawn_scene.bind.assert_called_once_with() + sync_render_state.assert_called_once_with(result) + sim._world.update.assert_not_called() + if initializes_direct_gpu: + sim._world.init_gpu_physics.assert_called_once_with() + assert events == ["runtime_config", "gpu_init", "bind"] + else: + sim._world.init_gpu_physics.assert_not_called() + assert events == ["runtime_config", "bind"] + + +def test_manager_delegates_differentiable_runtime_without_backend_name() -> None: + """Runtime availability is capability-based rather than name-based.""" + runtime = object() + sim = object.__new__(SimulationManager) + sim.physics = SimpleNamespace( + name="third_party", + differentiable_runtime=runtime, + ) + + assert sim.differentiable_runtime is runtime + + +def test_prepare_retries_runtime_and_binding_without_recommit() -> None: + result = MagicMock() + result.needs_rebuild = False + result.topology_revision = 3 + spawn_scene = MagicMock() + spawn_scene.builder.is_finalized = True + spawn_scene.builder.result = result + spawn_scene.builder.has_pending_changes = False + + sim = object.__new__(SimulationManager) + sync_render_state = MagicMock() + prepare_spawn_runtime = MagicMock(side_effect=[RuntimeError("first attempt"), None]) + sim.physics = SimpleNamespace( + name="default", + prepare_spawn_runtime=prepare_spawn_runtime, + sync_render_state=sync_render_state, + ) + sim.device = torch.device("cuda") + sim._world = MagicMock() + sim._spawn_scene = spawn_scene + sim._sensors = {} + sim._prepared_spawn_topology_revision = -1 + sim._synced_spawn_render_topology_revision = -1 + sim._camera_attachment_topology_revision = -1 + + with pytest.raises(RuntimeError, match="first attempt"): + sim.prepare() + sim.prepare() + + spawn_scene.commit.assert_not_called() + assert prepare_spawn_runtime.call_count == 2 + spawn_scene.bind.assert_called_once_with() + sync_render_state.assert_called_once_with(result) + + +def test_prepare_retries_camera_attachment_without_recommit() -> None: + result = MagicMock() + result.needs_rebuild = False + result.topology_revision = 3 + attach_parented_cameras = MagicMock( + side_effect=[RuntimeError("attach failed"), None] + ) + spawn_scene = MagicMock() + spawn_scene.builder.is_finalized = True + spawn_scene.builder.result = result + spawn_scene.builder.has_pending_changes = False + + sim = object.__new__(SimulationManager) + sync_render_state = MagicMock() + sim.physics = SimpleNamespace( + name="default", + prepare_spawn_runtime=MagicMock(), + sync_render_state=sync_render_state, + ) + sim.device = torch.device("cpu") + sim._world = MagicMock() + sim._spawn_scene = spawn_scene + sim._sensors = {} + sim._attach_parented_cameras = attach_parented_cameras + sim._prepared_spawn_topology_revision = -1 + sim._synced_spawn_render_topology_revision = -1 + sim._camera_attachment_topology_revision = -1 + + with pytest.raises(RuntimeError, match="attach failed"): + sim.prepare() + sim.prepare() + sim.prepare() + + assert attach_parented_cameras.call_count == 2 + assert sim._camera_attachment_topology_revision == 3 + spawn_scene.commit.assert_not_called() + sync_render_state.assert_called_once_with(result) + + +def test_prepare_syncs_render_state_once_per_topology_revision() -> None: + events: list[str] = [] + result = MagicMock() + result.needs_rebuild = False + result.topology_revision = 3 + spawn_scene = MagicMock() + spawn_scene.builder.is_finalized = True + spawn_scene.builder.result = result + spawn_scene.builder.has_pending_changes = False + spawn_scene.bind.side_effect = lambda: events.append("bind") + sync_render_state = MagicMock(side_effect=lambda _result: events.append("sync")) + + sim = object.__new__(SimulationManager) + sim.physics = SimpleNamespace( + name="newton", + prepare_spawn_runtime=MagicMock(), + sync_render_state=sync_render_state, + ) + sim.device = torch.device("cpu") + sim._world = MagicMock() + sim._spawn_scene = spawn_scene + attach_parented_cameras = MagicMock() + sim._attach_parented_cameras = attach_parented_cameras + sim._prepared_spawn_topology_revision = -1 + sim._synced_spawn_render_topology_revision = -1 + sim._camera_attachment_topology_revision = -1 + + sim.prepare() + sim.prepare() + result.topology_revision = 4 + sim.prepare() + sim.prepare() + + spawn_scene.commit.assert_not_called() + assert spawn_scene.bind.call_count == 4 + assert sync_render_state.call_count == 2 + sync_render_state.assert_has_calls([call(result), call(result)]) + assert attach_parented_cameras.call_count == 2 + assert sim._camera_attachment_topology_revision == 4 + sim._world.update.assert_not_called() + assert events == ["bind", "sync", "bind", "bind", "sync", "bind"] + + +def test_prepare_retries_render_state_sync_without_recommit() -> None: + result = MagicMock() + result.needs_rebuild = False + result.topology_revision = 3 + spawn_scene = MagicMock() + spawn_scene.builder.is_finalized = True + spawn_scene.builder.result = result + spawn_scene.builder.has_pending_changes = False + sync_render_state = MagicMock( + side_effect=[RuntimeError("sync failed"), None], + ) + + sim = object.__new__(SimulationManager) + sim.physics = SimpleNamespace( + name="newton", + prepare_spawn_runtime=MagicMock(), + sync_render_state=sync_render_state, + ) + sim.device = torch.device("cpu") + sim._world = MagicMock() + sim._spawn_scene = spawn_scene + sim._sensors = {} + sim._prepared_spawn_topology_revision = -1 + sim._synced_spawn_render_topology_revision = -1 + sim._camera_attachment_topology_revision = -1 + + with pytest.raises(RuntimeError, match="sync failed"): + sim.prepare() + sim.prepare() + sim.prepare() + + spawn_scene.commit.assert_not_called() + assert spawn_scene.bind.call_count == 3 + assert sync_render_state.call_count == 2 + sim._world.update.assert_not_called() + + +def test_add_camera_uses_owning_manager_render_context( + monkeypatch: pytest.MonkeyPatch, +) -> None: + world = object() + arenas = [object(), object()] + + sim = object.__new__(SimulationManager) + sim.sim_config = SimpleNamespace(num_envs=len(arenas)) + sim.device = torch.device("cpu") + sim._world = world + sim._arenas = arenas + sim._sensors = {} + sim._visualization_topology_revision = 0 + sim.SUPPORTED_SENSOR_TYPES = {"Camera": sim_manager_module.Camera} + + monkeypatch.setattr( + sim_manager_module.Camera, + "_build_sensor_from_config", + lambda self, config, device: None, + ) + monkeypatch.setattr(sim_manager_module.Camera, "reset", lambda self: None) + + sensor = sim.add_sensor(CameraCfg(uid="owned_camera")) + + assert sensor._world is world + assert sensor._arenas == arenas + assert sensor.num_instances == len(arenas) + + +def test_camera_attachment_uses_resolved_nodes_and_tracks_state( + monkeypatch: pytest.MonkeyPatch, +) -> None: + entities = [MagicMock(), MagicMock()] + parent_nodes = [object(), object()] + owner = MagicMock() + owner.num_envs = len(entities) + owner.get_world.return_value = object() + owner.get_env.side_effect = [object(), object()] + + def build_camera(sensor, config, device) -> None: + sensor._entities[:] = entities + + monkeypatch.setattr( + sim_manager_module.Camera, + "_build_sensor_from_config", + build_camera, + ) + monkeypatch.setattr(sim_manager_module.Camera, "reset", lambda self: None) + + sensor = sim_manager_module.Camera( + CameraCfg( + uid="attached_camera", + extrinsics=CameraCfg.ExtrinsicsCfg(parent="robot/tool"), + ), + owner=owner, + ) + + assert sensor.is_attached is False + sensor.attach_to_parent_nodes(parent_nodes) + + assert sensor.is_attached is True + for entity, parent_node in zip(entities, parent_nodes, strict=True): + entity.attach_node.assert_called_once_with(parent_node) + + +def test_manager_resolves_camera_parent_before_attachment() -> None: + parent_nodes = [object(), object()] + sensor = MagicMock() + sensor.cfg.extrinsics.parent = "robot/tool" + + sim = object.__new__(SimulationManager) + sim._resolve_spawn_sensor_parent_nodes = MagicMock(return_value=parent_nodes) + + sim._attach_camera_parent(sensor) + + sim._resolve_spawn_sensor_parent_nodes.assert_called_once_with("robot/tool") + sensor.attach_to_parent_nodes.assert_called_once_with(parent_nodes) + + +def test_parented_cameras_include_only_configured_camera_sensors() -> None: + parented_camera = object.__new__(sim_manager_module.Camera) + parented_camera.cfg = CameraCfg( + extrinsics=CameraCfg.ExtrinsicsCfg(parent="robot/tool") + ) + root_camera = object.__new__(sim_manager_module.Camera) + root_camera.cfg = CameraCfg() + sim = object.__new__(SimulationManager) + sim._sensors = { + "parented": parented_camera, + "root": root_camera, + "custom": MagicMock(), + } + sim._attach_camera_parent = MagicMock() + + sim._attach_parented_cameras() + + sim._attach_camera_parent.assert_called_once_with(parented_camera) def test_remove_asset_marks_visualization_topology_dirty() -> None: sim, runtime = _make_visualization_sim_manager() rigid_object = MagicMock() + spawn_scene = MagicMock() + spawn_scene.__contains__.return_value = True + spawn_scene.result = object() + sim._spawn_scene = spawn_scene + sim.prepare = MagicMock() sim._rigid_objects = {"cube": rigid_object} + sim._rigid_object_groups = {} + sim._deformable_objects = {} + sim._articulations = {} + sim._robots = {} + sim._lights = {} + sim._sensors = {} assert sim.remove_asset("cube") - rigid_object.destroy.assert_called_once_with() + spawn_scene.remove.assert_called_once_with("cube") + sim.prepare.assert_called_once_with() + rigid_object.destroy.assert_not_called() + assert "cube" not in sim._rigid_objects assert sim._visualization_topology_revision == 3 sim.stop_visualization() assert runtime.stopped @@ -1103,6 +1782,7 @@ def test_add_stereo_camera_marks_visualization_topology_dirty() -> None: sim.SUPPORTED_SENSOR_TYPES = { "StereoCamera": lambda cfg, device: sensor, } + sim.prepare = MagicMock() cfg = SimpleNamespace(sensor_type="StereoCamera", uid="cam_high") assert sim.add_sensor(cfg) is sensor @@ -1269,7 +1949,7 @@ def fake_save_window_record_worker( assert sim._window_record_save_threads == [] -def test_reset_objects_state_includes_soft_and_cloth_assets() -> None: +def test_reset_objects_state_includes_deformable_assets() -> None: sim = object.__new__(SimulationManager) sim._robots = {} sim._articulations = {} @@ -1277,10 +1957,12 @@ def test_reset_objects_state_includes_soft_and_cloth_assets() -> None: sim._rigid_object_groups = {} sim._lights = {} sim._sensors = {} - sim._soft_objects = {"soft": MagicMock()} - sim._cloth_objects = {"cloth": MagicMock()} + sim._deformable_objects = { + "soft": MagicMock(), + "cloth": MagicMock(), + } sim.reset_objects_state(env_ids=[1]) - sim._soft_objects["soft"].reset.assert_called_once_with([1]) - sim._cloth_objects["cloth"].reset.assert_called_once_with([1]) + sim._deformable_objects["soft"].reset.assert_called_once_with([1]) + sim._deformable_objects["cloth"].reset.assert_called_once_with([1]) diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py new file mode 100644 index 000000000..7e8c8aa37 --- /dev/null +++ b/tests/sim/test_sim_manager_cfg.py @@ -0,0 +1,475 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from contextlib import nullcontext +from types import SimpleNamespace +from unittest.mock import MagicMock + +import pytest +import torch + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + DefaultPhysicsCfg, + NewtonPhysicsCfg, + PhysicsBackendCfg, + WindowCameraPoseCfg, +) +from embodichain.lab.sim.physics import DefaultPhysicsBackend, NewtonPhysicsBackend +from embodichain.lab.sim.physics import newton as newton_physics +from embodichain.lab.sim import sim_manager + + +def test_simulation_manager_cfg_uses_default_physics_cfg() -> None: + cfg = SimulationManagerCfg() + + assert type(cfg.physics_cfg) is DefaultPhysicsCfg + + +@pytest.mark.no_sim +def test_newton_physics_cfg_owns_backend_device_default() -> None: + """The concrete Newton default shadows the generic backend default.""" + assert PhysicsBackendCfg().device == "cpu" + assert DefaultPhysicsCfg().device == "cpu" + assert NewtonPhysicsCfg().device == "cuda:0" + assert SimulationManagerCfg(physics_cfg=NewtonPhysicsCfg()).device == "cuda:0" + assert ( + SimulationManagerCfg(physics_cfg=NewtonPhysicsCfg(), sim_device=None).device + == "cuda:0" + ) + assert SimulationManagerCfg(physics_config=NewtonPhysicsCfg()).device == "cuda:0" + + +def test_physics_runtime_fields_are_stored_on_physics_cfg() -> None: + cfg = SimulationManagerCfg( + headless=True, + physics_dt=0.02, + device=torch.device("cpu"), + ) + + assert cfg.physics_dt == 0.02 + assert cfg.device == torch.device("cpu") + assert cfg.physics_cfg.physics_dt == 0.02 + assert cfg.physics_cfg.device == torch.device("cpu") + + serialized = cfg.to_dict() + assert "physics_dt" not in serialized + assert "device" not in serialized + assert serialized["physics_cfg"]["physics_dt"] == 0.02 + assert serialized["physics_cfg"]["device"] == torch.device("cpu") + + +def test_simulation_manager_cfg_keeps_legacy_physics_accessors() -> None: + cfg = SimulationManagerCfg(physics_cfg=NewtonPhysicsCfg()) + + cfg.physics_dt = 0.005 + cfg.device = "cuda:0" + + assert cfg.physics_cfg.physics_dt == 0.005 + assert cfg.physics_cfg.device == "cuda:0" + + +def test_simulation_manager_cfg_explicit_cpu_overrides_newton_default() -> None: + """An explicit runtime device has the same meaning for every backend.""" + cfg = SimulationManagerCfg( + physics_cfg=NewtonPhysicsCfg(device="cuda:0"), + device="cpu", + ) + + assert cfg.device == "cpu" + assert cfg.physics_cfg.device == "cpu" + + +def test_simulation_manager_cfg_initializes_window_camera_pose() -> None: + window_camera_pose = WindowCameraPoseCfg( + enable_hotkey=False, + convert_to_look_at=False, + ) + + cfg = SimulationManagerCfg(window_camera_pose=window_camera_pose) + + assert cfg.window_camera_pose == window_camera_pose + + +def test_simulation_manager_cfg_has_no_scene_construction_switch() -> None: + cfg = SimulationManagerCfg() + + assert "scene_construction" not in cfg.to_dict() + with pytest.raises(TypeError, match="scene_construction"): + SimulationManagerCfg(scene_construction="legacy") + + +def test_newton_physics_cfg_uses_device() -> None: + cfg = NewtonPhysicsCfg(device="cuda:1") + + serialized = cfg.to_dict() + assert serialized["device"] == "cuda:1" + assert serialized["physics_dt"] == 1.0 / 100.0 + assert "solver_type" not in serialized + + +@pytest.mark.no_sim +def test_newton_physics_cfg_preserves_dexsim_auto_solver_default() -> None: + from dexsim.engine.newton_physics import AutoSolverCfg + + cfg = NewtonPhysicsCfg() + + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) + + assert isinstance(dexsim_cfg.solver_cfg, AutoSolverCfg) + assert dexsim_cfg.solver_cfg.solver_type == "auto" + + +@pytest.mark.no_sim +def test_newton_physics_cfg_forwards_cuda_default_to_dexsim() -> None: + """The concrete Newton device default reaches the native config unchanged.""" + cfg = NewtonPhysicsCfg() + + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) + + assert dexsim_cfg.device == "cuda:0" + + +@pytest.mark.no_sim +def test_newton_physics_cfg_requires_dexsim_auto_solver_api( + monkeypatch: pytest.MonkeyPatch, +) -> None: + from dexsim.engine import newton_physics + + monkeypatch.delattr(newton_physics, "AutoSolverCfg") + + with pytest.raises( + ImportError, + match="AutoSolverCfg.*dexsim_engine build pinned by EmbodiChain", + ): + NewtonPhysicsCfg().to_dexsim_cfg(gpu_id=0) + + +@pytest.mark.no_sim +def test_newton_gradient_mode_rejects_auto_solver() -> None: + cfg = NewtonPhysicsCfg(requires_grad=True) + + with pytest.raises(RuntimeError, match="explicit.*semi_implicit"): + cfg.to_dexsim_cfg(gpu_id=0) + + +def test_newton_physics_cfg_passes_warp_log_suppression() -> None: + cfg = NewtonPhysicsCfg(suppress_warp_kernel_logs=False) + + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) + + assert dexsim_cfg.suppress_warp_kernel_logs is False + + +@pytest.mark.no_sim +@pytest.mark.parametrize("update_interval", [None, 4]) +def test_newton_physics_cfg_forwards_collision_pipeline_update_interval( + update_interval: int | None, +) -> None: + cfg = NewtonPhysicsCfg(collision_cfg={"update_interval": update_interval}) + + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) + + assert dexsim_cfg.collision_pipeline_cfg is not None + assert dexsim_cfg.collision_pipeline_cfg.update_interval == update_interval + assert not hasattr(cfg, "enable_collision_pipeline") + assert not hasattr(cfg, "collision_pipeline_update_interval") + + +@pytest.mark.parametrize( + ("physics_cfg", "expect_suppressed"), + [ + (NewtonPhysicsCfg(), True), + (NewtonPhysicsCfg(suppress_warp_kernel_logs=False), False), + (DefaultPhysicsCfg(), False), + ], +) +def test_warp_runtime_init_honors_newton_log_suppression( + monkeypatch: pytest.MonkeyPatch, + physics_cfg, + expect_suppressed: bool, +) -> None: + previous_log_level = sim_manager.wp.config.log_level + observed_log_levels = [] + + def fake_init() -> None: + observed_log_levels.append(sim_manager.wp.config.log_level) + + monkeypatch.setattr(sim_manager.wp, "init", fake_init) + try: + sim_manager._initialize_warp_runtime(physics_cfg) + expected_log_level = ( + sim_manager.wp.LOG_WARNING if expect_suppressed else previous_log_level + ) + assert observed_log_levels == [expected_log_level] + assert sim_manager.wp.config.log_level == previous_log_level + finally: + sim_manager.wp.config.log_level = previous_log_level + + +def test_newton_warp_log_suppression_covers_world_update() -> None: + previous_log_level = sim_manager.wp.config.log_level + observed_log_levels = [] + + class NoopProfiler: + def section(self, *_args, **_kwargs): + return nullcontext() + + class World: + def update(self, _physics_dt: float) -> None: + observed_log_levels.append(sim_manager.wp.config.log_level) + + manager = SimpleNamespace( + profiler=NoopProfiler(), + prepare=lambda: None, + is_physics_manually_update=True, + sim_config=SimpleNamespace( + physics_dt=0.01, + physics_cfg=NewtonPhysicsCfg(), + visualization=SimpleNamespace(backend="none"), + ), + update_gizmos=lambda: None, + _world=World(), + _visualization_sim_step=0, + _visualization_sim_time=0.0, + _window_record_state=None, + ) + try: + SimulationManager.update(manager, physics_dt=0.01) + assert observed_log_levels == [sim_manager.wp.LOG_WARNING] + assert sim_manager.wp.config.log_level == previous_log_level + finally: + sim_manager.wp.config.log_level = previous_log_level + + +@pytest.mark.no_sim +def test_newton_backend_exposes_resolved_solver_type() -> None: + backend = NewtonPhysicsBackend(SimpleNamespace()) + world_config = SimpleNamespace(newton_cfg=None) + sim_config = SimulationManagerCfg( + physics_cfg=NewtonPhysicsCfg( + device="cpu", + solver_cfg={"solver_type": "xpbd"}, + ), + ) + + backend.configure_world(world_config, sim_config) + + assert backend.solver_type == "xpbd" + assert world_config.newton_cfg.solver_cfg.solver_type == "xpbd" + + +@pytest.mark.no_sim +def test_newton_backend_reports_scene_resolved_auto_solver( + monkeypatch: pytest.MonkeyPatch, +) -> None: + world = object() + native_backend = SimpleNamespace(solver_type="mujoco_warp") + manager = SimpleNamespace(_world=world) + backend = NewtonPhysicsBackend(manager) + world_config = SimpleNamespace(newton_cfg=None) + sim_config = SimulationManagerCfg(physics_cfg=NewtonPhysicsCfg()) + monkeypatch.setattr( + "dexsim.engine.newton_physics.backend_registry.get_newton_backend", + lambda candidate: native_backend if candidate is world else None, + ) + + backend.configure_world(world_config, sim_config) + + assert world_config.newton_cfg.solver_cfg.solver_type == "auto" + assert backend.solver_type == "mujoco_warp" + + +def test_newton_teardown_releases_render_views_on_the_resolved_device( + monkeypatch: pytest.MonkeyPatch, +) -> None: + render_sync = MagicMock() + newton_backend = SimpleNamespace(render_sync=render_sync) + manager = SimpleNamespace(_world=object()) + backend = NewtonPhysicsBackend(manager) + world_config = SimpleNamespace(newton_cfg=None) + synchronize_device = MagicMock() + sim_config = SimulationManagerCfg( + gpu_id=2, + physics_cfg=NewtonPhysicsCfg(device="cuda"), + ) + monkeypatch.setattr( + newton_physics.wp, + "synchronize_device", + synchronize_device, + ) + from dexsim.engine.newton_physics import backend_registry + + monkeypatch.setattr( + backend_registry, + "get_newton_backend", + lambda world: newton_backend if world is manager._world else None, + ) + + backend.configure_world(world_config, sim_config) + backend.prepare_for_teardown() + + synchronize_device.assert_called_once_with("cuda:2") + render_sync.clear.assert_called_once_with() + + +def test_newton_teardown_skips_cpu_devices( + monkeypatch: pytest.MonkeyPatch, +) -> None: + render_sync = MagicMock() + newton_backend = SimpleNamespace(render_sync=render_sync) + manager = SimpleNamespace(_world=object()) + backend = NewtonPhysicsBackend(manager) + world_config = SimpleNamespace(newton_cfg=None) + synchronize_device = MagicMock() + sim_config = SimulationManagerCfg(physics_cfg=NewtonPhysicsCfg(device="cpu")) + monkeypatch.setattr( + newton_physics.wp, + "synchronize_device", + synchronize_device, + ) + from dexsim.engine.newton_physics import backend_registry + + monkeypatch.setattr( + backend_registry, + "get_newton_backend", + lambda world: newton_backend if world is manager._world else None, + ) + + backend.configure_world(world_config, sim_config) + backend.prepare_for_teardown() + + synchronize_device.assert_not_called() + render_sync.clear.assert_called_once_with() + + +def test_newton_backend_syncs_render_state_without_physics_step( + monkeypatch: pytest.MonkeyPatch, +) -> None: + world = object() + native_backend = SimpleNamespace( + sync_to_dexsim=MagicMock(), + sync_particle_fluids=MagicMock(), + ) + monkeypatch.setattr( + "dexsim.engine.newton_physics.backend_registry.get_newton_backend", + lambda candidate: native_backend if candidate is world else None, + ) + backend = NewtonPhysicsBackend(SimpleNamespace()) + + backend.sync_render_state(SimpleNamespace(world=world)) + + native_backend.sync_to_dexsim.assert_called_once_with(world) + native_backend.sync_particle_fluids.assert_called_once_with(world) + + +@pytest.mark.parametrize( + ("device", "initializes_direct_gpu"), + [(torch.device("cpu"), False), (torch.device("cuda"), True)], +) +def test_default_backend_prepares_runtime_for_device( + device: torch.device, + initializes_direct_gpu: bool, +) -> None: + manager = SimpleNamespace(device=device, _world=MagicMock()) + backend = DefaultPhysicsBackend(manager) + result = object() + + backend.prepare_spawn_runtime(result) + + if initializes_direct_gpu: + manager._world.init_gpu_physics.assert_called_once_with() + else: + manager._world.init_gpu_physics.assert_not_called() + + +@pytest.mark.no_sim +def test_newton_physics_cfg_converts_mapping_solver_cfg_to_dexsim_cfg() -> None: + from dexsim.engine.newton_physics import MJWarpSolverCfg + + cfg = NewtonPhysicsCfg( + device="cuda", + solver_cfg={ + "class_type": "MJWarpSolverCfg", + "iterations": 12, + "ls_iterations": 4, + "use_mujoco_contacts": False, + "enable_multiccd": True, + }, + ) + + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=2) + + assert dexsim_cfg.device == "cuda:2" + assert isinstance(dexsim_cfg.solver_cfg, MJWarpSolverCfg) + assert dexsim_cfg.solver_cfg.iterations == 12 + assert dexsim_cfg.solver_cfg.ls_iterations == 4 + assert dexsim_cfg.solver_cfg.use_mujoco_contacts is False + assert dexsim_cfg.solver_cfg.enable_multiccd is True + + +@pytest.mark.no_sim +def test_newton_physics_cfg_accepts_explicit_auto_solver_mapping() -> None: + from dexsim.engine.newton_physics import AutoSolverCfg + + cfg = NewtonPhysicsCfg(solver_cfg={"class_type": "AutoSolverCfg"}) + + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) + + assert isinstance(dexsim_cfg.solver_cfg, AutoSolverCfg) + + +@pytest.mark.no_sim +@pytest.mark.parametrize( + ("config_key", "solver_name"), + [("solver_type", "dexuni"), ("class_type", "DexUniSolverCfg")], +) +def test_newton_physics_cfg_accepts_dexuni_solver_mapping( + config_key: str, + solver_name: str, +) -> None: + from dexsim.engine.newton_physics import DexUniSolverCfg + + cfg = NewtonPhysicsCfg( + solver_cfg={ + config_key: solver_name, + "iterations": 12, + "step_rigid_bodies": False, + } + ) + + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) + + assert isinstance(dexsim_cfg.solver_cfg, DexUniSolverCfg) + assert dexsim_cfg.solver_cfg.solver_type == "dexuni" + assert dexsim_cfg.solver_cfg.iterations == 12 + assert dexsim_cfg.solver_cfg.step_rigid_bodies is False + + +@pytest.mark.no_sim +def test_newton_physics_cfg_directly_accepts_dexsim_solver_cfg_object() -> None: + from dexsim.engine.newton_physics import XPBDSolverCfg + + solver_cfg = XPBDSolverCfg(iterations=8, enable_restitution=True) + cfg = NewtonPhysicsCfg(solver_cfg=solver_cfg) + + dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) + + assert isinstance(dexsim_cfg.solver_cfg, XPBDSolverCfg) + assert dexsim_cfg.solver_cfg.iterations == 8 + assert dexsim_cfg.solver_cfg.enable_restitution is True diff --git a/tests/sim/test_sim_profiler.py b/tests/sim/test_sim_profiler.py index 1bd187ac7..3acb0ec04 100644 --- a/tests/sim/test_sim_profiler.py +++ b/tests/sim/test_sim_profiler.py @@ -22,6 +22,7 @@ import torch from embodichain.lab.sim import Profiler, ProfilerCfg, SimulationManager +from embodichain.lab.sim.cfg import DefaultPhysicsCfg pytestmark = pytest.mark.no_sim @@ -49,8 +50,10 @@ def _make_sim_update_probe(profiler: Profiler) -> SimulationManager: sim._visualization_runtime = None sim._visualization_sim_step = 0 sim._visualization_sim_time = 0.0 + sim.prepare = lambda: None sim.sim_config = types.SimpleNamespace( physics_dt=0.01, + physics_cfg=DefaultPhysicsCfg(), visualization=types.SimpleNamespace(backend="none"), ) return sim diff --git a/tests/sim/test_startup_summary.py b/tests/sim/test_startup_summary.py new file mode 100644 index 000000000..86793654a --- /dev/null +++ b/tests/sim/test_startup_summary.py @@ -0,0 +1,319 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import importlib +from types import SimpleNamespace + +import pytest +import torch + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import DefaultPhysicsCfg, NewtonPhysicsCfg, RenderCfg + +pytestmark = pytest.mark.no_sim + + +def _sim(physics_cfg=None): + cfg = SimulationManagerCfg( + headless=True, physics_cfg=physics_cfg, render_cfg=RenderCfg(renderer="hybrid") + ) + return SimpleNamespace( + sim_config=cfg, + device=torch.device(cfg.device), + num_envs=cfg.num_envs, + physics=SimpleNamespace( + name=( + "newton" if isinstance(cfg.physics_cfg, NewtonPhysicsCfg) else "default" + ), + solver_type=( + "mujoco_warp" + if isinstance(cfg.physics_cfg, NewtonPhysicsCfg) + else "TGS" + ), + cuda_graph_status="pending", + ), + _requested_renderer="auto", + _requested_solver="auto", + is_window_opened=False, + _robots={}, + _articulations={}, + _rigid_objects={}, + _rigid_object_groups={}, + _deformable_objects={}, + _sensors={}, + _lights={}, + _constraints={}, + _default_plane=None, + _world_config=SimpleNamespace(backend=SimpleNamespace(name="VULKAN")), + spawn_result=None, + ) + + +def test_startup_configuration_defaults_and_validation(): + cfg = SimulationManagerCfg() + assert getattr(cfg, "startup_summary", None) == "compact" + assert cfg.dexsim_startup_info is False + for mode in ("compact", "full", "off"): + assert SimulationManagerCfg(startup_summary=mode).startup_summary == mode + with pytest.raises(ValueError, match="startup_summary"): + SimulationManagerCfg(startup_summary="verbose") + + +def test_cpu_physics_does_not_imply_rendering_disabled(monkeypatch): + summary = importlib.import_module("embodichain.lab.sim._startup_summary") + monkeypatch.setattr(summary.torch.cuda, "is_available", lambda: False) + rows = summary.simulation_rows(_sim()) + text = summary.format_summary("Simulation initialized", rows, color=False) + assert "Default" in text + assert "TGS" in text + assert "auto -> hybrid" in text + assert "CLOSED" in text + assert "cpu" in text + assert "10 ms (100 Hz)" in text + assert "Engine threads" not in text and "Stepping" not in text + assert "PhysX" not in text + assert "\x1b" not in text and "\x00" not in text + + +def test_newton_reports_pending_then_resolved_solver_and_graph(monkeypatch): + summary = importlib.import_module("embodichain.lab.sim._startup_summary") + monkeypatch.setattr(summary.torch.cuda, "is_available", lambda: False) + sim = _sim(NewtonPhysicsCfg()) + sim.physics.solver_type = "auto" + before = str(summary.simulation_rows(sim)) + assert "PENDING" in before + sim.spawn_result = SimpleNamespace(topology_revision=1, needs_rebuild=False) + sim._ready_spawn_topology_revision = 1 + sim.physics.solver_type = "mujoco_warp" + after = str(summary.scene_rows(sim)) + assert "auto -> mujoco_warp" in after + assert "PENDING" in after and "CAPTURED" not in after + sim.physics.cuda_graph_status = "captured" + assert "CAPTURED" in str(summary.scene_rows(sim)) + + +def test_full_mode_includes_backend_specific_diagnostics(monkeypatch): + summary = importlib.import_module("embodichain.lab.sim._startup_summary") + monkeypatch.setattr(summary.torch.cuda, "is_available", lambda: False) + sim = _sim(DefaultPhysicsCfg(device="cuda:0")) + compact = str(summary.simulation_rows(sim)) + sim.sim_config.startup_summary = "full" + full = str(summary.simulation_rows(sim)) + assert "Contact capacity" not in compact + assert "Contact capacity" in full + assert "Cache" in full + + +def test_table_wraps_long_values_and_preserves_visible_alignment(): + summary = importlib.import_module("embodichain.lab.sim._startup_summary") + rows = [ + ("Runtime", "Config", "/some/very/long/path/" * 8), + ("Physics", "Solver", "mujoco_warp"), + ] + text = summary.format_summary("Simulation initialized", rows, color=False, width=80) + assert max(map(len, text.splitlines())) <= 80 + assert "mujoco_warp" in text + assert text.startswith("╭") and text.endswith("╯") + colored = summary.format_summary( + "Simulation initialized", rows, color=True, width=80 + ) + assert "\x1b[" in colored + + +def test_no_color_overrides_terminal_highlighting(monkeypatch): + summary = importlib.import_module("embodichain.lab.sim._startup_summary") + monkeypatch.setenv("NO_COLOR", "1") + monkeypatch.setattr(summary.sys, "stderr", SimpleNamespace(isatty=lambda: True)) + assert "\x1b" not in summary.format_summary("Ready", [("Scene", "State", "READY")]) + + +def test_startup_emission_is_once_and_off_is_quiet(monkeypatch): + from embodichain.utils import logger + + calls = [] + monkeypatch.setattr(logger, "log_info", lambda text, **kwargs: calls.append(text)) + summary = importlib.import_module("embodichain.lab.sim._startup_summary") + monkeypatch.setattr(summary.torch.cuda, "is_available", lambda: False) + sim = object.__new__(SimulationManager) + sim.__dict__.update(vars(_sim())) + sim._startup_summary_logged = False + sim._scene_summary_logged = False + sim._log_startup_summary() + sim._log_startup_summary() + assert len(calls) == 1 + sim.sim_config.startup_summary = "off" + sim._log_scene_summary() + assert len(calls) == 1 + + +def test_default_solver_reads_native_tgs_switch(monkeypatch): + import dexsim + from embodichain.lab.sim.physics.default import DefaultPhysicsBackend + + cfg = SimpleNamespace(enable_tgs=True) + monkeypatch.setattr(dexsim, "get_physics_config", lambda: cfg) + backend = DefaultPhysicsBackend(_sim()) + assert backend.solver_type == "TGS" + cfg.enable_tgs = False + assert backend.solver_type == "PGS" + + +def test_scene_emission_waits_for_readiness_and_is_once(monkeypatch): + from embodichain.utils import logger + + sim = object.__new__(SimulationManager) + sim.__dict__.update(vars(_sim())) + sim._is_constructed = True + sim._defer_startup_summary = False + sim._scene_summary_logged = False + sim._spawn_scene = SimpleNamespace( + builder=SimpleNamespace(is_finalized=False, has_pending_changes=False) + ) + calls = [] + monkeypatch.setattr(logger, "log_info", lambda text, **kwargs: calls.append(text)) + sim._log_scene_summary() + assert not calls + sim._spawn_scene.builder.is_finalized = True + sim._spawn_scene.builder.result = SimpleNamespace( + topology_revision=1, needs_rebuild=False + ) + sim._ready_spawn_topology_revision = 1 + sim._log_scene_summary() + sim._log_scene_summary() + assert len(calls) == 1 and "READY" in calls[0] + + +def test_newton_scene_emission_waits_for_cuda_graph_capture(monkeypatch): + from embodichain.utils import logger + + sim = object.__new__(SimulationManager) + sim.__dict__.update(vars(_sim(NewtonPhysicsCfg()))) + sim._is_constructed = True + sim._defer_startup_summary = False + sim._scene_summary_logged = False + sim._spawn_scene = SimpleNamespace( + builder=SimpleNamespace( + is_finalized=True, + has_pending_changes=False, + result=SimpleNamespace(topology_revision=1, needs_rebuild=False), + ) + ) + sim._ready_spawn_topology_revision = 1 + calls = [] + monkeypatch.setattr(logger, "log_info", lambda text, **kwargs: calls.append(text)) + + sim._log_scene_summary() + assert not calls + assert not sim._scene_summary_logged + + sim.physics.cuda_graph_status = "captured" + sim._log_scene_summary() + sim._log_scene_summary() + assert len(calls) == 1 + assert "CAPTURED" in calls[0] + + +def test_world_config_receives_startup_switch_before_construction(monkeypatch): + import embodichain.lab.sim.sim_manager as manager_module + + native_cfg = SimpleNamespace( + raytrace_config=SimpleNamespace(), postprocess_config=SimpleNamespace() + ) + monkeypatch.setattr(manager_module.dexsim, "WorldConfig", lambda: native_cfg) + sim = object.__new__(SimulationManager) + sim._material_cache_dir = "/tmp/test-material-cache" + sim.physics = SimpleNamespace(configure_world=lambda *_: None) + cfg = SimulationManagerCfg(render_cfg=RenderCfg(renderer="hybrid")) + assert sim._convert_sim_config(cfg).log_startup_info is False + cfg.dexsim_startup_info = True + assert sim._convert_sim_config(cfg).log_startup_info is True + + +@pytest.mark.parametrize( + "mode,native_info", [("compact", False), ("full", True), ("off", False)] +) +def test_gym_config_preserves_summary_preferences(mode, native_info): + from embodichain.lab.gym.utils.gym_utils import ( + config_to_cfg, + DEFAULT_MANAGER_MODULES, + ) + + cfg = config_to_cfg( + { + "id": "EmbodiedEnv-v1", + "physics": "default", + "env": {}, + "robot": {"uid": "TestRobot"}, + "startup_summary": mode, + "dexsim_startup_info": native_info, + }, + manager_modules=DEFAULT_MANAGER_MODULES, + ) + assert cfg.sim_cfg.startup_summary == mode + assert cfg.sim_cfg.dexsim_startup_info is native_info + + +def test_summary_uses_existing_device_metadata_without_runtime_queries(monkeypatch): + summary = importlib.import_module("embodichain.lab.sim._startup_summary") + sim = _sim() + sim._render_device_name = "Existing renderer GPU" + + def unexpected_query(*args, **kwargs): + pytest.fail("A read-only startup snapshot must not initialize CUDA") + + monkeypatch.setattr(torch.cuda, "is_available", unexpected_query) + monkeypatch.setattr(torch.cuda, "get_device_name", unexpected_query) + assert "Existing renderer GPU" in str(summary.simulation_rows(sim)) + + +def test_failed_preparation_does_not_report_ready_or_consume_snapshot(monkeypatch): + from embodichain.utils import logger + + sim = object.__new__(SimulationManager) + sim.__dict__.update(vars(_sim())) + sim._is_constructed = True + sim._defer_startup_summary = False + sim._scene_summary_logged = False + result = SimpleNamespace(topology_revision=1, needs_rebuild=False) + + def fail_runtime(_): + raise RuntimeError("runtime not prepared") + + scene = SimpleNamespace( + builder=SimpleNamespace( + is_finalized=True, result=result, has_pending_changes=False + ), + prepare_runtime_config=fail_runtime, + bind=lambda: None, + ) + sim._spawn_scene = scene + sim._world = SimpleNamespace(render_camera_group=lambda _: None) + calls = [] + monkeypatch.setattr(logger, "log_info", lambda text, **kwargs: calls.append(text)) + with pytest.raises(RuntimeError, match="runtime not prepared"): + sim.prepare() + sim.render_camera_group([]) + assert not calls + assert not sim._scene_summary_logged + scene.prepare_runtime_config = lambda _: None + sim._prepare_spawn_runtime = lambda _: None + sim._sync_spawn_render_state = lambda _: None + sim._attach_parented_cameras = lambda: None + sim.prepare() + sim.render_camera_group([]) + assert len(calls) == 1 and "READY" in calls[0] diff --git a/tests/test_agent_context_map.py b/tests/test_agent_context_map.py index 2c4169047..fab1a954f 100644 --- a/tests/test_agent_context_map.py +++ b/tests/test_agent_context_map.py @@ -69,6 +69,7 @@ def test_map_registers_the_supported_context_domains() -> None: "data-assets", "data-pipeline", "robot-workspace", + "differentiable-env", } assert set(topics) == expected_topic_ids @@ -81,6 +82,7 @@ def test_new_topics_cover_their_owning_packages() -> None: "data-assets": "embodichain/data/", "data-pipeline": "embodichain/data_pipeline/", "robot-workspace": "embodichain/lab/sim/motion/workspace/", + "differentiable-env": "embodichain/lab/gym/envs/differentiable_env.py", } for topic_id, prefix in expected_source_prefixes.items(): @@ -108,6 +110,8 @@ def test_representative_queries_route_against_the_repository_map() -> None: "OnlineDataEngine 采样失败后怎么处理？": ["data-pipeline"], "SimReady pipeline 的入口在哪里？": ["gen-sim"], "get_data_path 如何解析资产路径？": ["data-assets"], + "Newton physics backend config 在哪里？": ["simulation-system"], + "可微环境 APG 如何重置？": ["differentiable-env"], } assert { diff --git a/tests/test_main.py b/tests/test_main.py index 82e7f4a28..42019a1fc 100644 --- a/tests/test_main.py +++ b/tests/test_main.py @@ -243,7 +243,7 @@ def test_config_environment_entries_use_task_paths_and_artifacts( encoding="utf-8", ) (expert_task / "env.yaml").write_text( - "environment_id: pick_place\nsimulation: {}\nenv: {}\n", + "environment_id: pick_place\nphysics: default\nsimulation: {}\nenv: {}\n", encoding="utf-8", ) (expert_task / "notes.yaml").write_text( diff --git a/tests/test_release_metadata.py b/tests/test_release_metadata.py index 53600115b..71187fb3a 100644 --- a/tests/test_release_metadata.py +++ b/tests/test_release_metadata.py @@ -16,13 +16,17 @@ from __future__ import annotations -import tomllib from pathlib import Path from zipfile import ZipFile import pytest from packaging.requirements import Requirement +try: + import tomllib +except ModuleNotFoundError: # Python 3.10 + import tomli as tomllib + from scripts.validate_wheel_metadata import WheelMetadataError, validate_wheel from setup import get_package_dir, get_packages diff --git a/tests/toolkits/test_grasp_pose_generator.py b/tests/toolkits/test_grasp_pose_generator.py index 27f60ca43..99a8bc2ed 100644 --- a/tests/toolkits/test_grasp_pose_generator.py +++ b/tests/toolkits/test_grasp_pose_generator.py @@ -31,7 +31,7 @@ from embodichain.lab.sim.objects import Robot, RigidObject from embodichain.compute.trajectory import interpolate_with_distance from embodichain.toolkits.graspkit import ParallelJawGripperModelCfg -from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.lab.sim.motion.solvers import PytorchSolverCfg from embodichain.data import get_data_path from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser @@ -41,7 +41,7 @@ JointDrivePropertiesCfg, RobotCfg, LightCfg, - RigidBodyAttributesCfg, + RigidBodyPhysicsCfg, RigidObjectCfg, URDFCfg, ) @@ -62,7 +62,7 @@ def initialize_simulation() -> SimulationManager: """ config = SimulationManagerCfg( headless=True, - sim_device=torch.device("cuda"), + device=torch.device("cuda"), render_cfg=RenderCfg(renderer="auto"), physics_dt=1.0 / 100.0, arena_space=2.5, @@ -103,7 +103,7 @@ def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]) -> Robot: {"component_type": "hand", "urdf_path": gripper_urdf_path}, ] ), - drive_pros=JointDrivePropertiesCfg( + joint_drive_props=JointDrivePropertiesCfg( stiffness={"Joint[0-9]": 1e4, "FINGER[1-2]": 1e3}, damping={"Joint[0-9]": 1e3, "FINGER[1-2]": 1e2}, max_effort={"Joint[0-9]": 1e5, "FINGER[1-2]": 1e4}, @@ -136,13 +136,17 @@ def create_mug(sim: SimulationManager): uid="table", shape=MeshCfg( fpath=get_data_path("CoffeeCup/cup.ply"), + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=16, + ), ), - attrs=RigidBodyAttributesCfg( - mass=0.01, - dynamic_friction=0.97, - static_friction=0.99, + attrs=RigidBodyPhysicsCfg.from_dict( + { + "mass_props": {"mass": 0.01}, + "material_props": {"dynamic_friction": 0.97, "static_friction": 0.99}, + } ), - max_convex_hull_num=16, init_pos=[0.55, 0.0, 0.01], init_rot=[0.0, 0.0, -90], body_scale=(4, 4, 4), @@ -207,6 +211,7 @@ def test_grasp_pose_generator(): try: robot = create_robot(sim, position=[0.0, 0.0, 0.0]) mug = create_mug(sim) + sim.prepare() # get mug grasp pose grasp_generator = AntipodalGraspPoseGenerator( diff --git a/tests/utils/test_configclass.py b/tests/utils/test_configclass.py new file mode 100644 index 000000000..d0a5b8749 --- /dev/null +++ b/tests/utils/test_configclass.py @@ -0,0 +1,41 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for the configclass decorator.""" + +from __future__ import annotations + +from dataclasses import fields +from typing import ClassVar + +from embodichain.utils import configclass + + +@configclass +class _DeferredClassVarCfg: + values: list[int] = [] + label: ClassVar[str] = "shared" + + +def test_deferred_classvar_is_not_converted_to_a_dataclass_field() -> None: + first = _DeferredClassVarCfg() + second = _DeferredClassVarCfg() + first.values.append(1) + + assert [item.name for item in fields(_DeferredClassVarCfg)] == ["values"] + assert first.to_dict() == {"values": [1]} + assert second.values == [] + assert _DeferredClassVarCfg.label == "shared" diff --git a/tests/utils/test_logger.py b/tests/utils/test_logger.py index a5856237d..6b6455b2d 100644 --- a/tests/utils/test_logger.py +++ b/tests/utils/test_logger.py @@ -157,3 +157,48 @@ def test_log_error_allows_custom_level_color(): f"\033[95mERROR {_RESET_COLOR} │ EmbodiChain │ " f"\033[95mTest message{_RESET_COLOR}" ) + + +def test_console_handler_strips_ansi_when_redirected(monkeypatch): + import io + + monkeypatch.delenv("NO_COLOR", raising=False) + handler_type = getattr(logger_module, "_ConsoleHandler", None) + assert handler_type is not None + stream = io.StringIO() + handler = handler_type(stream) + handler.setFormatter(logger_module._DEFAULT_FORMATTER) + record = logging.LogRecord( + "test", + logging.WARNING, + __file__, + 1, + "\033[93mWARNING: keep this warning\033[0m", + (), + None, + ) + handler.emit(record) + assert "keep this warning" in stream.getvalue() + assert "\033" not in stream.getvalue() + + +def test_console_handler_honors_no_color_on_tty(monkeypatch): + import io + + class Terminal(io.StringIO): + def isatty(self): + return True + + stream = Terminal() + handler = logger_module._ConsoleHandler(stream) + record = logging.LogRecord( + "test", logging.INFO, __file__, 1, "\033[92mREADY\033[0m", (), None + ) + monkeypatch.delenv("NO_COLOR", raising=False) + handler.emit(record) + assert "\033[92m" in stream.getvalue() + stream.truncate(0) + stream.seek(0) + monkeypatch.setenv("NO_COLOR", "1") + handler.emit(record) + assert stream.getvalue() == "READY\n" diff --git a/tests/utils/test_math.py b/tests/utils/test_math.py new file mode 100644 index 000000000..fab263b1c --- /dev/null +++ b/tests/utils/test_math.py @@ -0,0 +1,94 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import torch + +from embodichain.utils.math import ( + convert_quat, + default_orientation, + matrix_from_quat, + quat_apply, + quat_conjugate, + quat_from_matrix, + quat_mul, + trans_matrix_to_xyz_quat, + xyz_quat_to_4x4_matrix, +) + + +def _distinct_xyzw() -> torch.Tensor: + """Return a normalized quaternion whose components expose order mistakes.""" + quaternion = torch.tensor([[1.0, 2.0, 3.0, 4.0]], dtype=torch.float32) + return quaternion / torch.linalg.vector_norm(quaternion, dim=-1, keepdim=True) + + +def test_quaternion_matrix_round_trip_uses_xyzw() -> None: + quaternion = _distinct_xyzw() + + rotation = matrix_from_quat(quaternion) + restored = quat_from_matrix(rotation) + + torch.testing.assert_close(restored, quaternion, atol=1.0e-6, rtol=1.0e-6) + + +def test_quaternion_product_and_conjugate_return_xyzw_identity() -> None: + quaternion = _distinct_xyzw() + + product = quat_mul(quaternion, quat_conjugate(quaternion)) + + torch.testing.assert_close( + product, + torch.tensor([[0.0, 0.0, 0.0, 1.0]]), + atol=1.0e-6, + rtol=1.0e-6, + ) + + +def test_quaternion_application_reads_scalar_from_last_component() -> None: + half_sqrt_two = 2.0**-0.5 + z_quarter_turn_xyzw = torch.tensor( + [[0.0, 0.0, half_sqrt_two, half_sqrt_two]], dtype=torch.float32 + ) + + rotated = quat_apply(z_quarter_turn_xyzw, torch.tensor([[1.0, 0.0, 0.0]])) + + torch.testing.assert_close( + rotated, + torch.tensor([[0.0, 1.0, 0.0]]), + atol=1.0e-6, + rtol=1.0e-6, + ) + + +def test_pose_vector_round_trip_uses_xyz_plus_xyzw() -> None: + pose = torch.cat((torch.tensor([[0.25, -0.5, 0.75]]), _distinct_xyzw()), dim=-1) + + restored = trans_matrix_to_xyz_quat(xyz_quat_to_4x4_matrix(pose)) + + torch.testing.assert_close(restored, pose, atol=1.0e-6, rtol=1.0e-6) + + +def test_identity_and_boundary_conversion_orders_are_explicit() -> None: + xyzw = torch.tensor([[1.0, 2.0, 3.0, 4.0]]) + + torch.testing.assert_close( + default_orientation(1, "cpu"), torch.tensor([[0.0, 0.0, 0.0, 1.0]]) + ) + torch.testing.assert_close( + convert_quat(xyzw, to="wxyz"), torch.tensor([[4.0, 1.0, 2.0, 3.0]]) + ) diff --git a/tests/visualization/test_protocol.py b/tests/visualization/test_protocol.py index 5c5ca4d3c..19a161d90 100644 --- a/tests/visualization/test_protocol.py +++ b/tests/visualization/test_protocol.py @@ -35,18 +35,21 @@ ) -def test_pose_conversion_preserves_embodichain_wxyz_order() -> None: - pose = np.array([1.0, 2.0, 3.0, 2.0, 0.0, 0.0, 0.0], dtype=np.float32) +def test_pose_conversion_converts_embodichain_xyzw_to_protocol_wxyz() -> None: + pose = np.array([1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 4.0], dtype=np.float32) position, wxyz = pose_to_position_wxyz(pose) np.testing.assert_allclose(position, [1.0, 2.0, 3.0]) - np.testing.assert_allclose(wxyz, [1.0, 0.0, 0.0, 0.0]) + np.testing.assert_allclose( + wxyz, + np.array([4.0, 1.0, 2.0, 3.0]) / np.sqrt(30.0), + ) def test_pose_conversion_accepts_batch_of_four_pose_vectors() -> None: poses = np.tile( - np.array([[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]], dtype=np.float32), + np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]], dtype=np.float32), (4, 1), ) diff --git a/tests/visualization/test_scene_exporter.py b/tests/visualization/test_scene_exporter.py index ef94f03d5..dbf7945c5 100644 --- a/tests/visualization/test_scene_exporter.py +++ b/tests/visualization/test_scene_exporter.py @@ -164,7 +164,8 @@ def get_local_pose(self, to_matrix: bool = False) -> np.ndarray: class _DeformableObject: - def __init__(self) -> None: + def __init__(self, deformable_type: str) -> None: + self.deformable_type = deformable_type local_vertices = np.array( [[0.0, 0.0, 0.0], [0.15, 0.0, 0.0], [0.0, 0.15, 0.0]], dtype=np.float32, @@ -177,16 +178,10 @@ def __init__(self) -> None: ) self._faces = np.array([[0, 1, 2]], dtype=np.int32) - def get_current_collision_vertices(self) -> np.ndarray: - return self.vertices - - def get_current_vertex_position(self) -> np.ndarray: + def get_surface_vertices(self) -> np.ndarray: return self.vertices - def get_collision_surface_triangles(self, env_ids: list[int]) -> np.ndarray: - return self.get_triangles(env_ids) - - def get_triangles(self, env_ids: list[int]) -> np.ndarray: + def get_surface_triangles(self, env_ids: list[int]) -> np.ndarray: return np.stack([self._faces for _ in env_ids]) @@ -224,17 +219,11 @@ def get_rigid_object_group_uid_list(self) -> list[str]: def get_rigid_object_group(self, uid: str) -> None: raise AssertionError(f"Unexpected rigid-object-group lookup: {uid}") - def get_soft_object_uid_list(self) -> list[str]: + def get_deformable_object_uid_list(self) -> list[str]: return [] - def get_soft_object(self, uid: str) -> None: - raise AssertionError(f"Unexpected soft-object lookup: {uid}") - - def get_cloth_object_uid_list(self) -> list[str]: - return [] - - def get_cloth_object(self, uid: str) -> None: - raise AssertionError(f"Unexpected cloth-object lookup: {uid}") + def get_deformable_object(self, uid: str) -> None: + raise AssertionError(f"Unexpected deformable-object lookup: {uid}") def get_sensor_uid_list(self) -> list[str]: return [] @@ -265,17 +254,11 @@ def get_articulation_uid_list(self) -> list[str]: def get_articulation(self, uid: str) -> None: raise AssertionError(f"Unexpected articulation lookup: {uid}") - def get_soft_object_uid_list(self) -> list[str]: - return [] - - def get_soft_object(self, uid: str) -> None: - raise AssertionError(f"Unexpected soft-object lookup: {uid}") - - def get_cloth_object_uid_list(self) -> list[str]: + def get_deformable_object_uid_list(self) -> list[str]: return [] - def get_cloth_object(self, uid: str) -> None: - raise AssertionError(f"Unexpected cloth-object lookup: {uid}") + def get_deformable_object(self, uid: str) -> None: + raise AssertionError(f"Unexpected deformable-object lookup: {uid}") def get_sensor_uid_list(self) -> list[str]: return [] @@ -453,8 +436,8 @@ class _CompleteSimulation(_Simulation): def __init__(self) -> None: super().__init__() self.rigid_group = _RigidObjectGroup() - self.soft = _DeformableObject() - self.cloth = _DeformableObject() + self.soft = _DeformableObject("volume") + self.cloth = _DeformableObject("surface") def get_rigid_object_group_uid_list(self) -> list[str]: return ["pair"] @@ -463,19 +446,11 @@ def get_rigid_object_group(self, uid: str) -> _RigidObjectGroup: assert uid == "pair" return self.rigid_group - def get_soft_object_uid_list(self) -> list[str]: - return ["jelly"] - - def get_soft_object(self, uid: str) -> _DeformableObject: - assert uid == "jelly" - return self.soft - - def get_cloth_object_uid_list(self) -> list[str]: - return ["flag"] + def get_deformable_object_uid_list(self) -> list[str]: + return ["jelly", "flag"] - def get_cloth_object(self, uid: str) -> _DeformableObject: - assert uid == "flag" - return self.cloth + def get_deformable_object(self, uid: str) -> _DeformableObject: + return {"jelly": self.soft, "flag": self.cloth}[uid] def test_manifest_deduplicates_geometry_and_escapes_paths() -> None: