diff --git a/.agents/skills/add-task-env/references/rl.md b/.agents/skills/add-task-env/references/rl.md index 07f66e40d8..fbafcabdc0 100644 --- a/.agents/skills/add-task-env/references/rl.md +++ b/.agents/skills/add-task-env/references/rl.md @@ -13,7 +13,7 @@ decision is which environment boundary the prompt requires. | Differentiable dynamics or APG | Differentiable lightweight env | `trainer.learning_env` | Do not use a simulator Gym config for APG. Differentiable algorithms require a -`DifferentiableVecEnv` and are rejected on the simulator path. +`DifferentiableEnvProtocol` and are rejected on the simulator path. ## Simulator Gym RL @@ -71,7 +71,7 @@ extensions/embodichain_tasks/embodichain_tasks//.py ``` Register the factory with `@register_learning_env` and implement the -`LearningVecEnv` structural contract: +`LearningEnvProtocol` structural contract: - `num_envs`, `device`, `single_observation_space`, and `single_action_space`; @@ -81,7 +81,7 @@ Register the factory with `@register_learning_env` and implement the - `close()`. For APG or another differentiable algorithm, implement -`DifferentiableVecEnv.detach_state()` as the truncated-backpropagation +`DifferentiableEnvProtocol.detach_state()` as the truncated-backpropagation boundary. It must detach dynamic state without resetting or resampling the task. diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 6551743cb7..bd79f1ed60 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -48,7 +48,7 @@ topics: title: Environment Framework aliases: [env framework, environment framework, task registration, list task, expert trajectory, 环境框架, 任务环境, 专家轨迹] - keywords: [BaseEnv, EmbodiedEnv, EpisodeContext, EnvCfg, ControlTiming, resolve_control_timing, register_env, list-task, run-task, run-example, target_control_frequency, + keywords: [UnitreeG1MotionRL-v1, motion_tracking, BaseEnv, EmbodiedEnv, EpisodeContext, EnvCfg, ControlTiming, resolve_control_timing, register_env, list-task, run-task, run-example, target_control_frequency, sim_steps_per_control, step_dt, environment.component, embodiment.component, file-owned physics backend, physics_config, ControllerAction, EnvProfiler, startup_summary, dexsim_startup_info, ExpertTrajectoryCfg, joint_command_mode, position_velocity, qpos qvel, expert action schema, ObservationHistorySnapshot, ObservationFrame, @@ -112,7 +112,7 @@ topics: title: IK Solver System aliases: [ik solver, inverse kinematics, IK 求解器, 逆运动学] keywords: [BaseSolver, SolverCfg, SRSSolver, OPWSolver, URSolver, PinkSolver, PytorchSolver, PinocchioSolver, - DifferentialSolver, NeuralIKSolver, FEPSolver, FEPSolverCfg, FEP, SSIKSolver, SSIKSolverCfg, SSIK, QposSeedSampler, QposSeedSelSampler, NullSpacePostureTask, compute kinematics, Warp + DifferentialSolver, NeuralIKSolver, FEPSolver, FEPSolverCfg, FEP, SSIKSolver, SSIKSolverCfg, SSIK, TreeKinematics, LinkKinematics, matrix_product, matrix_vector_product, QposSeedSampler, QposSeedSelSampler, NullSpacePostureTask, compute kinematics, Warp kinematics] paths: [topics/ik-solvers/ik-solvers.md] source_of_truth: @@ -380,7 +380,7 @@ 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, ScheduledDifferentiableVecEnv, DifferentiableRolloutSpec, + PPO, GRPO, APG, APGActorCritic, apg_actor_critic, SyncCollector, ScheduledDifferentiableEnvProtocol, DifferentiableRolloutSpec, complete rollout, full horizon, gradient accumulation, action adjoint, observation normalization] paths: [topics/rl-learning/rl-learning.md] source_of_truth: @@ -602,7 +602,7 @@ topics: title: Data Pipeline aliases: [real2sim motion, real2sim trajectory, real2sim, 真实轨迹原型, online data, online dataset, async recorder, dataset persistence, simple BC demo, ODS documentation, ODS 文档, 数据管线, 在线数据, 数据集持久化, 数据保存, 深度视频, 数据集校验, 数据集转换, 数据集划分] - keywords: [OnlineDataEngine, OnlineDataset, sample_batch, sample_batch_with_context, lock_index, LeRobotRecorder, AsyncLeRobotRecorder, + keywords: [MotionLibrary, MotionStartSampler, OnlineDataEngine, OnlineDataset, sample_batch, sample_batch_with_context, lock_index, LeRobotRecorder, AsyncLeRobotRecorder, DepthSidecarManager, depth_meta.json, fragment_id, finalize, expert action schema, qpos_slice, qvel_slice, validate_dataset, create_split_manifest, recover_recording, episode_uuid, language_persistent, SharedLanguageRegistry, resolve_language, configure_ods, ODSReuseProfileCfg, @@ -617,6 +617,8 @@ topics: - embodichain/data_pipeline/configuration.py - embodichain/data_pipeline/real2sim/ - embodichain/data_pipeline/runtime.py + - embodichain/data_pipeline/datasets/motion.py + - embodichain/data_pipeline/datasets/motion_sampling.py - embodichain/data_pipeline/datasets/online_data.py - embodichain/data_pipeline/datasets/sequence.py - embodichain/data_pipeline/datasets/sources.py @@ -699,14 +701,22 @@ topics: - id: robot-world-model title: Robot World Model aliases: [robot world model, RWM, 机器人世界模型] - keywords: [RobotWorldModel, RWMPredictor, TransitionReplay, build_rwm_trainer, + keywords: [RobotWorldModel, KinematicWorldModel, MotionTrackingPolicy, MotionModelEnv, OnlineModelBasedSchedule, RWMPredictor, TransitionReplay, build_rwm_trainer, replay_supervised_batch, learning/rwm] paths: [topics/robot-world-model/robot-world-model.md] source_of_truth: - extensions/embodichain_models/embodichain_models/dynamics/rwm/ - examples/learning/rwm/ + - examples/learning/motion_tracking/ + - embodichain/learning/dynamics/schedule.py + - extensions/embodichain_models/embodichain_models/policies/motion_tracking.py + - extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/motion_model.py watch_paths: [extensions/embodichain_models/embodichain_models/dynamics/rwm/, - examples/learning/rwm/, tests/models/dynamics/rwm/, + examples/learning/rwm/, examples/learning/motion_tracking/, tests/models/dynamics/rwm/, + embodichain/learning/dynamics/, tests/learning/test_online_schedule.py, + extensions/embodichain_models/embodichain_models/policies/motion_tracking.py, + extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/motion_model.py, + tests/gym/envs/tasks/test_motion_model.py, docs/source/features/neural_dynamics/rwm.md, docs/source/api_reference/rwm.rst] related_topics: [supervised-learning, learning-layers, neural-dynamics] status: active diff --git a/agent_context/topics/data-pipeline/data-pipeline.md b/agent_context/topics/data-pipeline/data-pipeline.md index e3a9b80cf3..44ff61c455 100644 --- a/agent_context/topics/data-pipeline/data-pipeline.md +++ b/agent_context/topics/data-pipeline/data-pipeline.md @@ -72,6 +72,20 @@ reader. It measures initial loading rather than per-batch disk reads and does not replace recording or offline-inspection tools. See the [loading design](../../../docs/architecture/dataset-loading-design.md) for source boundaries. +## Reference motions + +`datasets/motion.py` owns `MotionLibrary`: resident root/joint reference states, +explicit clip boundaries and current/future-frame queries. Portable archives +record joint order and quaternion convention. CSV preparation calls +`compute/trajectory/floating.py` for uniform-time resampling and velocity +estimation. Task environments own reference progress and reset selection; +world-model transition collection remains with the learning workflow. + +`datasets/motion_sampling.py` owns reference-start curriculum statistics. It mixes +uniform bins, capped low-return softmax and historical low-return weights. Repeated +bins in one completed-episode batch are averaged before EMA updates. The sampler +checkpoints motion layout and statistics; the task owns its named sampling RNG. + ## Online worker contract Shared image buffers are allocated by `init_rollout_buffer_from_config()` in diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index b2fe81f739..168fcf8a3a 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -40,6 +40,16 @@ rewards and contact termination rules remain in the task package; robot defaults and the distinct asset/contract joint orders are owned by [robot-system](../robot-system/robot-system.md). +Reference-motion tracking is owned by +`extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/`. +`g1_motion.py` owns simulator state, reference-frame progress, selected-row resets +and pre-action/terminal snapshots; unusable nonfinite successors hard-terminate +and are excluded from WM supervision. `_task.py` owns tensor objectives shared +with model-driven rollouts. The task's `ReferenceJointPositionAction` records +policy outputs separately from the reference-combined setpoint and executed +joint target. Read the task configuration's README for data preparation, +training and per-clip evaluation commands. + ## Launcher argument ownership `embodichain run-task` is the sole task execution command, dispatched to diff --git a/agent_context/topics/ik-solvers/ik-solvers.md b/agent_context/topics/ik-solvers/ik-solvers.md index f0b3a83244..7dcece4802 100644 --- a/agent_context/topics/ik-solvers/ik-solvers.md +++ b/agent_context/topics/ik-solvers/ik-solvers.md @@ -10,6 +10,8 @@ | Chain/reduced-model construction | `embodichain/lab/sim/utility/solver_utils.py` | | Seed generation and retrieval | `embodichain/lab/sim/motion/solvers/qpos_seed_sampler.py`, `qpos_seed_sel_sampler.py` in that directory | | Pink posture integration | `embodichain/lab/sim/motion/solvers/null_space_posture_task.py` | +| Floating-root full-tree FK and link-origin velocities | `embodichain/compute/kinematics/tree.py` → `TreeKinematics`, `LinkKinematics`; URDF geometry and explicit tensor joint order | +| Batched matrix products shared with models/tasks | `embodichain/compute/kinematics/matrix.py` → `matrix_product`, `matrix_vector_product`; compiled small matrices and Torch fallback | | Analytical kernels | `embodichain/compute/kinematics/_warp/` | | Bounded B=1 waypoint branch search with solver callbacks | `embodichain/compute/kinematics/ik_path.py`; stateful adapter in `lab/sim/motion/ik.py` | diff --git a/agent_context/topics/learning-layers/learning-layers.md b/agent_context/topics/learning-layers/learning-layers.md index b77ae22ac9..039a89773b 100644 --- a/agent_context/topics/learning-layers/learning-layers.md +++ b/agent_context/topics/learning-layers/learning-layers.md @@ -25,7 +25,9 @@ checkpoint/device protocol. It loads on demand so FK/IK imports do not load Warp Callers resolve robot caps and own geometric validation. Gradients are first-order and local to fixed filtering, sampling topology and active constraints. `learning/models/` owns general MLPs, feature normalization -and configuration-driven MLP construction. These shared components do not import +and configuration-driven MLP construction. `EmpiricalNormalizer.scale` exposes +the configured additive or quadratic epsilon scale; clipping is optional. +These shared components do not import RL, TensorDict or simulation. Concrete NeRD models, codecs and the shared-SL recipe live in the bundled `embodichain_models.dynamics.nerd` extension; teacher collection, historical training and research belong to diff --git a/agent_context/topics/rl-learning/rl-learning.md b/agent_context/topics/rl-learning/rl-learning.md index 946387e83d..c71b05110c 100644 --- a/agent_context/topics/rl-learning/rl-learning.md +++ b/agent_context/topics/rl-learning/rl-learning.md @@ -88,13 +88,19 @@ Select this path with `trainer.learning_env`, either as a registered name or as a mapping with `name` and `cfg`. `build_learning_env()` resolves factories registered with -`@register_learning_env`. A learning environment implements the -`LearningVecEnv` protocol. Differentiable algorithms additionally require -`DifferentiableVecEnv.detach_state()` as the truncated-backpropagation +`@register_learning_env`. Online workflows may supply `resources` to +`build_learning_policy_runtime`: the objects are passed by reference to the selected +environment factory, and keys cannot also appear in its scalar configuration. +Custom registered policies may implement `from_config(config, *, obs_space, +action_space, device)` to own their configuration and space validation. A learning environment implements the +`LearningEnvProtocol` protocol. The former `LearningVecEnv`, +`DifferentiableVecEnv` and `ScheduledDifferentiableVecEnv` names remain exact +aliases for downstream import compatibility. Differentiable algorithms additionally require +`DifferentiableEnvProtocol.detach_state()` as the truncated-backpropagation boundary. Variable-horizon APG environments may also implement -`ScheduledDifferentiableVecEnv.prepare_differentiable_rollout()`. Its +`ScheduledDifferentiableEnvProtocol.prepare_differentiable_rollout()`. Its `DifferentiableRolloutSpec` selects the next reset's complete horizon, per-environment objective scale, and scalar rollout metadata. @@ -137,10 +143,10 @@ RolloutKind.DIFFERENTIABLE → DifferentiableTrainer → DifferentiableCollector → graph-connected DifferentiableRollout segments - → APG update + → APG or APGActorCritic update ``` -PPO and GRPO use `STANDARD`. APG uses `DIFFERENTIABLE`. +PPO and GRPO use `STANDARD`. APG and APGActorCritic use `DIFFERENTIABLE`. `get_trainer_class()` centralizes this selection for lightweight learning environments. @@ -156,12 +162,24 @@ 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. +`apg_actor_critic` adds terminal-value gradients, detached lambda-return critic +regression and a separate critic optimizer. Actor/critic parameter groups must +be disjoint. Time truncations require the full pre-reset observation batch in +`info["terminal_observation"]`; hard termination suppresses bootstrap. Optional +soft continuation retains its input derivative. Task-data validity masks are +detached. See the field contract in `DifferentiableEnvProtocol` and +`DifferentiableTransition`. Normalizer updates belong between optimizer updates. + +`DifferentiableTrainer.update_once()` provides the collection/update boundary +for online model learning, recording counters and training metrics. The outer +workflow owns evaluation and persistence when using this single-update API. + ## Component Ownership | Component | Owner | |-----------|-------| | Algorithm base, rollout kind, optimizer scheduling | `algo/base.py`, `utils/optimizer.py` | -| PPO, GRPO, APG implementations | `algo/ppo.py`, `algo/grpo.py`, `algo/apg.py` | +| Algorithm implementations | `algo/ppo.py`, `algo/grpo.py`, `algo/apg.py`, `algo/apg_actor_critic.py` | | Standard rollout storage and views | `buffer/` | | Standard and differentiable collection | `collector/` | | Policy interface, actor-critic, actor-only, policy builder | `learning/rl/policies/` | @@ -227,6 +245,16 @@ import from `embodichain.learning.models`. See [learning layers](../learning-lay for differentiable IK ownership. Root exports load on demand; importing a lightweight APG component must not eagerly load the simulator-backed Trainer. +APG Actor-Critic can call `policy.update_critic_normalization()` after actor +backward and before constructing detached lambda targets. Set +`update_critic_normalization=True` for that contract; the default leaves critic +statistics unchanged. Actor collection and backward keep critic parameters and +statistics fixed while preserving derivatives through critic inputs. Statistics +include weighted current states and valid bootstrap successors absent from the +next current-state row; complete-rollout post-terminal suffixes are excluded. +Critic minibatches contain only positive-weight samples. Empty valid sets leave +critic parameters and optimizer state unchanged, including Adam momentum. + ## Invariants - The selected environment must expose batched observation/action spaces and diff --git a/agent_context/topics/rl-learning/training.md b/agent_context/topics/rl-learning/training.md index be5db1c298..c0fa1119fb 100644 --- a/agent_context/topics/rl-learning/training.md +++ b/agent_context/topics/rl-learning/training.md @@ -131,12 +131,12 @@ example is an experimental gradient reference, not a general simulator task. ### Add a Lightweight Environment -1. Implement `LearningVecEnv`, or `DifferentiableVecEnv` for APG. +1. Implement `LearningEnvProtocol`, or `DifferentiableEnvProtocol` for APG. 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. For variable complete APG rollouts, implement - `ScheduledDifferentiableVecEnv` and return the full non-truncated horizon. + `ScheduledDifferentiableEnvProtocol` 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 diff --git a/agent_context/topics/robot-world-model/robot-world-model.md b/agent_context/topics/robot-world-model/robot-world-model.md index c40f355c9f..0743801beb 100644 --- a/agent_context/topics/robot-world-model/robot-world-model.md +++ b/agent_context/topics/robot-world-model/robot-world-model.md @@ -3,8 +3,9 @@ ## Ownership and entry points `embodichain_models.dynamics.rwm` is the canonical model package. Its `model.py` -owns fixed-period frame-local state increments; `robot_state.py` owns the -RWM state representation and SO(3) transforms. They have no simulation or example +owns body-frame state increments; `kinematic.py` owns `KinematicWorldModel`, +with heading-frame increments and full-body FK features. `robot_state.py` owns the +canonical state representation and SO(3) transforms. They have no simulation or example dependency. Shared MLPs/normalizers stay in core `learning.models`; optimizer updates, run artifacts and recovery stay in [SL](../supervised-learning/supervised-learning.md). @@ -16,11 +17,18 @@ through `embodichain run-example learning/rwm`; private support is not a model A The [RWM guide](../../../docs/source/features/neural_dynamics/rwm.md) owns the user-facing principles, tensor layout, prediction and replay examples. +`examples/learning/motion_tracking/fit_world_model.py` collects official G1 physical +transitions and fits the kinematic variant through the same `SupervisedLearner`. +Task/reference data remain outside the model package. + ## State, graph and data boundaries Canonical state is position, row-major body-to-world rotation, world linear and -angular velocity, then ordered joint positions and velocities. Increments use -the current body frame with right-multiplied exponential rotation coordinates. +angular velocity, then ordered joint positions and velocities. Both variants use +right-multiplied exponential rotation coordinates. The kinematic variant uses +heading axes for position/linear-velocity changes and each state's own body axes +for angular-velocity changes. Its input features share +`compute/kinematics/features.py` with tracking tasks. Fixed control period, action semantics and joint order enter model/checkpoint identity. Normalization changes only from explicitly selected measured training transitions and is frozen during prediction differentiation. @@ -29,12 +37,17 @@ The objective retains the full multi-step graph and excludes invalid suffix padding before model/error arithmetic. Predictor reset allocates replacements, preserving outstanding results and gradients through unselected rows. Freeze model parameters and normalization for action/state differentiation, and finish -dependent backward work before changing weights. +dependent backward work before changing weights. The objective supports explicit +feature weights and temporal mean/sum reduction; complete-window adaptation +rejects episode-crossing or unusable transitions before graph construction. `replay.py` owns fixed-byte-budget device storage. Append copies detached, owned physical transitions before reset. Windows/history respect episode -boundaries and ring overwrite; sampling returns owned tensors. Acquisition, -reset and optional replay refill remain caller-owned. +boundaries and ring overwrite; sampling returns owned tensors. Declared auxiliary +fields preserve task progress and controls within the allocation budget. Use +`history_complete` to exclude starts whose required history was overwritten; +real episode-start padding remains complete. Acquisition, reset and optional +replay refill remain caller-owned. ## Validation @@ -44,3 +57,96 @@ The controlled three-seed fitting protocol and its unchanged acceptance gate live in `examples/learning/rwm/README.md`; historical reports retain their recorded source/data hashes. These runs qualify fitting/interfaces, not robot policy performance. + +## Online motion learning + +`embodichain_models.policies.motion_tracking` owns the goal encoder/decoder actor +and scalar critic. The official task's `tracking/motion_model.py` composes shared +FK, reference queries and reward/continuation rules into a tensor-only environment. +Replay starts include clean history, previous actions, physical context and frame +progress. Clip suffixes use validity masks; a complete horizon retains gradients +until the trainer finishes backward, then `detach_state()` preserves values and +reference progress without drawing new noise. + +The G1/SuperMimic composition keeps `trainer / policy / algorithm` blocks and a task-owned +`workflow` block. `trainer.learning_env` resolves `MotionTrackingModelRL` through +`build_learning_policy_runtime(resources=...)`; `resources` shares the live WM, +motion library, task and Replay start provider without copying their state. +`MotionTrackingPolicy.from_config` owns network configuration and observation/action +space validation. Collection and evaluation use separate `workflow.collection_env` +and `workflow.evaluation_env` paths. Checkpoints retain the full recipe, and the +entry normalizes earlier flat recipe fields before compatibility checks. It validates +`MotionTrackingModelRL`, `motion_tracking` and `apg_actor_critic` selections before +physical allocation. Model/objective/replay construction and three-optimizer +recovery remain specific to this composition; provider factories belong with a +second model integration. + +`learning/dynamics/schedule.py::OnlineModelBasedSchedule` owns prefill/warmup/update counters and fractional +update credit. `examples/learning/motion_tracking/train.py` composes physical +collection, shared SL and `DifferentiableTrainer.update_once()`. It freezes the WM +around the full policy update. The recipe checkpoint includes all three networks, +optimizers, schedule, normalizers, RNGs and reference sampling statistics. Replay +is refilled from a newly reset physical environment after restoring learning state. +The task's `agents/model_based*.yaml` configs distinguish functional tests, +four-clip development, complete 40-clip convergence and frozen-WM policy tests. +The full recipe uses 60,000 collection iterations, no model-only warmup and +24,000 Actor / 240,000 WM / 480,000 Critic updates. Its default physical route is +`env.newton.yaml` with the `mujoco_warp` solver; `env.dexuni.yaml` retains the +DexUni alternative. `save_every: 0` retains final checkpoints only. +`--wm-checkpoint` initializes the fixed-model recipe from fitted or online WM +weights, while `--no-evaluate-final` separates successful training from evaluation. +The installed entry is `embodichain run-example learning/motion_tracking/train.py`. + +The full recipe enables `compile_mode: reduce-overhead` for the actor, complete +WM objective, tensor-only model transition and reference-window FK. Sampling and episode bookkeeping +remain outside compiled regions. `compute/kinematics/matrix.py` shares fused compiled FP32/FP64 small-matrix +products across FK, model integration and task features; eager calls retain Torch +matrix kernels. Tracking orientation error computes the squared shortest angle +directly from rotation trace and antisymmetric entries. The model environment has +no auto-reset and supplies next observations without a redundant terminal copy. Actor compilation +uses `nn.Module.compile`, preserving parameter ownership and checkpoint keys. +Physical collection starts its own CUDA graph step before copying measured data +into Replay. Partial-reset features use dynamic compilation without CUDA graph +capture; full-batch task analytics retain the selected compilation mode. +The motion recipe uses fused Adam on CUDA for WM, Actor and Critic, with the +existing CPU Adam implementation on CPU. +The differentiable trainer releases consumed rollout snapshots after gradient +accumulation and normalization updates, before the next collection or Critic fit. +`episode_context_in_info` controls JSON provenance output independently of episode +validity checks. Both options are runtime settings outside checkpoint identity. + +`evaluate_online.py` loads a checkpoint in a separate physical process and runs +all selected clips from their first frame, with deterministic actions, no observation +or reset noise and a full-clip horizon. Optional trajectories contain measured successor state, the matching reference, +active masks, joint names and tracked-link names for visualization. Per-clip failures and normalized full-clip return measure +convergence. The [online recipe guide](../../../examples/learning/motion_tracking/README.md) +owns commands and output semantics. `latest.pt` retains complete learning state; +`policy-NNNNNN.pt` retains evaluation weights at configured save/evaluation intervals. + +Quality evaluation defaults to `--mode quality`; `--mode physics-regression` +requests Warp run-to-run determinism. `--max-steps` bounds a functional check and +marks the result as a partial-horizon evaluation. Evaluation records its own +hardware, software, precision and data provenance. + +`evaluate_world_model.py` loads a frozen online checkpoint and collects fresh +physical episodes under policy or reference-PD control. It compares open-loop +predictions and a state-hold diagnostic at 1/8/16/32 steps, respecting episode +boundaries and retaining model/normalizer values. State, reward and termination +errors measure each prediction endpoint; the supervised training loss aggregates +weighted errors over its fitted window. Training results also expose actual Adam +step ranges and per-invocation first-call durations, including lazy initialization. + +The motion example's `prepare_lafan1.py` downloads the pinned G1 retargeting +source and preserves original-data, retargeting-project and download-mirror +notices. `download_pretrained.py` resolves a model revision once and downloads +the G1 inference bundle from `DexForceAI/embodichain_model`; the record stores +the immutable revision. Evaluation loads it with `weights_only=True`. +`replay.py` consumes the self-contained evaluation NPZ and uses public +`RobotMarkers` and `SimulationManager.sync_render_state()` for FastRT playback +without physics steps. The visualization interfaces are supplied by #833. + +ODS owns production/storage/supply pacing; the model-based schedule owns learning +order and update credit, and device replay owns measured episode sequences. The +[G1 workflow guide](../../../examples/learning/motion_tracking/README.md#replay-and-ods-ownership) +defines padding, masks, controls and future source-adapter boundaries. Connecting +ODS must preserve one collection trigger and keep autograd with learner callbacks. diff --git a/agent_context/topics/simulation-system/simulation-system.md b/agent_context/topics/simulation-system/simulation-system.md index 92606297e4..0353010249 100644 --- a/agent_context/topics/simulation-system/simulation-system.md +++ b/agent_context/topics/simulation-system/simulation-system.md @@ -129,6 +129,13 @@ formats such as visualization `*wxyz`, cuRobo poses and GenSim `rotation_quaternion_wxyz` must be converted before EmbodiChain math. Adapter tests need non-symmetric rotations to expose component-order mistakes. +Spawn-bound Newton articulations write selected COM poses through +`SceneArticulationView.apply_link_com_pose`, which uses the SDK batch property +setter. This retains descriptor edits while refreshing the solver once per +batch; per-link scalar writes cause repeated model refreshes during large +domain-randomization resets. The batch preserves current principal moments +when the requested COM orientation changes. + ## Invariants - Configure arenas, devices, renderer and backend before manager construction. diff --git a/docs/source/api_reference/embodichain/embodichain.compute.rst b/docs/source/api_reference/embodichain/embodichain.compute.rst index 502d7c8523..22cfaa1cda 100644 --- a/docs/source/api_reference/embodichain/embodichain.compute.rst +++ b/docs/source/api_reference/embodichain/embodichain.compute.rst @@ -50,6 +50,7 @@ stationary paths and repeated endpoints remain valid inputs. retime_joint_paths PositionRangeMonitor + resample_floating_trajectory differentiate_positions resample_in_time interpolate_with_distance @@ -234,3 +235,39 @@ measurements leave task acceptance and support thresholds to callers. .. autofunction:: planar_placement_metrics .. autofunction:: validate_planar_pose +Floating-root motion computation +-------------------------------- + +``TreeKinematics`` evaluates a URDF tree from an explicit joint order and world +root-origin state. It supports fixed, revolute, continuous and prismatic joints, +batches arbitrary leading tensor axes and retains input derivatives. Outputs +include link-origin velocities; simulator center-of-mass velocity conversion +belongs to the calling environment. + +.. currentmodule:: embodichain.compute.kinematics + +.. autosummary:: + + TreeKinematics + LinkKinematics + heading_rotation + heading_kinematic_features + +.. automodule:: embodichain.compute.kinematics.tree + :members: + +``resample_floating_trajectory`` places a root/joint clip on a fixed frequency +grid within its source duration. Root quaternions use XYZW shortest-arc SLERP; +translation and joints use linear interpolation. Returned velocities use +centered differences and one-sided endpoints, with angular velocity in world +coordinates. + +.. automodule:: embodichain.compute.trajectory.floating + :members: + +``heading_kinematic_features`` is shared by the full-body tracking task and +kinematic world model. It combines root height/orientation, local velocities, +joints and link-origin positions/velocities using an explicit heading convention. + +.. automodule:: embodichain.compute.kinematics.features + :members: diff --git a/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst b/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst index 8b777dec91..3ae2358b3b 100644 --- a/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst +++ b/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst @@ -117,3 +117,27 @@ diagnostic reports, quality evidence and split-manifest consumption. .. automodule:: embodichain.data_pipeline.datasets.inspection :members: + +Reference motion clips +---------------------- + +``MotionLibrary`` stores floating-root reference clips on one device. It loads +explicit joint and quaternion conventions, resamples headerless CSV inputs and +writes portable NPZ archives. Current and future queries return owned tensors; +future offsets hold the endpoints of each clip instead of crossing boundaries. + +.. automodule:: embodichain.data_pipeline.datasets.motion + :members: + :no-index: + +Adaptive motion starts +---------------------- + +``MotionStartSampler`` samples clip-local time bins from uniform, current +low-return and historical low-return distributions. Simultaneous returns in one +bin are averaged before updating its score. Checkpoints retain motion identity, +visits, return estimates and historical weights; callers own the sampling RNG. + +.. automodule:: embodichain.data_pipeline.datasets.motion_sampling + :members: + :no-index: diff --git a/docs/source/api_reference/embodichain/embodichain.data_pipeline.rst b/docs/source/api_reference/embodichain/embodichain.data_pipeline.rst index bfe2e2bf72..260e808089 100644 --- a/docs/source/api_reference/embodichain/embodichain.data_pipeline.rst +++ b/docs/source/api_reference/embodichain/embodichain.data_pipeline.rst @@ -29,6 +29,7 @@ Datasets .. automodule:: embodichain.data_pipeline.datasets :members: + :exclude-members: MotionLibrary, MotionStartSampler, MotionStartSamplerCfg :undoc-members: :show-inheritance: diff --git a/docs/source/api_reference/embodichain/embodichain.learning.models.rst b/docs/source/api_reference/embodichain/embodichain.learning.models.rst index 00f2ac0c3d..709ab9ba3c 100644 --- a/docs/source/api_reference/embodichain/embodichain.learning.models.rst +++ b/docs/source/api_reference/embodichain/embodichain.learning.models.rst @@ -9,6 +9,9 @@ General neural networks and feature normalization These components support supervised learning and RL without importing RL, TensorDict or simulation. ``build_mlp_from_cfg`` constructs an MLP from a network configuration; ``EmpiricalNormalizer`` stores feature moments as module buffers. +Its default scale is standard deviation plus epsilon. ``variance_epsilon=True`` +selects the square root of variance plus epsilon squared, and ``clip`` optionally +bounds normalized features. The ``scale`` property also supports output decoding. RL policy interfaces and construction belong to :doc:`embodichain.learning.rl.policies`. 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 8410924e82..123598165c 100644 --- a/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst +++ b/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst @@ -8,7 +8,7 @@ Overview Algorithm registry and algorithm-construction helpers for RL training. The on-policy algorithms :class:`PPO` and :class:`GRPO` and the differentiable -algorithm :class:`APG` all derive from :class:`BaseAlgorithm`. Each algorithm +algorithms :class:`APG` and :class:`APGActorCritic` all derive from :class:`BaseAlgorithm`. Each algorithm declares a :class:`RolloutKind` that selects the compatible trainer. :func:`build_algo` looks up a registered algorithm by name and wires it to a policy, while :func:`compute_gae` provides generalized advantage estimation. @@ -21,6 +21,8 @@ policy, while :func:`compute_gae` provides generalized advantage estimation. RolloutKind APGCfg APG + APGActorCriticCfg + APGActorCritic PPOCfg PPO GRPOCfg @@ -33,6 +35,7 @@ policy, while :func:`compute_gae` provides generalized advantage estimation. build_algo get_registered_algo_names compute_gae + differentiable_lambda_returns complete_discounted_return segmented_discounted_return @@ -40,3 +43,28 @@ policy, while :func:`compute_gae` provides generalized advantage estimation. :members: :undoc-members: :show-inheritance: + +APG Actor-Critic environment inputs +------------------------------------ + +``apg_actor_critic`` uses the same differentiable trainer as ``apg``. Its policy +exposes disjoint actor and critic parameter groups and a scalar ``get_value``. +The collector selects the configured actor/critic observation groups. An +auto-resetting environment returns a full, graph-preserving successor batch +in ``info["terminal_observation"]`` for value bootstrap at time truncations. +Hard termination suppresses bootstrap while retaining the final reward. + +Optional per-environment ``continuation``, ``valid`` and ``next_valid`` tensors +represent soft continuation and task-data availability. The actor differentiates +through continuation and terminal-value inputs, with critic parameters and +statistics fixed. Critic regression detaches observations, lambda targets and +soft-survival weights. With ``update_critic_normalization=True``, the algorithm +updates the policy's critic normalizer after actor backpropagation and then +recomputes lambda targets under the new statistics. The default leaves statistics +unchanged. + +``DifferentiableTrainer.update_once()`` collects and optimizes one update, +maintains counters and records metrics. Online model-learning workflows can +call it between supervised updates. ``train()`` also manages periodic evaluation +and checkpoints. The PointMass configuration is +``extensions/embodichain_tasks/configs/tasks/classic_control/point_mass/agents/apg_actor_critic.yaml``. diff --git a/docs/source/api_reference/embodichain/embodichain.learning.rl.rst b/docs/source/api_reference/embodichain/embodichain.learning.rl.rst index b9ed35155a..76ef001140 100644 --- a/docs/source/api_reference/embodichain/embodichain.learning.rl.rst +++ b/docs/source/api_reference/embodichain/embodichain.learning.rl.rst @@ -29,9 +29,9 @@ collection logic, policy/model builders, and training entry points. DifferentiableTrainer DifferentiableTrainerCfg DifferentiableRolloutSpec - DifferentiableVecEnv - LearningVecEnv - ScheduledDifferentiableVecEnv + DifferentiableEnvProtocol + LearningEnvProtocol + ScheduledDifferentiableEnvProtocol RunningObservationNormalizer BatchedGradientNormStats build_learning_env @@ -45,6 +45,7 @@ Algorithms ---------- .. automodule:: embodichain.learning.rl.algo + :no-index: :members: :undoc-members: :show-inheritance: @@ -52,6 +53,19 @@ Algorithms Environments ------------ +The protocols describe batched inputs and outputs through ``num_envs`` and the +single-row spaces. ``LearningVecEnv``, ``DifferentiableVecEnv`` and +``ScheduledDifferentiableVecEnv`` remain aliases of the corresponding +``*EnvProtocol`` classes for existing consumers. + +.. currentmodule:: embodichain.learning.rl + +.. autosummary:: + + LearningVecEnv + DifferentiableVecEnv + ScheduledDifferentiableVecEnv + .. automodule:: embodichain.learning.rl.env :members: :undoc-members: diff --git a/docs/source/api_reference/embodichain/embodichain.learning.rst b/docs/source/api_reference/embodichain/embodichain.learning.rst index 09479ec0f6..b7d55a30ca 100644 --- a/docs/source/api_reference/embodichain/embodichain.learning.rst +++ b/docs/source/api_reference/embodichain/embodichain.learning.rst @@ -10,6 +10,7 @@ embodichain.learning models rl sl + dynamics Reinforcement Learning ---------------------- @@ -93,3 +94,21 @@ optimizer updates and committed-update recovery through ``learning.sl``. :maxdepth: 1 embodichain.learning.sl + +Online dynamics and policy learning +----------------------------------- + +``OnlineModelBasedSchedule`` separates physical prefill, model-only warmup and +fractional online update credit. Model callbacks finish before a policy callback +collects and differentiates its trajectory. The schedule checkpoints counters; +its caller checkpoints models, optimizers, random streams and sampling state. + +.. currentmodule:: embodichain.learning.dynamics + +.. autosummary:: + + OnlineModelBasedScheduleCfg + OnlineModelBasedSchedule + +.. automodule:: embodichain.learning.dynamics.schedule + :members: diff --git a/docs/source/api_reference/index.rst b/docs/source/api_reference/index.rst index c18fe744e4..3a79d391c4 100644 --- a/docs/source/api_reference/index.rst +++ b/docs/source/api_reference/index.rst @@ -104,3 +104,9 @@ documentation. CI runs this same checker after style checks and before tests. :caption: Locomotion robots locomotion_robots + + +.. toctree:: + :maxdepth: 1 + + kinematics_matrix diff --git a/docs/source/api_reference/kinematics_matrix.rst b/docs/source/api_reference/kinematics_matrix.rst new file mode 100644 index 0000000000..c58d1d58a7 --- /dev/null +++ b/docs/source/api_reference/kinematics_matrix.rst @@ -0,0 +1,30 @@ +Matrix products for kinematics +============================== + +The shared matrix helpers preserve Torch broadcasting and differentiation. +Under ``torch.compile``, FP32 and FP64 products with trailing 3-by-3 matrices +use pointwise reductions that can fuse with surrounding geometry. Eager +execution, other sizes and dtypes, and autocast regions use Torch matrix +multiplication. The dynamics model, motion-tracking task and tree kinematics +share these operations. + +Package imports +--------------- + +.. currentmodule:: embodichain.compute.kinematics + +.. autosummary:: + + matrix_product + matrix_vector_product + +.. autofunction:: matrix_product +.. autofunction:: matrix_vector_product + +Implementation module +--------------------- + +.. currentmodule:: embodichain.compute.kinematics.matrix + +.. autofunction:: matrix_product +.. autofunction:: matrix_vector_product diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index c54ffa059a..56b507ee8b 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -4113,6 +4113,21 @@ embodichain.learning.rl.algo.apg complete_discounted_return segmented_discounted_return +embodichain.learning.rl.algo.apg_actor_critic +------------------------------------------- + +.. currentmodule:: embodichain.learning.rl.algo.apg_actor_critic + +Short differentiable trajectories use terminal values for actor optimization +and detached lambda returns for critic regression. Actor and critic optimizers +retain separate state in checkpoints. + +.. autosummary:: + + APGActorCritic + APGActorCriticCfg + differentiable_lambda_returns + embodichain.learning.rl.algo.base --------------------------------- @@ -4467,6 +4482,25 @@ embodichain_tasks.configs get_config_path +embodichain_tasks.locomotion.managers.actions +---------------------------------------------- + +The reference position action combines a policy output with the next motion +frame using the task's per-joint reference weights, then applies encoder bias. + +.. automodule:: embodichain_tasks.locomotion.managers.actions + :members: + +embodichain_tasks.locomotion.tracking.g1_motion +----------------------------------------------- + +The G1 motion task connects reference data and differentiable tracking +calculations to the simulator Gym loop. It owns frame progress, history, +reference-state resets and terminal snapshots before automatic row resets. + +.. automodule:: embodichain_tasks.locomotion.tracking.g1_motion + :members: + embodichain_tasks.manipulation.push_cube ---------------------------------------- diff --git a/docs/source/api_reference/rwm.rst b/docs/source/api_reference/rwm.rst index 2241a67e1f..328b61b83f 100644 --- a/docs/source/api_reference/rwm.rst +++ b/docs/source/api_reference/rwm.rst @@ -19,6 +19,8 @@ initialize simulation or import example helpers. .. autosummary:: + KinematicWorldModelCfg + KinematicWorldModel RobotWorldModelCfg RobotWorldModel frozen_world_model @@ -28,6 +30,10 @@ initialize simulation or import example helpers. TransitionReplay build_rwm_trainer +.. autoclass:: KinematicWorldModelCfg + +.. autoclass:: KinematicWorldModel + .. autoclass:: RobotWorldModelCfg .. autoclass:: RobotWorldModel @@ -57,6 +63,8 @@ away from its pi branch cut are supported. .. autosummary:: + pack_robot_state + unpack_robot_state rotation_exp rotation_log robot_state_features @@ -64,6 +72,10 @@ away from its pi branch cut are supported. integrate_robot_state robot_state_error +.. autofunction:: pack_robot_state + +.. autofunction:: unpack_robot_state + .. autofunction:: rotation_exp .. autofunction:: rotation_log @@ -101,6 +113,42 @@ state/action gradients through dependent rollout and backward work. .. autofunction:: frozen_world_model +Kinematic-feature model +----------------------- + +``KinematicWorldModel`` combines the shared MLP and normalizer with full-body FK +features. For a 29-joint G1 it uses 245 state features, 29 executed position +setpoints and 33 recorded physical parameters. Its default three 2048-wide SiLU +layers contain 9,166,918 parameters. Input normalization uses +``sqrt(variance + epsilon**2)`` and clips at ten; output increments use the same +scale convention with epsilon 1e-4. FK, normalization and integration remain FP32. + +This variant uses heading axes for displacement and linear-velocity changes. +Angular-velocity increments compare each state's own body axes. Root rotation +uses the same right-multiplied exponential coordinates as the body-frame model. +Both models accept the canonical matrix-based state and preserve state/action +input gradients when frozen. Geometry XML and the complete model configuration +are included in checkpoint identity. + +.. currentmodule:: embodichain_models.dynamics.rwm.kinematic + +.. autosummary:: + + KinematicWorldModelCfg + KinematicWorldModel + heading_state_increment + integrate_heading_state + +.. autoclass:: KinematicWorldModelCfg + :members: + +.. autoclass:: KinematicWorldModel + :members: + +.. autofunction:: heading_state_increment + +.. autofunction:: integrate_heading_state + Supervision and prediction lifecycle ------------------------------------ @@ -108,7 +156,11 @@ The objective builds an untruncated autoregressive graph. Invalid prefix-mask suffixes are excluded before differentiable arithmetic, including nonfinite padding. The predictor returns newly allocated states; selective reset preserves unselected rows and outstanding graphs. Callers explicitly detach prediction -state when beginning a new graph. +state when beginning a new graph. The objective accepts feature weights and +``temporal_reduction="sum"`` for a time-summed window loss. With +``error_floor=None`` it uses the model's increment scale directly. Complete +window adaptation excludes episode-crossing windows and optional false +``usable`` transition flags before constructing the prediction graph. .. currentmodule:: embodichain_models.dynamics.rwm.supervised @@ -138,6 +190,11 @@ Append copies detached physical transitions before environment reset. Sampled windows and oldest-first histories retain episode boundaries and survive ring overwrite. The fixed allocation can be checked against a byte ceiling. Replay snapshots retain their sampling RNG; acquisition and refill remain caller-owned. +Declared auxiliary scalar/vector fields retain reference progress, policy controls +or transition usability with the same ownership and byte budget. Start samples +include ``history_complete``: reset padding is complete, while missing rows lost +to ring overwrite are not. Version-1 snapshots remain loadable for layouts with +no auxiliary fields. .. currentmodule:: embodichain_models.dynamics.rwm.replay @@ -165,3 +222,24 @@ example runs through ``embodichain run-example learning/rwm``. build_rwm_trainer .. autofunction:: build_rwm_trainer + +Motion policy and model environment +------------------------------------ + +The motion actor concatenates proprioceptive history with an encoded reference +goal. Its scalar critic consumes separate privileged state features. Actor and +critic normalizers are updated explicitly at their respective learning phases. + +.. automodule:: embodichain_models.policies.motion_tracking + :members: + +The tensor-only motion environment starts from measured replay states, clean +history and reference progress. It preserves action/state derivatives across a +fixed horizon and reports invalid clip suffixes through validity masks. The +workflow freezes model parameters until policy backpropagation completes. + +.. automodule:: embodichain_tasks.locomotion.tracking.motion_model + :members: + +.. automodule:: embodichain_tasks.locomotion.managers.events + :members: diff --git a/embodichain/compute/kinematics/__init__.py b/embodichain/compute/kinematics/__init__.py index e2150cd522..62e06a667a 100644 --- a/embodichain/compute/kinematics/__init__.py +++ b/embodichain/compute/kinematics/__init__.py @@ -34,9 +34,18 @@ from .dls import damped_least_squares_step, solve_dls_ik from .pose_error import pose_error from .serial_chain import SerialChainKinematics +from .features import heading_rotation, heading_kinematic_features +from .matrix import matrix_product, matrix_vector_product +from .tree import TreeKinematics, LinkKinematics from .types import DifferentiableIKCfg, DifferentiableIKResult __all__ = [ + "matrix_product", + "matrix_vector_product", + "heading_rotation", + "heading_kinematic_features", + "TreeKinematics", + "LinkKinematics", "damped_least_squares_step", "solve_dls_ik", "pose_error", diff --git a/embodichain/compute/kinematics/features.py b/embodichain/compute/kinematics/features.py new file mode 100644 index 0000000000..2eccbe7ebd --- /dev/null +++ b/embodichain/compute/kinematics/features.py @@ -0,0 +1,91 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Heading-frame features shared by dynamics models and tracking tasks.""" + +from __future__ import annotations + +import torch + +from .matrix import matrix_product, matrix_vector_product + +__all__ = ["heading_rotation", "heading_kinematic_features"] + + +def heading_rotation(rotation: torch.Tensor) -> torch.Tensor: + """Extract Z-up heading from the horizontal projection of the root X axis. + + Args: + rotation: Body-to-world matrices with arbitrary leading batch axes. + + Returns: + Yaw rotation matrices. Heading is undefined when the root X axis is + vertical; callers use this representation away from that singularity. + """ + yaw = torch.atan2(rotation[..., 1, 0], rotation[..., 0, 0]) + c, s = yaw.cos(), yaw.sin() + z, o = torch.zeros_like(c), torch.ones_like(c) + heading = torch.stack((c, -s, z, s, c, z, z, z, o), -1) + return heading.reshape(*yaw.shape, 3, 3) + + +def heading_kinematic_features( + position: torch.Tensor, + rotation: torch.Tensor, + linear_velocity: torch.Tensor, + angular_velocity: torch.Tensor, + joint_position: torch.Tensor, + joint_velocity: torch.Tensor, + resting_position: torch.Tensor, + link_position: torch.Tensor, + link_linear_velocity: torch.Tensor, +) -> torch.Tensor: + """Encode root/joint state and ordered link origins in the root heading frame. + + Args: + position: World root-origin positions, ending in 3. + rotation: Body-to-world root rotations, ending in (3, 3). + linear_velocity: World root-origin linear velocities. + angular_velocity: World root angular velocities. + joint_position: Ordered joint positions, ending in J. + joint_velocity: Ordered joint velocities. + resting_position: Joint offsets, ending in J. + link_position: World positions, ending in (L, 3), usually excluding root. + link_linear_velocity: World link-origin velocities, ending in (L, 3). + + Returns: + Features of width ``13 + 2*J + 6*L``: height, heading-relative orientation + (first two columns), heading linear velocity, body angular velocity, + centered joints, joint velocities, relative link positions and velocities. + """ + heading = heading_rotation(rotation) + local = lambda r, v: matrix_vector_product(r.transpose(-1, -2), v) + orientation = matrix_product(heading.transpose(-1, -2), rotation) + return torch.cat( + ( + position[..., 2:3], + orientation[..., :, :2].transpose(-1, -2).flatten(-2), + local(heading, linear_velocity), + local(rotation, angular_velocity), + joint_position - resting_position, + joint_velocity, + local( + heading.unsqueeze(-3), link_position - position.unsqueeze(-2) + ).flatten(-2), + local(heading.unsqueeze(-3), link_linear_velocity).flatten(-2), + ), + -1, + ) diff --git a/embodichain/compute/kinematics/matrix.py b/embodichain/compute/kinematics/matrix.py new file mode 100644 index 0000000000..f4ba6fff8d --- /dev/null +++ b/embodichain/compute/kinematics/matrix.py @@ -0,0 +1,71 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Batched matrix products with fused small-matrix evaluation under compilation.""" + +from __future__ import annotations + +import torch + +__all__ = ["matrix_product", "matrix_vector_product"] + + +def matrix_product(left: torch.Tensor, right: torch.Tensor) -> torch.Tensor: + """Multiply matrices with broadcast batch axes and preserve gradients. + + Args: + left: Matrices with shape ``(..., M, K)``. + right: Matrices with shape ``(..., K, N)`` and broadcastable batch axes. + + Returns: + Matrix products with shape ``(..., M, N)``. Compiled FP32/FP64 3-by-3 products + use pointwise reductions that fuse with adjacent geometry. Other + sizes, eager calls and autocast regions use Torch matrix multiplication. + """ + if ( + torch.compiler.is_compiling() + and left.shape[-2:] == (3, 3) + and right.shape[-2:] == (3, 3) + and left.dtype == right.dtype + and left.dtype in (torch.float32, torch.float64) + and not torch.is_autocast_enabled(left.device.type) + ): + return (left.unsqueeze(-1) * right.unsqueeze(-3)).sum(-2) + return left @ right + + +def matrix_vector_product(matrix: torch.Tensor, vector: torch.Tensor) -> torch.Tensor: + """Apply matrices to vectors with broadcast batch axes and preserve gradients. + + Args: + matrix: Matrices with shape ``(..., M, K)``. + vector: Vectors with shape ``(..., K)`` and broadcastable batch axes. + + Returns: + Vectors with shape ``(..., M)``. Compiled FP32/FP64 3-by-3 products fuse their + pointwise reduction; other sizes, eager calls and autocast regions + use Torch matrix multiplication. + """ + if ( + torch.compiler.is_compiling() + and matrix.shape[-2:] == (3, 3) + and vector.shape[-1] == 3 + and matrix.dtype == vector.dtype + and matrix.dtype in (torch.float32, torch.float64) + and not torch.is_autocast_enabled(matrix.device.type) + ): + return (matrix * vector.unsqueeze(-2)).sum(-1) + return (matrix @ vector.unsqueeze(-1)).squeeze(-1) diff --git a/embodichain/compute/kinematics/tree.py b/embodichain/compute/kinematics/tree.py new file mode 100644 index 0000000000..8c729b8220 --- /dev/null +++ b/embodichain/compute/kinematics/tree.py @@ -0,0 +1,285 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Floating-base tree kinematics with differentiable link-origin velocities.""" + +from __future__ import annotations + +from collections.abc import Sequence +from dataclasses import dataclass +from xml.etree import ElementTree as ET + +import torch + +from embodichain.utils.math import matrix_from_euler +from .matrix import matrix_product, matrix_vector_product + +__all__ = ["TreeKinematics", "LinkKinematics"] + + +@dataclass(frozen=True) +class LinkKinematics: + """World-frame link origins, rotations and velocities with leading batch axes.""" + + position: torch.Tensor + rotation: torch.Tensor + linear_velocity: torch.Tensor + angular_velocity: torch.Tensor + + +class TreeKinematics: + """Evaluate a URDF tree independently of a simulator. + + Fixed, revolute, continuous and prismatic joints are supported. Joint input + order is explicit; output links follow a parent-before-child traversal. + Construction parses geometry once. Evaluation batches links by tree depth + and preserves derivatives through root state, joint position and velocity. + + Args: + urdf: URDF XML text. + joint_names: Ordered names of all independent movable joints. + device: Device for immutable geometry tensors. + dtype: Floating point precision shared with evaluation inputs. + """ + + def __init__( + self, + urdf: str, + joint_names: Sequence[str], + *, + device: str | torch.device = "cpu", + dtype: torch.dtype = torch.float32, + ) -> None: + if dtype not in (torch.float32, torch.float64): + raise ValueError("Tree kinematics supports float32 or float64") + model = ET.fromstring(urdf) + self.urdf = urdf + self.joint_names = tuple(joint_names) + if not self.joint_names or len(set(self.joint_names)) != len(self.joint_names): + raise ValueError("Joint names must be unique") + self.device = torch.empty(0, device=device).device + self.dtype = dtype + joints = model.findall("joint") + links = [link.attrib["name"] for link in model.findall("link")] + children = {joint.find("child").attrib["link"] for joint in joints} + roots = set(links) - children + if len(roots) != 1: + raise ValueError("URDF must contain one rooted tree") + movable = [j.attrib["name"] for j in joints if j.attrib["type"] != "fixed"] + if set(movable) != set(self.joint_names): + raise ValueError( + "Joint order must name every movable URDF joint exactly once" + ) + by_parent: dict[str, list[ET.Element]] = {} + for joint in joints: + if ( + joint.attrib["type"] + not in ("fixed", "revolute", "continuous", "prismatic") + or joint.find("mimic") is not None + ): + raise ValueError("Tree kinematics requires independent scalar joints") + by_parent.setdefault(joint.find("parent").attrib["link"], []).append(joint) + names = [next(iter(roots))] + parent, depth, joint_ids = [0], [0], [len(self.joint_names)] + positions, angles, axes, slides = [[0.0] * 3], [[0.0] * 3], [[0.0] * 3], [0.0] + limits: dict[str, tuple[float, float]] = {} + + def vector(node: ET.Element | None, key: str) -> list[float]: + return [ + float(x) + for x in ( + node.get(key, "0 0 0") if node is not None else "0 0 0" + ).split() + ] + + def visit(name: str, index: int) -> None: + for joint in by_parent.get(name, ()): + child = joint.find("child").attrib["link"] + if child in names: + raise ValueError("URDF has a cycle or repeated child link") + names.append(child) + child_id = len(names) - 1 + kind = joint.attrib["type"] + parent.append(index) + depth.append(depth[index] + 1) + joint_ids.append( + len(self.joint_names) + if kind == "fixed" + else self.joint_names.index(joint.attrib["name"]) + ) + origin = joint.find("origin") + positions.append(vector(origin, "xyz")) + angles.append(vector(origin, "rpy")) + axis = ( + vector(joint.find("axis"), "xyz") + if joint.find("axis") is not None + else [1.0, 0.0, 0.0] + ) + if kind == "fixed": + axis = [0.0] * 3 + axes.append(axis) + slides.append(float(kind == "prismatic")) + if kind != "fixed": + limit = joint.find("limit") + limits[joint.attrib["name"]] = ( + (float("-inf"), float("inf")) + if kind == "continuous" + else ( + float(limit.attrib["lower"]), + float(limit.attrib["upper"]), + ) + ) + visit(child, child_id) + + visit(names[0], 0) + if set(names) != set(links): + raise ValueError("URDF contains disconnected links") + self.link_names = tuple(names) + self.moving_link_names = tuple( + [names[0]] + + [ + names[i] + for i, j in enumerate(joint_ids[1:], 1) + if j < len(self.joint_names) + ] + ) + self.position_limits = torch.tensor( + [limits[n] for n in self.joint_names], device=self.device, dtype=dtype + ) + self._positions = torch.tensor(positions, device=self.device, dtype=dtype) + # URDF fixed-axis roll/pitch/yaw equals intrinsic ZYX with reversed angles. + self._rotations = matrix_from_euler( + torch.tensor(angles, device=self.device, dtype=dtype).flip(-1), "ZYX" + ) + axes_tensor = torch.tensor(axes, device=self.device, dtype=dtype) + norms = axes_tensor.norm(dim=-1, keepdim=True) + if any( + j < len(self.joint_names) and float(norms[i]) == 0 + for i, j in enumerate(joint_ids) + ): + raise ValueError("Movable joint axes must be nonzero") + self._axes = axes_tensor / norms.clamp_min(1e-12) + self._slides = torch.tensor(slides, device=self.device, dtype=dtype) + self._joint_ids = torch.tensor(joint_ids, device=self.device, dtype=torch.long) + self._parent_axes = (self._rotations @ self._axes.unsqueeze(-1)).squeeze(-1) + x, y, z = self._axes.unbind(-1) + zero = torch.zeros_like(x) + self._axis_skew = torch.stack( + (zero, -z, y, z, zero, -x, -y, x, zero), -1 + ).reshape(-1, 3, 3) + self._axis_skew_squared = self._axis_skew @ self._axis_skew + self._identity = torch.eye(3, device=self.device, dtype=dtype) + self._levels = [] + previous_level = [0] + level_order = [0] + for level in range(1, max(depth) + 1): + selected = [i for i, d in enumerate(depth) if d == level] + self._levels.append( + tuple( + torch.tensor(a, device=self.device, dtype=torch.long) + for a in ( + selected, + [previous_level.index(parent[i]) for i in selected], + [joint_ids[i] for i in selected], + ) + ) + ) + level_order.extend(selected) + previous_level = selected + self._link_order = torch.tensor( + [level_order.index(i) for i in range(len(names))], device=self.device + ) + + def link_indices(self, names: Sequence[str]) -> torch.Tensor: + """Return device indices in the requested link order.""" + return torch.tensor( + [self.link_names.index(n) for n in names], + device=self.device, + dtype=torch.long, + ) + + def evaluate( + self, + position: torch.Tensor, + rotation: torch.Tensor, + qpos: torch.Tensor, + linear_velocity: torch.Tensor, + angular_velocity: torch.Tensor, + qvel: torch.Tensor, + ) -> LinkKinematics: + """Return world link states for ``(..., dof)`` joint and root inputs. + + Linear velocities refer to link origins, including the floating root. + Returned tensors own their storage and retain the input computation graph. + """ + batch = qpos.shape[:-1] + if ( + qpos.shape[-1] != len(self.joint_names) + or qvel.shape != qpos.shape + or position.shape != (*batch, 3) + or rotation.shape != (*batch, 3, 3) + or linear_velocity.shape != position.shape + or angular_velocity.shape != position.shape + ): + raise ValueError("Inconsistent root/joint batch shapes") + values = (position, rotation, qpos, linear_velocity, angular_velocity, qvel) + if any(x.device != self.device or x.dtype != self.dtype for x in values): + raise ValueError("Kinematics inputs must share model device and dtype") + # The final column supplies zero motion to fixed joints. + q = torch.cat((qpos, torch.zeros_like(qpos[..., :1])), -1) + dq = torch.cat((qvel, torch.zeros_like(qvel[..., :1])), -1) + # Axes are constant and already normalized. Evaluate joint rotations + # together; only the parent-to-child propagation depends on tree depth. + angles = q.index_select(-1, self._joint_ids) * (1 - self._slides) + local_rotation = ( + self._identity + + angles.sin()[..., None, None] * self._axis_skew + + (2 * (angles * 0.5).sin().square())[..., None, None] + * self._axis_skew_squared + ) + relative_rotation = matrix_product(self._rotations, local_rotation) + p, r = [position.unsqueeze(-2)], [rotation.unsqueeze(-3)] + v, w = [linear_velocity.unsqueeze(-2)], [angular_velocity.unsqueeze(-2)] + for children, parents, joints in self._levels: + parent_r = r[-1].index_select(-3, parents) + slide = self._slides[children] + coordinate = q.index_select(-1, joints) + velocity = dq.index_select(-1, joints) + axis_world = matrix_vector_product(parent_r, self._parent_axes[children]) + translation = matrix_vector_product( + parent_r, self._positions[children] + ) + axis_world * (coordinate * slide).unsqueeze(-1) + child_p = p[-1].index_select(-2, parents) + translation + child_r = matrix_product( + parent_r, relative_rotation.index_select(-3, children) + ) + parent_w = w[-1].index_select(-2, parents) + child_v = ( + v[-1].index_select(-2, parents) + + torch.cross(parent_w, translation, dim=-1) + + axis_world * (velocity * slide).unsqueeze(-1) + ) + child_w = parent_w + axis_world * (velocity * (1 - slide)).unsqueeze(-1) + p.append(child_p) + r.append(child_r) + v.append(child_v) + w.append(child_w) + return LinkKinematics( + torch.cat(p, -2).index_select(-2, self._link_order), + torch.cat(r, -3).index_select(-3, self._link_order), + torch.cat(v, -2).index_select(-2, self._link_order), + torch.cat(w, -2).index_select(-2, self._link_order), + ) diff --git a/embodichain/compute/trajectory/__init__.py b/embodichain/compute/trajectory/__init__.py index 823d599d9a..09f22c6301 100644 --- a/embodichain/compute/trajectory/__init__.py +++ b/embodichain/compute/trajectory/__init__.py @@ -23,6 +23,7 @@ from .toppra import retime_joint_paths from .stability import PositionRangeMonitor +from .floating import resample_floating_trajectory from .interpolation import ( interpolate_minimum_jerk, interpolate_with_distance, @@ -40,6 +41,7 @@ __all__ = [ "retime_joint_paths", "PositionRangeMonitor", + "resample_floating_trajectory", "differentiate_positions", "resample_in_time", "retime_to_control_grid", diff --git a/embodichain/compute/trajectory/floating.py b/embodichain/compute/trajectory/floating.py new file mode 100644 index 0000000000..8237c02bbb --- /dev/null +++ b/embodichain/compute/trajectory/floating.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. +# ---------------------------------------------------------------------------- + +"""Time resampling of floating-root motion references.""" + +from __future__ import annotations +import math +import torch +from embodichain.utils.math import ( + matrix_from_quat, + quat_from_matrix, + axis_angle_from_quat, +) + +__all__ = ["resample_floating_trajectory"] + + +def resample_floating_trajectory( + qpos: torch.Tensor, source_fps: float, target_fps: float +) -> tuple[torch.Tensor, torch.Tensor]: + """Resample one root/joint clip and estimate world root-origin velocities. + + Args: + qpos: ``(frames, 7 + joints)`` positions with root XYZ and XYZW quaternion. + source_fps: Uniform input sample frequency. + target_fps: Uniform output frequency. The final sample is the last target + grid point within the input duration; no extra endpoint is appended. + + Returns: + Resampled qpos and ``[world linear, world angular, joint velocity]`` qvel. + Translation and joints use linear interpolation; orientation uses shortest + arc SLERP. Velocities use centered differences and one-sided endpoints. + """ + if any(not math.isfinite(f) or f <= 0 for f in (source_fps, target_fps)): + raise ValueError("Motion sample frequencies must be finite and positive") + if ( + qpos.ndim != 2 + or qpos.shape[0] < 2 + or qpos.shape[1] < 8 + or not qpos.is_floating_point() + or not bool(torch.isfinite(qpos).all()) + ): + raise ValueError( + "Motion positions require finite (frames>=2, 7+joints) samples" + ) + if bool((qpos[:, 3:7].norm(dim=-1) < 1e-8).any()): + raise ValueError("Root quaternions must be nonzero") + dt = 1.0 / target_fps + intervals = (qpos.shape[0] - 1) * target_fps / source_fps + # Include an endpoint on the target grid despite one-ULP division rounding. + count = math.floor(math.nextafter(intervals, math.inf)) + 1 + if count < 2: + raise ValueError("Target grid must contain at least two frames") + t = torch.arange(count, device=qpos.device, dtype=qpos.dtype) * dt * source_fps + lower = t.floor().long().clamp(max=qpos.shape[0] - 2) + alpha = (t - lower.to(t.dtype)).clamp(0, 1).unsqueeze(-1) + start, end = qpos[lower], qpos[lower + 1] + linear = (1 - alpha) * start + alpha * end + qa = torch.nn.functional.normalize(start[:, 3:7], dim=-1) + qb = torch.nn.functional.normalize(end[:, 3:7], dim=-1) + dot = (qa * qb).sum(-1, keepdim=True) + qb = torch.where(dot < 0, -qb, qb) + dot = dot.abs().clamp(max=1.0) + small = dot > 1.0 - 1e-7 + angle = torch.acos(torch.where(small, torch.zeros_like(dot), dot)) + denominator = angle.sin() + wa = torch.where(small, 1 - alpha, ((1 - alpha) * angle).sin() / denominator) + wb = torch.where(small, alpha, (alpha * angle).sin() / denominator) + quat = torch.nn.functional.normalize(wa * qa + wb * qb, dim=-1) + sampled = torch.cat((linear[:, :3], quat, linear[:, 7:]), -1) + translation_joints = torch.cat((sampled[:, :3], sampled[:, 7:]), -1) + velocity = torch.gradient(translation_joints, spacing=dt, dim=0)[0] + rotation = matrix_from_quat(quat) + before = torch.cat((rotation[:1], rotation[:-2], rotation[-2:-1]), 0) + after = torch.cat((rotation[1:2], rotation[2:], rotation[-1:]), 0) + span = sampled.new_full((count, 1), 2 * dt) + span[0] = dt + span[-1] = dt + angular = ( + axis_angle_from_quat(quat_from_matrix(after @ before.transpose(-1, -2))) / span + ) + return sampled, torch.cat((velocity[:, :3], angular, velocity[:, 3:]), -1) diff --git a/embodichain/data_pipeline/datasets/__init__.py b/embodichain/data_pipeline/datasets/__init__.py index 80ea9e3531..67492e18d5 100644 --- a/embodichain/data_pipeline/datasets/__init__.py +++ b/embodichain/data_pipeline/datasets/__init__.py @@ -22,6 +22,8 @@ from typing import TYPE_CHECKING, Any if TYPE_CHECKING: + from .motion import MotionLibrary + from .motion_sampling import MotionStartSampler, MotionStartSamplerCfg from .online_data import OnlineDataset from .sampler import ChunkSizeSampler, GMMChunkSampler, UniformChunkSampler from .sequence import ( @@ -37,6 +39,9 @@ from .schema import validate_data_schema, data_schema_fingerprint __all__ = [ + "MotionLibrary", + "MotionStartSampler", + "MotionStartSamplerCfg", "ChunkSizeSampler", "GMMChunkSampler", "OnlineDataset", @@ -60,6 +65,9 @@ def __getattr__(name: str) -> Any: if name not in __all__: raise AttributeError(f"module {__name__!r} has no attribute {name!r}") module_name = { + "MotionLibrary": "motion", + "MotionStartSampler": "motion_sampling", + "MotionStartSamplerCfg": "motion_sampling", "OnlineDataset": "online_data", "OfflineDataset": "offline_data", "SequenceDataset": "sequence_data", diff --git a/embodichain/data_pipeline/datasets/motion.py b/embodichain/data_pipeline/datasets/motion.py new file mode 100644 index 0000000000..b53a6cbbb2 --- /dev/null +++ b/embodichain/data_pipeline/datasets/motion.py @@ -0,0 +1,285 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Resident reference motion clips with explicit joint order and frame boundaries.""" + +from __future__ import annotations + +from collections.abc import Sequence +from pathlib import Path +from typing import Literal +import hashlib +import json +import math + +import numpy as np +import torch + +from embodichain.utils.math import matrix_from_quat + +__all__ = ["MotionLibrary"] + + +class MotionLibrary: + """Own floating-base reference states and query them on the target device. + + State fields are ``pos`` (world root position), ``quat`` (XYZW), ``rot`` + (rotation matrix), ``q`` (joint position), ``v`` / ``w`` (world root-origin + linear/angular velocity) and ``qd`` (joint velocity). Queries return owned + tensors; future offsets hold both clip endpoints and never cross clips. + + Attributes: + content_digest: Versioned identity of the selected float32 motion data, + clip layout, timing and joint order, cached at construction. + + Args: + qpos: Packed ``[root XYZ, quaternion, joint positions]`` array. + qvel: Packed ``[world linear, world angular, joint velocities]`` array. + lengths: Frame count of each consecutive clip. + names: Unique clip names in storage order. + joint_names: Joint names in the supplied position/velocity column order. + fps: Sampling frequency in Hz. + quaternion_order: Input quaternion convention; outputs always use XYZW. + device: Device for resident data and frame indices. + """ + + def __init__( + self, + qpos: np.ndarray, + qvel: np.ndarray, + lengths: Sequence[int], + names: Sequence[str], + joint_names: Sequence[str], + fps: float, + *, + quaternion_order: Literal["xyzw", "wxyz"] = "xyzw", + device: str | torch.device = "cpu", + ) -> None: + self.names = tuple(str(n) for n in names) + self.joint_names = tuple(joint_names) + lengths_array = np.asarray(lengths, dtype=np.int64) + dof = len(self.joint_names) + if not math.isfinite(fps) or fps <= 0: + raise ValueError("Motion fps must be finite and positive") + if ( + not len(lengths_array) + or lengths_array.ndim != 1 + or np.any(lengths_array < 2) + or len(self.names) != len(lengths_array) + or len(set(self.names)) != len(self.names) + ): + raise ValueError("Motion clips need unique names and at least two frames") + if not dof or len(set(self.joint_names)) != dof: + raise ValueError("Motion joint names must be nonempty and unique") + total = int(lengths_array.sum()) + if qpos.shape != (total, 7 + dof) or qvel.shape != (total, 6 + dof): + raise ValueError("Motion arrays do not match clip lengths and joint order") + if not np.isfinite(qpos).all() or not np.isfinite(qvel).all(): + raise ValueError("Motion states must be finite") + if quaternion_order not in ("xyzw", "wxyz"): + raise ValueError("Quaternion order must be xyzw or wxyz") + quaternion = np.asarray(qpos[:, 3:7], dtype=np.float32) + if quaternion_order == "wxyz": + quaternion = quaternion[:, [1, 2, 3, 0]] + if np.any(np.linalg.norm(quaternion, axis=-1) < 1e-8): + raise ValueError("Motion quaternions must be nonzero") + self.device = torch.empty(0, device=device).device + self.fps = float(fps) + self.dt = 1.0 / self.fps + self.num_frames = total + self.num_clips = len(self.names) + self.lengths = torch.tensor(lengths_array, device=self.device) + self.starts = torch.cat( + (self.lengths.new_zeros(1), self.lengths.cumsum(0)[:-1]) + ) + self.ends = self.starts + self.lengths + columns = { + "pos": qpos[:, :3], + "quat": quaternion, + "q": qpos[:, 7:], + "v": qvel[:, :3], + "w": qvel[:, 3:6], + "qd": qvel[:, 6:], + } + # Hash selected float32 source states once on the CPU, before device + # upload. Lengths and joint order give the byte stream its interpretation. + digest = hashlib.sha256() + digest.update( + json.dumps( + { + "version": 1, + "dtype": " MotionLibrary: + """Load packed qpos/qvel clips, optionally selecting a named subset. + + The archive contains qpos, qvel, lengths, names and fps. Optional starts + must describe consecutive clips. Optional joint_names must match the + requested order; archives without it require the caller's explicit order. + """ + with np.load(Path(path).expanduser(), allow_pickle=False) as data: + if ( + "quaternion_order" in data + and str(data["quaternion_order"]) != quaternion_order + ): + raise ValueError( + "Motion archive quaternion convention differs from the requested order" + ) + lengths = data["lengths"].astype(np.int64) + names = data["names"].astype(str).tolist() + starts = np.r_[0, np.cumsum(lengths)[:-1]] + if "starts" in data and not np.array_equal(data["starts"], starts): + raise ValueError("Motion archive must contain consecutive clips") + if "joint_names" in data and tuple( + data["joint_names"].astype(str) + ) != tuple(joint_names): + raise ValueError("Motion archive joint order does not match the robot") + qpos, qvel = data["qpos"], data["qvel"] + if clips is not None: + selected = [names.index(name) for name in clips] + rows = np.concatenate( + [np.arange(starts[i], starts[i] + lengths[i]) for i in selected] + ) + qpos, qvel = qpos[rows], qvel[rows] + lengths, names = lengths[selected], [names[i] for i in selected] + return cls( + qpos, + qvel, + lengths, + names, + joint_names, + float(data["fps"]), + quaternion_order=quaternion_order, + device=device, + ) + + @classmethod + def from_csv( + cls, + paths: Sequence[str | Path], + joint_names: Sequence[str], + *, + source_fps: float = 30.0, + target_fps: float = 50.0, + device: str | torch.device = "cpu", + ) -> MotionLibrary: + """Read headerless XYZ/XYZW/joint CSV clips and resample on the CPU. + + Paths determine clip order and their stems provide unique clip names. + Joint columns follow ``joint_names``. Resident output moves to ``device`` + after preprocessing; parsing does not require a simulator or GPU. + """ + from embodichain.compute.trajectory.floating import resample_floating_trajectory + + positions, velocities, lengths, names = [], [], [], [] + for path in paths: + path = Path(path).expanduser() + raw = np.loadtxt(path, delimiter=",", dtype=np.float32, ndmin=2) + if raw.shape[1] != 7 + len(joint_names): + raise ValueError( + f"Motion CSV joint width differs from joint_names: {path}" + ) + qpos, qvel = resample_floating_trajectory( + torch.from_numpy(raw), source_fps, target_fps + ) + positions.append(qpos.numpy()) + velocities.append(qvel.numpy()) + lengths.append(len(qpos)) + names.append(path.stem) + if not names: + raise ValueError("At least one motion CSV is required") + return cls( + np.concatenate(positions), + np.concatenate(velocities), + lengths, + names, + joint_names, + target_fps, + device=device, + ) + + def save_npz(self, path: str | Path) -> None: + """Write a portable XYZW archive with explicit joint and clip metadata.""" + s = {k: v.detach().cpu().numpy() for k, v in self._states.items()} + np.savez_compressed( + Path(path), + qpos=np.concatenate((s["pos"], s["quat"], s["q"]), -1), + qvel=np.concatenate((s["v"], s["w"], s["qd"]), -1), + lengths=self.lengths.cpu().numpy(), + starts=self.starts.cpu().numpy(), + names=np.asarray(self.names), + joint_names=np.asarray(self.joint_names), + fps=self.fps, + quaternion_order="xyzw", + ) + + def clip_ids(self, frames: torch.Tensor) -> torch.Tensor: + """Map valid global frame indices to clips, preserving leading axes.""" + return torch.searchsorted(self.starts, frames.contiguous(), right=True) - 1 + + def frame(self, frames: torch.Tensor) -> dict[str, torch.Tensor]: + """Read valid global indices, returning independent float32 state tensors.""" + state = { + k: v.index_select(0, frames.reshape(-1)).reshape( + *frames.shape, *v.shape[1:] + ) + for k, v in self._states.items() + } + state["rot"] = matrix_from_quat(state["quat"]) + return state + + def future( + self, frames: torch.Tensor, offsets: torch.Tensor + ) -> dict[str, torch.Tensor]: + """Query offsets from each frame with endpoint hold inside that clip.""" + clips = self.clip_ids(frames) + indices = frames.unsqueeze(-1) + offsets + indices = torch.maximum(indices, self.starts[clips].unsqueeze(-1)) + indices = torch.minimum(indices, (self.ends[clips] - 1).unsqueeze(-1)) + return self.frame(indices) + + def remaining(self, frames: torch.Tensor) -> torch.Tensor: + """Return the number of frames after each valid index in its clip.""" + return self.ends[self.clip_ids(frames)] - frames - 1 diff --git a/embodichain/data_pipeline/datasets/motion_sampling.py b/embodichain/data_pipeline/datasets/motion_sampling.py new file mode 100644 index 0000000000..a9bf0ca6cb --- /dev/null +++ b/embodichain/data_pipeline/datasets/motion_sampling.py @@ -0,0 +1,182 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Adaptive reference-start sampling from per-bin tracking returns.""" + +from __future__ import annotations + +from collections.abc import Mapping +from typing import Any +import torch +from embodichain.utils import configclass +from .motion import MotionLibrary + +__all__ = ["MotionStartSampler", "MotionStartSamplerCfg"] + + +@configclass +class MotionStartSamplerCfg: + """Mixture of uniform, current low-return and past low-return probabilities.""" + + bin_seconds: float = 1.0 + uniform_weight: float = 0.5 + current_weight: float = 0.4 + past_weight: float = 0.1 + temperature: float = 25.0 + probability_cap: float = 50.0 + return_ema: float = 0.1 + past_decay: float = 0.999 + + +class MotionStartSampler: + """Sample clip-local time bins and learn their normalized return statistics. + + The cap is a multiple of one bin's uniform share, applied to the current + softmax component before mixing. Scores from simultaneous episodes in the + same bin are averaged before their exponential moving average is updated. + """ + + def __init__( + self, motion: MotionLibrary, cfg: MotionStartSamplerCfg | None = None + ) -> None: + self.motion, self.cfg = motion, cfg or MotionStartSamplerCfg() + c = self.cfg + if ( + c.bin_seconds <= 0 + or c.temperature < 0 + or c.probability_cap < 1 + or not 0 < c.return_ema <= 1 + or not 0 < c.past_decay <= 1 + or min(c.uniform_weight, c.current_weight, c.past_weight) < 0 + or abs(c.uniform_weight + c.current_weight + c.past_weight - 1) > 1e-8 + ): + raise ValueError("Invalid reference-start sampling configuration.") + self.width = max(1, round(c.bin_seconds * motion.fps)) + lengths = motion.lengths.cpu().tolist() + starts = motion.starts.cpu().tolist() + lo = [] + hi = [] + clip = [] + base = [] + for i, (start, length) in enumerate(zip(starts, lengths)): + base.append(len(lo)) + for offset in range(0, length, self.width): + lo.append(start + offset) + hi.append(min(start + offset + self.width, start + length)) + clip.append(i) + self.lower = torch.tensor(lo, device=motion.device) + self.upper = torch.tensor(hi, device=motion.device) + self.clips = torch.tensor(clip, device=motion.device) + self.base = torch.tensor(base, device=motion.device) + self.scores = torch.zeros(len(lo), device=motion.device) + self.visits = torch.zeros(len(lo), device=motion.device) + self.past = torch.zeros(len(lo), device=motion.device) + + def bins(self, frames: torch.Tensor) -> torch.Tensor: + """Map reference frames to their clip-local time bins.""" + clips = self.motion.clip_ids(frames) + return self.base[clips] + (frames - self.motion.starts[clips]) // self.width + + @torch.no_grad() + def probabilities(self) -> torch.Tensor: + """Advance the past-softmax envelope and return the sampling mixture.""" + c = self.cfg + n = self.scores.numel() + soft = (-c.temperature * self.scores).softmax(0) + cap = min(1.0, c.probability_cap / n) + for _ in range(8): + excess = (soft - cap).clamp_min(0).sum() + free = torch.where(soft > cap, 0.0, soft) + soft = soft.clamp_max(cap) + excess * free / free.sum().clamp_min(1e-30) + self.past.copy_(torch.maximum(self.past * c.past_decay, soft)) + return ( + c.uniform_weight / n + + c.current_weight * soft + + c.past_weight * self.past / self.past.sum() + ) + + @torch.no_grad() + def sample(self, count: int, generator: torch.Generator) -> torch.Tensor: + """Draw frames leaving at least one transition before the clip ends.""" + bins = torch.multinomial( + self.probabilities(), count, replacement=True, generator=generator + ) + last = self.motion.ends[self.clips[bins]] - 1 + lower = self.lower[bins] + upper = torch.minimum(self.upper[bins], last).clamp_min(lower + 1) + frames = ( + lower + + ( + torch.rand(count, device=self.motion.device, generator=generator) + * (upper - lower) + ).long() + ) + return torch.minimum(frames, last - 1).clamp_min( + self.motion.starts[self.clips[bins]] + ) + + @torch.no_grad() + def update(self, frames: torch.Tensor, normalized_returns: torch.Tensor) -> None: + """Merge completed episodes by their start frames, including repeated bins.""" + bins = self.bins(frames) + count = torch.zeros_like(self.scores).index_add_( + 0, bins, torch.ones_like(normalized_returns) + ) + sums = torch.zeros_like(self.scores).index_add_(0, bins, normalized_returns) + mean = sums / count.clamp_min(1) + merged = torch.where( + self.visits > 0, + (1 - self.cfg.return_ema) * self.scores + self.cfg.return_ema * mean, + mean, + ) + self.scores.copy_(torch.where(count > 0, merged.clamp(0, 1), self.scores)) + self.visits.add_(count) + + def state_dict(self) -> dict[str, Any]: + """Save distribution statistics with the motion-layout identity.""" + return { + "config": self.cfg.to_dict(), + "names": self.motion.names, + "lengths": self.motion.lengths.cpu().tolist(), + "content_digest": self.motion.content_digest, + "scores": self.scores.clone(), + "visits": self.visits.clone(), + "past": self.past.clone(), + } + + def validate_state_dict(self, state: Mapping[str, Any]) -> None: + """Validate saved bins and statistics before mutating the sampler.""" + if ( + state["config"] != self.cfg.to_dict() + or tuple(state["names"]) != self.motion.names + or state["lengths"] != self.motion.lengths.cpu().tolist() + or state.get("content_digest") != self.motion.content_digest + ): + raise ValueError("Motion sampler checkpoint layout or content differs.") + for key in ("scores", "visits", "past"): + value = state[key] + if ( + value.shape != self.scores.shape + or not bool(torch.isfinite(value).all()) + or bool((value < 0).any()) + ): + raise ValueError("Invalid motion sampler statistics.") + + def load_state_dict(self, state: Mapping[str, Any]) -> None: + """Restore the current and historical curriculum distribution.""" + self.validate_state_dict(state) + for key in ("scores", "visits", "past"): + getattr(self, key).copy_(state[key]) diff --git a/embodichain/lab/gym/envs/differentiable_env.py b/embodichain/lab/gym/envs/differentiable_env.py index ab13a59991..60731c226d 100644 --- a/embodichain/lab/gym/envs/differentiable_env.py +++ b/embodichain/lab/gym/envs/differentiable_env.py @@ -13,12 +13,14 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- -"""Newton-backed kinematic environment for analytic policy gradient. +"""Simulation environment entry point for analytic policy gradient. -Wraps a task-defined Newton kinematics callback in a Warp tape and bridges +The current implementation 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. +Supported operations are task-defined differentiable kinematics, observations +and rewards. Physics-solver stepping and contact dynamics require a separate +differentiable provider. Usage: @@ -44,7 +46,11 @@ def _read_outputs(self, final_state) -> dict: ... class DifferentiableEnv(EmbodiedEnv): - """Newton-only environment with an APG-ready kinematic :meth:`step`. + """Simulation base with an APG-ready :meth:`step`. + + The current provider requires ``NewtonPhysicsCfg(requires_grad=True)`` and + implements differentiable kinematics. Configuration is checked before the + simulation is constructed. Subclasses implement :meth:`_apply_action_kernel`, :meth:`_make_kinematic_step_fn`, and :meth:`_read_outputs`. The action, diff --git a/embodichain/lab/sim/motion/_cartesian.py b/embodichain/lab/sim/motion/_cartesian.py index 278e95e6dd..d0ff75347d 100644 --- a/embodichain/lab/sim/motion/_cartesian.py +++ b/embodichain/lab/sim/motion/_cartesian.py @@ -79,7 +79,14 @@ def _generate_cartesian_motion( if enabled and options.start_qpos.shape[0] != 1: raise ValueError("Cartesian segment recovery currently supports B1 only.") start = generator.robot.compute_fk( - qpos=options.start_qpos, name=options.control_part, to_matrix=True + qpos=options.start_qpos, + name=options.control_part, + to_matrix=True, + **( + {"env_ids": generator._env_ids} + if getattr(generator, "_env_ids", None) is not None + else {} + ), ) base = replace( options, diff --git a/embodichain/lab/sim/motion/ik.py b/embodichain/lab/sim/motion/ik.py index 837a6152d0..faf2f10cea 100644 --- a/embodichain/lab/sim/motion/ik.py +++ b/embodichain/lab/sim/motion/ik.py @@ -83,8 +83,11 @@ def resolve_continuous_ik_path( robot = generator.robot seed = options.start_qpos.clone() bounds = robot.get_qpos_limits(name=options.control_part).to(seed) - lower, upper = bounds[..., 0], bounds[..., 1] limits = authored_joint_velocity_limits(robot, options.control_part).to(seed) + if getattr(generator, "_env_ids", None) is not None: + bounds = bounds[list(generator._env_ids)] + limits = limits[list(generator._env_ids)] + lower, upper = bounds[..., 0], bounds[..., 1] step_dt = options.interpolation_dt def solve( diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index 473e4c564d..88bae56c6a 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -815,6 +815,8 @@ def _generate_once( limits = authored_joint_velocity_limits( self.robot, options.control_part ).to(raw_result.positions) + if getattr(self, "_env_ids", None) is not None: + limits = limits[list(self._env_ids)] success = torch.as_tensor( raw_result.success, device=raw_result.positions.device, dtype=torch.bool ) @@ -1238,6 +1240,11 @@ def _solve_seeded_cartesian_waypoints( pose=poses[:, index].to(seed.device), name=control_part, joint_seed=seed, + **( + {"env_ids": self._env_ids} + if getattr(self, "_env_ids", None) is not None + else {} + ), ) valid = normalize_success_mask( valid, diff --git a/embodichain/learning/__init__.py b/embodichain/learning/__init__.py index 47b846ace2..ad6e708e23 100644 --- a/embodichain/learning/__init__.py +++ b/embodichain/learning/__init__.py @@ -24,7 +24,7 @@ from importlib import import_module from types import ModuleType -__all__ = ["layers", "models", "rl", "sl"] +__all__ = ["layers", "models", "rl", "sl", "dynamics"] def __getattr__(name: str) -> ModuleType: diff --git a/embodichain/learning/dynamics/__init__.py b/embodichain/learning/dynamics/__init__.py new file mode 100644 index 0000000000..afac607508 --- /dev/null +++ b/embodichain/learning/dynamics/__init__.py @@ -0,0 +1,23 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Schedules for alternating physical collection, dynamics and policy learning.""" + +from __future__ import annotations + +from .schedule import OnlineModelBasedScheduleCfg, OnlineModelBasedSchedule + +__all__ = ["OnlineModelBasedScheduleCfg", "OnlineModelBasedSchedule"] diff --git a/embodichain/learning/dynamics/schedule.py b/embodichain/learning/dynamics/schedule.py new file mode 100644 index 0000000000..6ade9661e4 --- /dev/null +++ b/embodichain/learning/dynamics/schedule.py @@ -0,0 +1,154 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Count and order measured collection, model warmup and online update cycles.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping +from typing import Any +import math + +from embodichain.utils import configclass + +__all__ = ["OnlineModelBasedScheduleCfg", "OnlineModelBasedSchedule"] + + +@configclass +class OnlineModelBasedScheduleCfg: + """Vector-step update ratio and distinct model-only warmup budget.""" + + warmup_updates: int = 500 + updates_per_collect: float = 0.4 + model_updates_per_cycle: int = 10 + + +class OnlineModelBasedSchedule: + """Drive supplied update operations synchronously at graph-safe boundaries. + + Collection commits detached physical data. Model updates finish before each + policy update starts; the policy callback completes its full backward pass + before returning. Callbacks own tensors, optimizers and their checkpoints. + This schedule owns progress and fractional update credit, independently of + environment count and network architecture. + """ + + def __init__(self, cfg: OnlineModelBasedScheduleCfg) -> None: + if ( + cfg.warmup_updates < 0 + or cfg.model_updates_per_cycle < 0 + or not math.isfinite(cfg.updates_per_collect) + or cfg.updates_per_collect <= 0 + ): + raise ValueError("Invalid online learning update budget.") + self.cfg = cfg + self.iterations = self.prefill_steps = self.physical_transitions = 0 + self.warmup_updates = self.model_updates = self.policy_updates = 0 + self.credit = 0.0 + + def collect(self, callback: Callable[[], int], *, prefill: bool = False) -> None: + """Commit one vector collection and record its physical transition count.""" + self.physical_transitions += callback() + self.prefill_steps += int(prefill) + + def warmup(self, update_model: Callable[[], dict[str, float]]) -> dict[str, float]: + """Perform one pending model-only update without consuming online credit.""" + if self.warmup_updates >= self.cfg.warmup_updates: + raise RuntimeError("Model warmup is already complete.") + metrics = update_model() + self.warmup_updates += 1 + self.model_updates += 1 + return metrics + + def step( + self, + collect: Callable[[], int], + update_model: Callable[[], dict[str, float]], + update_policy: Callable[[], dict[str, float]], + ) -> dict[str, float]: + """Collect once and execute the due model/policy cycles in dependency order.""" + if self.warmup_updates != self.cfg.warmup_updates: + raise RuntimeError("Complete model warmup before online learning.") + self.collect(collect) + self.credit += self.cfg.updates_per_collect + metrics: dict[str, float] = {} + samples: dict[str, int] = {} + + def merge(prefix: str, values: Mapping[str, float]) -> None: + for key, value in values.items(): + name = prefix + key + metrics[name] = metrics.get(name, 0.0) + float(value) + samples[name] = samples.get(name, 0) + 1 + + while self.credit >= 1.0 - 1e-12: + for _ in range(self.cfg.model_updates_per_cycle): + merge("wm/", update_model()) + self.model_updates += 1 + merge("apg/", update_policy()) + self.policy_updates += 1 + self.credit = max(0.0, self.credit - 1.0) + self.iterations += 1 + return {key: value / samples[key] for key, value in metrics.items()} + + def state_dict(self) -> dict[str, Any]: + """Serialize update progress; replay contents are independently owned.""" + return { + "config": self.cfg.to_dict(), + **{ + key: getattr(self, key) + for key in ( + "iterations", + "prefill_steps", + "physical_transitions", + "warmup_updates", + "model_updates", + "policy_updates", + "credit", + ) + }, + } + + def validate_state_dict(self, state: Mapping[str, Any]) -> None: + """Check schedule identity and counters before replacing live progress.""" + if ( + state.keys() != self.state_dict().keys() + or state["config"] != self.cfg.to_dict() + ): + raise ValueError("Online schedule checkpoint configuration differs.") + for key in ( + "iterations", + "prefill_steps", + "physical_transitions", + "warmup_updates", + "model_updates", + "policy_updates", + ): + if not isinstance(state[key], int) or state[key] < 0: + raise ValueError( + "Online schedule counters must be nonnegative integers." + ) + if ( + not 0 <= state["credit"] < 1 + or state["warmup_updates"] > self.cfg.warmup_updates + ): + raise ValueError("Invalid online schedule credit or warmup progress.") + + def load_state_dict(self, state: Mapping[str, Any]) -> None: + """Restore completed update counters and fractional scheduling credit.""" + self.validate_state_dict(state) + for key, value in state.items(): + if key != "config": + setattr(self, key, value) diff --git a/embodichain/learning/models/normalizer.py b/embodichain/learning/models/normalizer.py index c4d8d6dcae..2e134e42c2 100644 --- a/embodichain/learning/models/normalizer.py +++ b/embodichain/learning/models/normalizer.py @@ -32,24 +32,45 @@ class EmpiricalNormalizer(nn.Module): Args: feature_dim: Number of features in the final observation dimension. - epsilon: Stabilizer added to the running standard deviation. + epsilon: Positive scale stabilizer. + variance_epsilon: Use ``sqrt(variance + epsilon**2)`` instead of + ``standard_deviation + epsilon``. + clip: Optional symmetric normalized-value bound. """ - def __init__(self, feature_dim: int, epsilon: float = 1e-2) -> None: + def __init__( + self, + feature_dim: int, + epsilon: float = 1e-2, + *, + variance_epsilon: bool = False, + clip: float | None = None, + ) -> None: super().__init__() if feature_dim <= 0: raise ValueError("feature_dim must be positive") if epsilon <= 0.0: raise ValueError("epsilon must be positive") + if clip is not None and not 0 < clip < float("inf"): + raise ValueError("clip must be finite and positive") + self.variance_epsilon, self.clip = variance_epsilon, clip self.epsilon = float(epsilon) self.register_buffer("mean", torch.zeros(1, feature_dim)) self.register_buffer("variance", torch.ones(1, feature_dim)) self.register_buffer("standard_deviation", torch.ones(1, feature_dim)) self.register_buffer("count", torch.zeros((), dtype=torch.long)) + @property + def scale(self) -> torch.Tensor: + """Return the stabilized scale used for normalization and decoding.""" + if self.variance_epsilon: + return (self.variance + self.epsilon**2).sqrt() + return self.standard_deviation + self.epsilon + def forward(self, value: torch.Tensor) -> torch.Tensor: """Return observations normalized by the current running moments.""" - return (value - self.mean) / (self.standard_deviation + self.epsilon) + result = (value - self.mean) / self.scale + return result if self.clip is None else result.clamp(-self.clip, self.clip) @torch.no_grad() def update(self, value: torch.Tensor) -> None: diff --git a/embodichain/learning/rl/__init__.py b/embodichain/learning/rl/__init__.py index 044a3b87de..d4f574d258 100644 --- a/embodichain/learning/rl/__init__.py +++ b/embodichain/learning/rl/__init__.py @@ -35,11 +35,14 @@ "DifferentiableObservation": ("env", "DifferentiableObservation"), "DifferentiableRolloutSpec": ("env", "DifferentiableRolloutSpec"), "DifferentiableVecEnv": ("env", "DifferentiableVecEnv"), + "DifferentiableEnvProtocol": ("env", "DifferentiableEnvProtocol"), "LearningVecEnv": ("env", "LearningVecEnv"), + "LearningEnvProtocol": ("env", "LearningEnvProtocol"), "build_learning_env": ("env", "build_learning_env"), "get_registered_learning_env_names": ("env", "get_registered_learning_env_names"), "register_learning_env": ("env", "register_learning_env"), "ScheduledDifferentiableVecEnv": ("env", "ScheduledDifferentiableVecEnv"), + "ScheduledDifferentiableEnvProtocol": ("env", "ScheduledDifferentiableEnvProtocol"), "stratified_rollout_value": ("env", "stratified_rollout_value"), "evaluate_episodes": ("evaluation", "evaluate_episodes"), "BatchedGradientNormStats": ("gradients", "BatchedGradientNormStats"), @@ -65,10 +68,13 @@ def __getattr__(name: str) -> Any: "DifferentiableTrainerCfg", "DifferentiableRolloutSpec", "DifferentiableVecEnv", + "DifferentiableEnvProtocol", "BatchedGradientNormStats", "LearningVecEnv", + "LearningEnvProtocol", "RunningObservationNormalizer", "ScheduledDifferentiableVecEnv", + "ScheduledDifferentiableEnvProtocol", "build_learning_env", "clip_batched_gradient_norm", "evaluate_episodes", diff --git a/embodichain/learning/rl/algo/__init__.py b/embodichain/learning/rl/algo/__init__.py index cededb4852..a6cf62fdac 100644 --- a/embodichain/learning/rl/algo/__init__.py +++ b/embodichain/learning/rl/algo/__init__.py @@ -34,6 +34,11 @@ complete_discounted_return, segmented_discounted_return, ) +from .apg_actor_critic import ( + APGActorCritic, + APGActorCriticCfg, + differentiable_lambda_returns, +) from .base import BaseAlgorithm, RolloutKind from .common import compute_gae from .grpo import GRPO, GRPOCfg @@ -41,6 +46,7 @@ _ALGO_REGISTRY: Dict[str, Tuple[Type[Any], Type[Any]]] = { "apg": (APGCfg, APG), + "apg_actor_critic": (APGActorCriticCfg, APGActorCritic), "ppo": (PPOCfg, PPO), "grpo": (GRPOCfg, GRPO), } @@ -53,8 +59,9 @@ def get_registered_algo_names() -> list[str]: def _normalize_algo_cfg_kwargs(cfg_kwargs: Dict[str, Any]) -> Dict[str, Any]: """Coerce nested optimizer/scheduler mappings from YAML/JSON.""" normalized = dict(cfg_kwargs) - if "optimizer" in normalized: - normalized["optimizer"] = coerce_optimizer_cfg(normalized["optimizer"]) + for name in ("optimizer", "critic_optimizer"): + if name in normalized: + normalized[name] = coerce_optimizer_cfg(normalized[name]) if "lr_scheduler" in normalized: normalized["lr_scheduler"] = coerce_lr_scheduler_cfg(normalized["lr_scheduler"]) return normalized @@ -98,6 +105,9 @@ def build_algo( __all__ = [ "BaseAlgorithm", "RolloutKind", + "APGActorCritic", + "APGActorCriticCfg", + "differentiable_lambda_returns", "APGCfg", "APG", "complete_discounted_return", diff --git a/embodichain/learning/rl/algo/apg.py b/embodichain/learning/rl/algo/apg.py index 9d40b09d05..3d131e7ee3 100644 --- a/embodichain/learning/rl/algo/apg.py +++ b/embodichain/learning/rl/algo/apg.py @@ -19,6 +19,7 @@ from __future__ import annotations from typing import Dict +from contextlib import AbstractContextManager, nullcontext import torch @@ -111,7 +112,7 @@ def __init__(self, cfg: APGCfg, policy: torch.nn.Module) -> None: self.cfg = cfg self.policy = policy self.device = torch.device(cfg.device) - self._setup_optimization(cfg, policy.parameters()) + self._setup_optimization(cfg, self._optimization_parameters()) self._update_active = False self._update_valid = True self._discount: torch.Tensor | None = None @@ -121,6 +122,13 @@ def __init__(self, cfg: APGCfg, policy: torch.nn.Module) -> None: self._num_accumulated_steps = 0 self._action_gradient_stats: list[BatchedGradientNormStats] = [] + def rollout_context(self) -> AbstractContextManager[None]: + """Return the model-state context used during graph-preserving collection.""" + return nullcontext() + + def _optimization_parameters(self) -> tuple[torch.nn.Parameter, ...]: + return tuple(self.policy.parameters()) + def update(self, rollout: DifferentiableRollout) -> Dict[str, float]: """Apply one pathwise-gradient update from a rollout segment.""" self.begin_update() @@ -251,7 +259,7 @@ def _accumulate_loss( # 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()) + parameters = self._optimization_parameters() gradients_are_finite = all( parameter.grad is None or bool(torch.isfinite(parameter.grad).all()) for parameter in parameters @@ -270,7 +278,7 @@ def finish_update(self) -> Dict[str, float]: if self._num_accumulated_steps == 0: raise RuntimeError("Cannot finish an APG update without any segments.") - parameters = tuple(self.policy.parameters()) + parameters = self._optimization_parameters() if not self._update_valid: metrics = self._accumulated_metrics( grad_norm=float("nan"), @@ -296,6 +304,7 @@ def finish_update(self) -> Dict[str, float]: ) self._update_active = False return metrics + self._after_gradient_clip() self.optimizer.step() self._step_scheduler() metrics = self._accumulated_metrics( @@ -306,6 +315,9 @@ def finish_update(self) -> Dict[str, float]: self._update_active = False return metrics + def _after_gradient_clip(self) -> None: + """Apply algorithm-specific gradient transforms after norm validation.""" + def cancel_update(self) -> None: self.optimizer.zero_grad(set_to_none=True) self._update_active = False diff --git a/embodichain/learning/rl/algo/apg_actor_critic.py b/embodichain/learning/rl/algo/apg_actor_critic.py new file mode 100644 index 0000000000..cc6afe8424 --- /dev/null +++ b/embodichain/learning/rl/algo/apg_actor_critic.py @@ -0,0 +1,481 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Pathwise actor optimization with terminal values and detached lambda targets.""" + +from __future__ import annotations + +from contextlib import AbstractContextManager, contextmanager +from dataclasses import dataclass +import time +from typing import Any, Iterator + +import torch +from tensordict import TensorDict + +from embodichain.learning.rl.collector import DifferentiableRollout +from embodichain.learning.rl.utils import ( + LRSchedulerCfg, + OptimizerCfg, + build_optimizer, + coerce_optimizer_cfg, +) +from embodichain.utils import configclass +from .apg import APG, APGCfg + +__all__ = ["APGActorCritic", "APGActorCriticCfg", "differentiable_lambda_returns"] + + +def differentiable_lambda_returns( + rewards: torch.Tensor, + next_values: torch.Tensor, + continuation: torch.Tensor, + done: torch.Tensor, + *, + gamma: float, + gae_lambda: float, +) -> torch.Tensor: + """Compute time-major lambda returns with episode-local bootstrap. + + Args: + rewards: Rewards shaped ``[time, environments]``. + next_values: Values of successors captured before auto-reset. + continuation: Soft continuation times hard-termination and validity masks. + done: Episode boundaries. Truncations bootstrap but stop the recursion. + gamma: Discount per control step. + gae_lambda: Mixing coefficient between one-step and longer targets. + + Returns: + Returns with the same shape as rewards. Detachment is caller-owned. + """ + if ( + rewards.shape != next_values.shape + or rewards.shape != continuation.shape + or rewards.shape != done.shape + ): + raise ValueError("Lambda-return fields must have matching shapes.") + if rewards.ndim != 2 or rewards.shape[0] == 0: + raise ValueError("Lambda returns require a non-empty time-major batch.") + future = next_values[-1] + returns = [] + for index in range(rewards.shape[0] - 1, -1, -1): + future = torch.where(done[index], next_values[index], future) + future = rewards[index] + gamma * continuation[index] * ( + (1.0 - gae_lambda) * next_values[index] + gae_lambda * future + ) + returns.append(future) + return torch.stack(returns[::-1]) + + +@configclass +class APGActorCriticCfg(APGCfg): + """Actor and critic optimization settings for short differentiable rollouts. + + ``normalize_return`` divides the actor objective by the collected horizon. + Critic targets retain unscaled discounted reward units. Critic normalization + statistics can be updated after actor backpropagation, before forming targets. + """ + + critic_optimizer: OptimizerCfg = OptimizerCfg(learning_rate=2e-4) + critic_epochs: int = 5 + critic_minibatches: int = 4 + critic_max_grad_norm: float = 1.0 + actor_grad_value_clip: float = 0.0 + normalize_return: bool = True + update_critic_normalization: bool = False + + +@dataclass +class _CriticBatch: + inputs: torch.Tensor + successors: torch.Tensor + rewards: torch.Tensor + continuation: torch.Tensor + done: torch.Tensor + weights: torch.Tensor + + +class APGActorCritic(APG): + """Optimize an actor through rewards and a frozen terminal-value function. + + Policies expose disjoint actor/critic ``optimization_parameter_groups()`` + and ``get_value(TensorDict)``. Each segment bootstraps its final successor; + complete rollouts ignore rows after their first episode boundary. Critic + fitting uses detached inputs, lambda targets and soft-survival weights. + """ + + def __init__(self, cfg: APGActorCriticCfg, policy: torch.nn.Module) -> None: + if not hasattr(policy, "optimization_parameter_groups"): + raise TypeError( + "APGActorCritic requires actor and critic parameter groups." + ) + actor, critic = policy.optimization_parameter_groups() + self._actor_parameters, self._critic_parameters = tuple(actor), tuple(critic) + if not actor or not critic or set(map(id, actor)) & set(map(id, critic)): + raise ValueError( + "Actor and critic parameter groups must be non-empty and disjoint." + ) + if not 0.0 <= cfg.gamma <= 1.0 or not 0.0 <= cfg.gae_lambda <= 1.0: + raise ValueError("gamma and gae_lambda must lie in [0, 1].") + if cfg.critic_epochs < 1 or cfg.critic_minibatches < 1: + raise ValueError("Critic epochs and minibatches must be positive.") + if cfg.critic_max_grad_norm <= 0 or cfg.actor_grad_value_clip < 0: + raise ValueError("Invalid actor or critic gradient clipping limit.") + if cfg.update_critic_normalization and not callable( + getattr(policy, "update_critic_normalization", None) + ): + raise TypeError("Policy must expose update_critic_normalization().") + super().__init__(cfg, policy) + cfg.critic_optimizer = coerce_optimizer_cfg(cfg.critic_optimizer) + self.critic_optimizer = build_optimizer( + self._critic_parameters, cfg.critic_optimizer + ) + self._critic_batches: list[_CriticBatch] = [] + + def _optimization_parameters(self) -> tuple[torch.nn.Parameter, ...]: + return self._actor_parameters + + def rollout_context(self) -> AbstractContextManager[None]: + """Keep critic parameters and statistics fixed during actor collection.""" + return self._frozen_critic() + + @contextmanager + def _frozen_critic(self) -> Iterator[None]: + flags = [parameter.requires_grad for parameter in self._critic_parameters] + modules = [] + for name in ("critic", "critic_obs_normalizer"): + module = getattr(self.policy, name, None) + if isinstance(module, torch.nn.Module): + modules.extend(module.modules()) + modes = [(module, module.training) for module in modules] + try: + for module, _ in modes: + module.training = False + for parameter in self._critic_parameters: + parameter.requires_grad_(False) + yield + finally: + for module, mode in modes: + module.training = mode + for parameter, flag in zip(self._critic_parameters, flags): + parameter.requires_grad_(flag) + + def _value(self, observation: torch.Tensor) -> torch.Tensor: + fields = TensorDict( + {"obs": observation, "critic_obs": observation}, + batch_size=observation.shape[:-1], + ) + return self.policy.get_value(fields)["value"] + + def begin_update(self) -> None: + super().begin_update() + self.critic_optimizer.zero_grad(set_to_none=True) + self._critic_batches = [] + + def accumulate_segment(self, rollout: DifferentiableRollout) -> None: + """Accumulate a bootstrapped segment, restarting weights after reset.""" + self._accumulate( + rollout, complete=False, objective_scale=1.0, accumulation_scale=1.0 + ) + + def accumulate_complete_rollout( + self, + rollout: DifferentiableRollout, + *, + objective_scale: float | torch.Tensor = 1.0, + accumulation_scale: float = 1.0, + ) -> None: + """Accumulate one independent rollout through its first episode boundary.""" + self._accumulate( + rollout, + complete=True, + objective_scale=objective_scale, + accumulation_scale=accumulation_scale, + ) + + def _accumulate( + self, + rollout: DifferentiableRollout, + *, + complete: bool, + objective_scale: float | torch.Tensor, + accumulation_scale: float, + ) -> None: + if not self._update_active: + raise RuntimeError("Call begin_update() before accumulating a rollout.") + if not rollout.num_steps or accumulation_scale <= 0: + raise ValueError( + "A non-empty rollout and positive accumulation scale are required." + ) + transitions = rollout.transitions + observations, successors = [], [] + missing_terminal = [ + step.truncated + for step in transitions + if step.terminal_critic_observation is None + ] + if missing_terminal and bool(torch.stack(missing_terminal).any()): + raise ValueError( + "Truncation requires a pre-reset terminal_critic_observation." + ) + for step in transitions: + observations.append( + step.critic_observation + if step.critic_observation is not None + else step.observation + ) + successor = ( + step.next_critic_observation + if step.next_critic_observation is not None + else step.next_observation + ) + if step.terminal_critic_observation is not None: + successor = torch.where( + step.done.unsqueeze(-1), step.terminal_critic_observation, successor + ) + successors.append(successor) + inputs = torch.stack([observation.detach() for observation in observations]) + critic_successors = torch.stack( + [successor.detach() for successor in successors] + ) + rewards = rollout.rewards + ones = torch.ones_like(rewards[0]) + valid = torch.stack( + [ + ones if step.valid is None else step.valid.to(ones) + for step in transitions + ] + ).detach() + next_valid = torch.stack( + [ + ones if step.next_valid is None else step.next_valid.to(ones) + for step in transitions + ] + ).detach() + soft = torch.stack( + [ + ones if step.continuation is None else step.continuation.to(ones) + for step in transitions + ] + ) + terminated = torch.stack([step.terminated for step in transitions]) + done = torch.stack([step.done for step in transitions]) + continuation = soft * (~terminated).to(soft) * next_valid + rewards = rewards * next_valid + scale = torch.as_tensor( + objective_scale, device=rewards.device, dtype=rewards.dtype + ).detach() + if ( + scale.ndim > 1 + or (scale.ndim == 1 and scale.shape != ones.shape) + or not bool(torch.isfinite(scale).all()) + ): + raise ValueError( + "objective_scale must be finite and scalar or one value per environment." + ) + with self._frozen_critic(): + # Only episode boundaries and the final step bootstrap the actor. + # Other successor values enter detached critic targets below. + if not bool(done[:-1].any()): + values = torch.cat( + (torch.zeros_like(rewards[:-1]), self._value(successors[-1])[None]) + ) + else: + bootstrap = done.clone() + bootstrap[-1] = True + values = torch.zeros_like(rewards).masked_scatter( + bootstrap, self._value(torch.stack(successors)[bootstrap]) + ) + returns, entropy = torch.zeros_like(ones), torch.zeros_like(ones) + weight, discount = ones, ones + critic_weights = [] + for index, step in enumerate(transitions): + weight = weight * valid[index] + critic_weights.append(weight.detach()) + returns = returns + discount * weight * rewards[index] + if "entropy" in step.policy_output.keys(): + entropy = ( + entropy + discount * weight * step.policy_output["entropy"] + ) + boundary = ( + step.done + if index + 1 < rollout.num_steps + else torch.ones_like(step.done) + ) + returns = ( + returns + + discount + * self.cfg.gamma + * weight + * continuation[index] + * boundary + * values[index] + ) + weight = weight * continuation[index] + discount = discount * self.cfg.gamma + if complete: + weight = weight * (~step.done) + else: + weight = torch.where(step.done, ones, weight) + discount = torch.where(step.done, ones, discount) + divisor = rollout.num_steps if self.cfg.normalize_return else 1 + objective = (returns * scale).mean() / divisor + entropy = (entropy * scale).mean() / divisor + loss = (-objective - self.cfg.ent_coef * entropy) * accumulation_scale + self._critic_batches.append( + _CriticBatch( + inputs.detach(), + critic_successors, + rewards.detach(), + continuation.detach(), + done, + torch.stack(critic_weights), + ) + ) + self._accumulate_loss( + rollout, + loss=loss, + objective=objective * accumulation_scale, + entropy=entropy * accumulation_scale, + ) + + def finish_update(self) -> dict[str, float]: + """Apply the actor update, then regress the critic on detached targets.""" + metrics = super().finish_update() + if metrics["skipped_update"]: + self._critic_batches = [] + metrics["critic_seconds"] = 0.0 + return metrics + critic_started = time.perf_counter() + inputs = torch.cat( + [batch.inputs.flatten(0, 1) for batch in self._critic_batches] + ) + if self.cfg.update_critic_normalization: + # Count valid current states once. Add bootstrapped successors only + # when they are absent from the next valid current-state row. + samples = [] + for batch in self._critic_batches: + active = batch.weights > 0 + samples.append(batch.inputs[active]) + missing = torch.ones_like(batch.done) + missing[:-1] = batch.done[:-1] | ~active[1:] + bootstrap = active & (batch.continuation > 0) & missing + samples.append(batch.successors[bootstrap]) + samples = torch.cat(samples) + if samples.shape[0]: + self.policy.update_critic_normalization(samples) + with torch.no_grad(), self._frozen_critic(): + targets = torch.cat( + [ + differentiable_lambda_returns( + batch.rewards, + self._value(batch.successors), + batch.continuation, + batch.done, + gamma=self.cfg.gamma, + gae_lambda=self.cfg.gae_lambda, + ).flatten() + for batch in self._critic_batches + ] + ) + weights = torch.cat([batch.weights.flatten() for batch in self._critic_batches]) + # Empty-weight rows must not create Adam steps: momentum can update + # parameters even when the current minibatch gradient is zero. + target_mean, weight_mean = float(targets.mean()), float(weights.mean()) + active = weights > 0 + inputs, targets, weights = inputs[active], targets[active], weights[active] + totals = inputs.new_zeros(3) + updates = 0 + for _ in range(self.cfg.critic_epochs if inputs.shape[0] else 0): + indices = torch.randperm(inputs.shape[0], device=inputs.device) + for batch in indices.tensor_split( + min(self.cfg.critic_minibatches, inputs.shape[0]) + ): + predictions = self._value(inputs[batch]) + loss = ( + (predictions - targets[batch]).square() * weights[batch] + ).sum() / weights[batch].sum().clamp_min(1.0) + self.critic_optimizer.zero_grad(set_to_none=True) + loss.backward() + norm = torch.nn.utils.clip_grad_norm_( + self._critic_parameters, self.cfg.critic_max_grad_norm + ) + if bool(torch.isfinite(loss) & torch.isfinite(norm)): + self.critic_optimizer.step() + totals[0] += loss.detach() + totals[1] += norm.detach() + updates += 1 + else: + self.critic_optimizer.zero_grad(set_to_none=True) + totals[2] += 1 + if not self.cfg.skip_nonfinite_updates: + raise FloatingPointError( + "APGActorCritic produced non-finite critic gradients." + ) + metrics.update( + critic_loss=float(totals[0]) / max(updates, 1), + critic_grad_norm=float(totals[1]) / max(updates, 1), + critic_updates=float(updates), + critic_skipped_updates=float(totals[2]), + critic_target_mean=target_mean, + critic_weight_mean=weight_mean, + ) + self._critic_batches = [] + metrics["critic_seconds"] = time.perf_counter() - critic_started + return metrics + + def _after_gradient_clip(self) -> None: + if self.cfg.actor_grad_value_clip > 0: + torch.nn.utils.clip_grad_value_( + self._actor_parameters, self.cfg.actor_grad_value_clip + ) + + def cancel_update(self) -> None: + super().cancel_update() + self.critic_optimizer.zero_grad(set_to_none=True) + self._critic_batches = [] + + def state_dict(self) -> dict[str, Any]: + """Include both optimizers in trainer checkpoints.""" + return { + **super().state_dict(), + "critic_optimizer": self.critic_optimizer.state_dict(), + } + + def validate_state_dict( + self, state: dict[str, Any], *, resume_scheduler: str = "restore" + ) -> None: + """Validate both optimizer layouts before restoring live state.""" + super().validate_state_dict(state, resume_scheduler=resume_scheduler) + saved = state["critic_optimizer"] + groups = saved["param_groups"] + if len(groups) != len(self.critic_optimizer.param_groups): + raise ValueError("Incompatible critic optimizer parameter groups.") + for group, current in zip(groups, self.critic_optimizer.param_groups): + if len(group["params"]) != len(current["params"]): + raise ValueError("Incompatible critic optimizer parameter count.") + for identifier, parameter in zip(group["params"], current["params"]): + for value in saved["state"].get(identifier, {}).values(): + if ( + isinstance(value, torch.Tensor) + and value.ndim > 0 + and value.shape != parameter.shape + ): + raise ValueError("Incompatible critic optimizer state shape.") + + def _load_state_dict(self, state: dict[str, Any], *, resume_scheduler: str) -> None: + super()._load_state_dict(state, resume_scheduler=resume_scheduler) + self.critic_optimizer.load_state_dict(state["critic_optimizer"]) diff --git a/embodichain/learning/rl/checkpoint.py b/embodichain/learning/rl/checkpoint.py index 63479d4f10..485668afe0 100644 --- a/embodichain/learning/rl/checkpoint.py +++ b/embodichain/learning/rl/checkpoint.py @@ -30,7 +30,7 @@ import numpy as np import torch -from embodichain.learning.rl.env import ScheduledDifferentiableVecEnv +from embodichain.learning.rl.env import ScheduledDifferentiableEnvProtocol __all__: list[str] = [] @@ -178,7 +178,7 @@ def _metadata(trainer: Any) -> dict[str, Any]: trainer_config["gradient_accumulation_steps"] = ( trainer.cfg.gradient_accumulation_steps ) - if not isinstance(trainer.env, ScheduledDifferentiableVecEnv): + if not isinstance(trainer.env, ScheduledDifferentiableEnvProtocol): trainer_config["update_horizon"] = trainer.update_horizon return { "trainer": _type_name(trainer), diff --git a/embodichain/learning/rl/collector/differentiable.py b/embodichain/learning/rl/collector/differentiable.py index 36eb2b4754..bbffbb3516 100644 --- a/embodichain/learning/rl/collector/differentiable.py +++ b/embodichain/learning/rl/collector/differentiable.py @@ -26,7 +26,7 @@ from embodichain.learning.rl.env import ( DifferentiableObservation, - DifferentiableVecEnv, + DifferentiableEnvProtocol, ) from embodichain.learning.rl.gradients import ( BatchedGradientNormStats, @@ -34,7 +34,7 @@ ) from embodichain.learning.rl.policies import Policy from embodichain.learning.rl.normalization import RunningObservationNormalizer -from embodichain.learning.rl.utils import flatten_dict_observation +from embodichain.learning.rl.utils import flatten_observation_groups __all__ = [ "DifferentiableCollector", @@ -45,7 +45,14 @@ @dataclass(frozen=True) class DifferentiableTransition: - """One graph-preserving environment transition.""" + """One graph-preserving environment transition. + + ``terminal_critic_observation`` is the full successor batch before auto-reset. + ``next_critic_observation`` corresponds to the returned environment observation. + ``continuation`` is a differentiable probability; hard termination is + applied separately. ``valid`` and ``next_valid`` mark available task data + at the current and successor states. Missing masks default to one. + """ observation: torch.Tensor policy_output: TensorDict @@ -54,6 +61,12 @@ class DifferentiableTransition: truncated: torch.Tensor next_observation: torch.Tensor info: dict[str, Any] + critic_observation: torch.Tensor | None = None + next_critic_observation: torch.Tensor | None = None + terminal_critic_observation: torch.Tensor | None = None + continuation: torch.Tensor | None = None + valid: torch.Tensor | None = None + next_valid: torch.Tensor | None = None @property def action(self) -> torch.Tensor: @@ -134,7 +147,7 @@ class DifferentiableCollector: def __init__( self, - env: DifferentiableVecEnv, + env: DifferentiableEnvProtocol, policy: Policy, device: torch.device, *, @@ -211,7 +224,7 @@ def collect( ) for _ in range(num_steps): - observation = self._flatten_observation(self._observation) + observation = self._flatten_observation(self._observation).clone() policy_observation = ( self.observation_normalizer.normalize(observation) if self.observation_normalizer is not None @@ -222,6 +235,8 @@ def collect( batch_size=[self.env.num_envs], device=self.device, ) + critic_observation = self._critic_observation(self._observation).clone() + policy_input["critic_obs"] = critic_observation policy_output = self.policy.get_differentiable_action( policy_input, deterministic=deterministic, @@ -246,6 +261,12 @@ def collect( next_observation, reward, terminated, truncated, info = self.env.step( action ) + next_critic_observation = self._critic_observation(next_observation).clone() + terminal_critic_observation = ( + self._critic_observation(info["terminal_observation"]).clone() + if "terminal_observation" in info + else None + ) transition = DifferentiableTransition( observation=observation, policy_output=policy_output, @@ -254,6 +275,12 @@ def collect( truncated=truncated.to(self.device), next_observation=self._flatten_observation(next_observation), info=info, + critic_observation=critic_observation, + next_critic_observation=next_critic_observation, + terminal_critic_observation=terminal_critic_observation, + continuation=info.get("continuation"), + valid=info.get("valid"), + next_valid=info.get("next_valid"), ) transitions.append(transition) self._observation = next_observation @@ -279,4 +306,23 @@ def _flatten_observation( raise RuntimeError("The differentiable collector has no observation.") if isinstance(observation, torch.Tensor): return observation.to(self.device) - return flatten_dict_observation(observation.to(self.device)) + return flatten_observation_groups( + observation.to(self.device), getattr(self.policy, "actor_obs_groups", None) + ) + + def _critic_observation( + self, observation: DifferentiableObservation + ) -> torch.Tensor: + if getattr(self.policy, "uses_separate_critic_obs", False): + if isinstance(observation, torch.Tensor): + return observation.to(self.device) + return flatten_observation_groups( + observation.to(self.device), + getattr(self.policy, "critic_obs_groups", None), + ) + actor_observation = self._flatten_observation(observation) + return ( + self.observation_normalizer.normalize(actor_observation) + if self.observation_normalizer is not None + else actor_observation + ) diff --git a/embodichain/learning/rl/differentiable_trainer.py b/embodichain/learning/rl/differentiable_trainer.py index 9076c983fe..54250c7cf9 100644 --- a/embodichain/learning/rl/differentiable_trainer.py +++ b/embodichain/learning/rl/differentiable_trainer.py @@ -34,8 +34,8 @@ ) from embodichain.learning.rl.env import ( DifferentiableRolloutSpec, - DifferentiableVecEnv, - ScheduledDifferentiableVecEnv, + DifferentiableEnvProtocol, + ScheduledDifferentiableEnvProtocol, ) from embodichain.learning.rl.evaluation import evaluate_episodes from embodichain.learning.rl.policies import Policy @@ -80,11 +80,11 @@ class DifferentiableTrainer: def __init__( self, cfg: DifferentiableTrainerCfg, - env: DifferentiableVecEnv, + env: DifferentiableEnvProtocol, policy: Policy, algorithm: APG, writer: SummaryWriter | None = None, - eval_env: DifferentiableVecEnv | None = None, + eval_env: DifferentiableEnvProtocol | None = None, observation_normalizer: RunningObservationNormalizer | None = None, ) -> None: if cfg.segment_length <= 0: @@ -193,7 +193,7 @@ def train( raise ValueError("total_updates cannot be negative.") scheduled = self.cfg.rollout_mode == "complete" and isinstance( - self.env, ScheduledDifferentiableVecEnv + self.env, ScheduledDifferentiableEnvProtocol ) if total_updates is not None: estimated_updates = total_updates @@ -235,39 +235,13 @@ def train( if total_updates is not None else self.global_step < total_timesteps ): - self.algorithm._check_schedule_horizon(1) - 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 - ) - 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 - elapsed = max(time.time() - self.start_time, 1e-6) - entry = { - "global_step": float(self.global_step), - "num_updates": float(self.num_updates), - "charts/SPS": float((self.global_step - self._start_step) / elapsed), - "charts/episode_reward_avg_100": ( - float(sum(self.ret_window) / len(self.ret_window)) - if self.ret_window - else float("nan") - ), - "charts/episode_length_avg_100": ( - float(sum(self.len_window) / len(self.len_window)) - if self.len_window - else float("nan") - ), - **{f"train/{key}": value for key, value in metrics.items()}, - } - self.train_history.append(entry) - self._log(metrics) + 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 + ) + update_steps = min(self.update_horizon, remaining_vector_steps) + self.update_once(num_steps=update_steps) saved_best = False if ( @@ -290,6 +264,52 @@ def train( return self.get_summary() + def update_once(self, *, num_steps: int | None = None) -> dict[str, float]: + """Collect and optimize one update, recording counters and training metrics. + + A model-learning workflow can call this method between real-data + collection and supervised fitting. Evaluation and scheduled checkpoints + are managed by :meth:`train` or by the surrounding workflow. + + Args: + num_steps: Vector steps for segmented mode; defaults to update_horizon. + Complete mode obtains its horizon from the rollout schedule. + + Returns: + Actor and critic optimization metrics for this update. + """ + self.algorithm._check_schedule_horizon(1) + self.policy.train() + if self.cfg.rollout_mode == "complete": + collected_steps, metrics = self._complete_rollout_update() + else: + steps = self.update_horizon if num_steps is None else num_steps + if steps <= 0: + raise ValueError("num_steps must be positive.") + collected_steps, metrics = self._segmented_update(steps) + self.global_step += collected_steps * self.env.num_envs + self.num_updates += 1 + elapsed = max(time.time() - self.start_time, 1e-6) + entry = { + "global_step": float(self.global_step), + "num_updates": float(self.num_updates), + "charts/SPS": float((self.global_step - self._start_step) / elapsed), + "charts/episode_reward_avg_100": ( + float(sum(self.ret_window) / len(self.ret_window)) + if self.ret_window + else float("nan") + ), + "charts/episode_length_avg_100": ( + float(sum(self.len_window) / len(self.len_window)) + if self.len_window + else float("nan") + ), + **{f"train/{key}": value for key, value in metrics.items()}, + } + self.train_history.append(entry) + self._log(metrics) + return metrics + def _segmented_update(self, update_steps: int) -> tuple[int, dict[str, float]]: """Run one backward-compatible TBPTT optimizer update.""" collected_steps = 0 @@ -300,15 +320,18 @@ def _segmented_update(self, update_steps: int) -> tuple[int, dict[str, float]]: 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, - ) + with self.algorithm.rollout_context(): + 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 + # Retire snapshots before the next collection or critic update. + del rollout return collected_steps, self.algorithm.finish_update() except Exception: self.algorithm.cancel_update() @@ -325,11 +348,12 @@ def _complete_rollout_update(self) -> tuple[int, dict[str, float]]: 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, - ) + with self.algorithm.rollout_context(): + 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, @@ -338,6 +362,8 @@ def _complete_rollout_update(self) -> tuple[int, dict[str, float]]: self._update_observation_normalizer(rollout) self.collector.detach_state() collected_steps += rollout.num_steps + # Retire snapshots before the next collection or critic update. + del rollout for key, value in spec.metadata.items(): metadata.setdefault(str(key), []).append(float(value)) self._rollout_index += 1 @@ -350,7 +376,7 @@ def _complete_rollout_update(self) -> tuple[int, dict[str, float]]: return collected_steps, metrics def _prepare_complete_rollout(self) -> DifferentiableRolloutSpec: - if isinstance(self.env, ScheduledDifferentiableVecEnv): + if isinstance(self.env, ScheduledDifferentiableEnvProtocol): return self.env.prepare_differentiable_rollout(self._rollout_index) return DifferentiableRolloutSpec(num_steps=self.update_horizon) diff --git a/embodichain/learning/rl/env.py b/embodichain/learning/rl/env.py index 60bdeaff7a..ce1b86bd8c 100644 --- a/embodichain/learning/rl/env.py +++ b/embodichain/learning/rl/env.py @@ -29,17 +29,20 @@ __all__ = [ "DifferentiableObservation", "DifferentiableRolloutSpec", + "DifferentiableEnvProtocol", "DifferentiableVecEnv", + "LearningEnvProtocol", "LearningVecEnv", "build_learning_env", "get_registered_learning_env_names", "register_learning_env", + "ScheduledDifferentiableEnvProtocol", "ScheduledDifferentiableVecEnv", "stratified_rollout_value", ] DifferentiableObservation: TypeAlias = torch.Tensor | TensorDict -LearningEnvFactory: TypeAlias = Callable[..., "LearningVecEnv"] +LearningEnvFactory: TypeAlias = Callable[..., "LearningEnvProtocol"] _LEARNING_ENV_REGISTRY: dict[str, LearningEnvFactory] = {} @@ -87,8 +90,14 @@ def stratified_rollout_value(index: int, minimum: int, maximum: int) -> int: @runtime_checkable -class LearningVecEnv(Protocol): - """Structural interface shared by lightweight vector environments.""" +class LearningEnvProtocol(Protocol): + """Structural interface for batched learning environments. + + ``num_envs`` is the leading dimension of actions and observations, including + each tensor in a structured observation. The single spaces describe one row; + rewards, termination flags and truncation flags have shape ``[num_envs]``. + Tensors reside on ``device``. + """ num_envs: int device: torch.device @@ -120,13 +129,21 @@ def close(self) -> None: @runtime_checkable -class DifferentiableVecEnv(LearningVecEnv, Protocol): +class DifferentiableEnvProtocol(LearningEnvProtocol, Protocol): """Batched env that preserves the autograd path through ``step``. ``detach_state`` is the truncated-backpropagation boundary: detach differentiable internal state and return the current observation without resetting or resampling the episode. Finished rows must auto-reset inside ``step``, returning the terminal reward/done with the next initial observation. + + Value-bootstrapped algorithms also consume ``info["terminal_observation"]``: + a full observation batch captured before auto-reset, with its autograd graph + intact. It is required when a row truncates. Optional ``continuation`` gives + a differentiable probability in [0, 1]; ``valid`` and ``next_valid`` are + boolean data-availability masks at current and successor states. These + per-environment tensors have shape ``[num_envs]`` and default to one. + Rewards and transition metadata must retain their values until backward. """ def detach_state(self) -> DifferentiableObservation: @@ -135,7 +152,7 @@ def detach_state(self) -> DifferentiableObservation: @runtime_checkable -class ScheduledDifferentiableVecEnv(DifferentiableVecEnv, Protocol): +class ScheduledDifferentiableEnvProtocol(DifferentiableEnvProtocol, Protocol): """Differentiable env that schedules variable independent rollouts. The trainer calls :meth:`prepare_differentiable_rollout` immediately before @@ -159,6 +176,12 @@ def prepare_differentiable_rollout( ... +# Compatibility names preserve protocol identity for existing rollout consumers. +LearningVecEnv = LearningEnvProtocol +DifferentiableVecEnv = DifferentiableEnvProtocol +ScheduledDifferentiableVecEnv = ScheduledDifferentiableEnvProtocol + + def _is_nested_package_shadow(existing: Any, candidate: Any) -> bool: """Return True when ``candidate`` is an editable-install nested duplicate. @@ -217,7 +240,7 @@ def build_learning_env( num_envs: int, device: torch.device | str, **cfg: Any, -) -> LearningVecEnv: +) -> LearningEnvProtocol: """Build a registered lightweight vector environment.""" key = name.lower() if key not in _LEARNING_ENV_REGISTRY: diff --git a/embodichain/learning/rl/policies/registry.py b/embodichain/learning/rl/policies/registry.py index e6af23cebd..c48eb024d6 100644 --- a/embodichain/learning/rl/policies/registry.py +++ b/embodichain/learning/rl/policies/registry.py @@ -113,6 +113,8 @@ def build_policy( Built-in MLP policies still resolve flattened `obs_dim` / `action_dim`, while custom policies may accept richer `obs_space` / `action_space` inputs. + Policies with ``from_config(config, *, obs_space, action_space, device)`` + own configuration decoding and space validation. """ name = policy_block["name"].lower() if name not in _POLICY_REGISTRY: @@ -170,6 +172,12 @@ def build_policy( actor_obs_groups=actor_obs_groups, ) + factory = getattr(policy_cls, "from_config", None) + if factory is not None: + return factory( + policy_block, obs_space=obs_space, action_space=action_space, device=device + ) + init_params = inspect.signature(policy_cls.__init__).parameters build_kwargs: dict[str, object] = {"device": device} if "obs_space" in init_params: diff --git a/embodichain/learning/rl/runtime.py b/embodichain/learning/rl/runtime.py index 88e10e77cc..d8c87877fd 100644 --- a/embodichain/learning/rl/runtime.py +++ b/embodichain/learning/rl/runtime.py @@ -18,7 +18,7 @@ from __future__ import annotations -from collections.abc import Sequence +from collections.abc import Mapping, Sequence from dataclasses import dataclass from pathlib import Path from typing import Any @@ -97,6 +97,7 @@ def _build_learning_environment( *, device: torch.device, num_envs: int, + resources: Mapping[str, Any] | None = None, ) -> tuple[str, Any]: """Build the lightweight Environment declared by a training config.""" env_block = config["trainer"]["learning_env"] @@ -106,6 +107,13 @@ def _build_learning_environment( else: env_name = env_block["name"] env_config = dict(env_block.get("cfg", {})) + if resources is not None: + duplicate = env_config.keys() & resources.keys() + if duplicate: + raise ValueError( + f"Learning environment config and resources overlap: {sorted(duplicate)}" + ) + env_config.update(resources) return str(env_name), build_learning_env( str(env_name), num_envs=num_envs, @@ -144,12 +152,26 @@ def build_learning_policy_runtime( *, device: torch.device, num_envs: int, + resources: Mapping[str, Any] | None = None, ) -> PolicyRuntime: - """Build a lightweight Environment and its configured Policy.""" + """Build a configured learning Environment and Policy. + + Args: + config: Training configuration with trainer and policy blocks. + device: Device shared by the environment and policy. + num_envs: Learning rollout batch size. + resources: Workflow-owned objects injected into the environment factory, + such as a trained model and replay start provider. Keys must not + overlap the environment configuration. Objects are shared by reference. + + Returns: + Environment and policy constructed through their registries. + """ env_name, env = _build_learning_environment( config, device=device, num_envs=num_envs, + resources=resources, ) try: policy = _build_learning_policy(config["policy"], env, device) diff --git a/embodichain/utils/math.py b/embodichain/utils/math.py index 0754089449..d2cb994053 100644 --- a/embodichain/utils/math.py +++ b/embodichain/utils/math.py @@ -504,7 +504,9 @@ def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor: https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L91-L99 """ # Use torch.where for vectorized operation instead of indexed assignment - return torch.where(x > 0, torch.sqrt(x), torch.zeros_like(x)) + positive = x > 0 + safe_x = torch.where(positive, x, torch.ones_like(x)) + return torch.where(positive, torch.sqrt(safe_x), torch.zeros_like(x)) @torch.jit.script @@ -667,8 +669,11 @@ def axis_angle_to_rotation_matrix(axis_angle: torch.Tensor) -> torch.Tensor: # Rodrigues formula with stable Taylor fallback near zero angle. theta2 = torch.sum(axis_angle * axis_angle, dim=-1, keepdim=True) - theta = torch.sqrt(theta2) eps = 1.0e-8 + # Keep the inactive trigonometric branch finite at zero as well: where() + # still differentiates both branches before selecting their adjoints. + safe_theta2 = theta2.clamp_min(eps) + theta = torch.sqrt(safe_theta2) sin_over_theta = torch.where( theta2 > eps, @@ -677,7 +682,7 @@ def axis_angle_to_rotation_matrix(axis_angle: torch.Tensor) -> torch.Tensor: ) one_minus_cos_over_theta2 = torch.where( theta2 > eps, - (1.0 - torch.cos(theta)) / theta2, + (1.0 - torch.cos(theta)) / safe_theta2, 0.5 - theta2 / 24.0 + theta2 * theta2 / 720.0, ) @@ -813,7 +818,9 @@ def axis_angle_from_quat(quat: torch.Tensor, eps: float = 1.0e-6) -> torch.Tenso 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 + angle.abs() > eps, + torch.sin(half_angle) / angle.clamp_min(eps), + 0.5 - angle * angle / 48, ) return quat[..., :3] / sin_half_angles_over_angles.unsqueeze(-1) diff --git a/examples/learning/motion_tracking/README.md b/examples/learning/motion_tracking/README.md new file mode 100644 index 0000000000..a064b90ffd --- /dev/null +++ b/examples/learning/motion_tracking/README.md @@ -0,0 +1,379 @@ +# G1 motion tracking with an online world model + +This example collects physical G1 transitions, fits a world model with supervised +learning, and updates an APG Actor-Critic policy through imagined trajectories. +The official `UnitreeG1MotionRL-v1` task provides physical collection and independent +policy evaluation. The published model supports 29-joint G1 motion tracking at +50 Hz. + +The commands below use the installed `embodichain run-example` entry. Data and +outputs can live outside the source checkout. + +## Install the runtime + +Use a source checkout that includes the runtime prerequisites from #836 and the +motion-tracking changes from #924. FastRT replay also requires #833. From the +repository root, install the published `dexsim_engine==0.6.0rc1` wheel matching +Python 3.11 and the host Linux ABI, then install EmbodiChain and its bundled tasks +and models: + +```bash +uv venv --python 3.11 .venv-g1 +uv pip install --python .venv-g1/bin/python --torch-backend cu128 \ + 'torch>=2.7,<2.8' 'torchvision>=0.22,<0.23' +uv pip install --python .venv-g1/bin/python \ + --index http://pyp.open3dv.site:2345/simple \ + --allow-insecure-host pyp.open3dv.site 'dexsim_engine==0.6.0rc1' +uv pip install --python .venv-g1/bin/python . +uv pip check --python .venv-g1/bin/python +source .venv-g1/bin/activate +embodichain run-example learning/motion_tracking/train.py --help +``` + +The SDK pin matches `pyproject.toml`. The SDK index provides the published Linux +wheel for the selected Python ABI; its native libraries require a compatible +CUDA driver and Linux runtime. Record the source revision and installed SDK +version with run results. + +The Python 3.11 dependency profile uses PyTorch 2.7, TorchVision 0.22 and +TorchCodec 0.5. The first install command selects CUDA 12.8 wheels for Torch and +TorchVision; installing the project selects TorchCodec's CPU decoder from PyPI. +Earlier candidate installation checks used PyTorch 2.10, TorchVision 0.25 and +TorchCodec 0.10. The complete training recipe was run on a single NVIDIA RTX PRO +5000 with 72 GiB of memory. +The functional recipe uses smaller networks, batches and replay storage. + +TorchCodec requires FFmpeg shared libraries, and the simulator requires EGL/OpenGL +runtime libraries. On Ubuntu, install them with +`sudo apt install ffmpeg libegl1 libgl1 libglu1-mesa libopengl0 libsm6 libvulkan1`. Conda also provides FFmpeg through +`conda install -c conda-forge ffmpeg`. FastRT replay uses an NVIDIA GPU. + +Create a workspace and select its asset cache: + +```bash +mkdir -p g1-workspace +cd g1-workspace +export EMBODICHAIN_DATA_ROOT="$PWD/data" +``` + +Keep this environment variable set for data preparation, evaluation, training and +replay. Without it, assets use `~/.cache/embodichain_data`. + +## Prepare the motion data + +```bash +embodichain run-example learning/motion_tracking/prepare_lafan1.py +``` + +The entry downloads the fixed public source revision and prepares +`$EMBODICHAIN_DATA_ROOT/LAFAN1G1/`: + +| Output | Contents | +|---|---| +| `motion.npz` | 40 clips, 441,140 frames, 29 joints, 50 Hz, XYZW root quaternions and world-frame velocities | +| `robot/` | G1 URDF, meshes and robot-local notices | +| `preparation.json` | Source revision, clip/frame/joint counts and preparation parameters | +| `SOURCES.md`, `sources/retargeting/` | Source URLs, revision and upstream notices | + +The data sources are: + +- Original motion capture: [Ubisoft La Forge LAFAN1](https://github.com/ubisoft/ubisoft-laforge-animation-dataset), under [CC BY-NC-ND 4.0](https://github.com/ubisoft/ubisoft-laforge-animation-dataset/blob/master/license.txt). +- G1 retargeting project: [Unitree Robotics LAFAN1 Retargeting Dataset](https://www.modelscope.cn/datasets/unitreerobotics/LAFAN1_Retargeting_Dataset). +- Download mirror: [lvhaidong/LAFAN1_Retargeting_Dataset](https://huggingface.co/datasets/lvhaidong/LAFAN1_Retargeting_Dataset/tree/ce1572906efe6157840e8474d5a0d7aa87481e74), fixed at revision `ce1572906efe6157840e8474d5a0d7aa87481e74`. + +Preparation resamples the supplied 30 Hz joint trajectories to 50 Hz on the local +machine and retains their source notices. The robot description retains its +upstream BSD 3-Clause notice. `--source` accepts an existing source directory; +`--cache-dir` selects a Hugging Face download cache. + +## Download and evaluate the pretrained model + +Download the published seed-0 inference bundle: + +```bash +embodichain run-example learning/motion_tracking/download_pretrained.py \ + --revision e08d6548315be2b24a401e93dcb6434a614ea49f \ + --output models/g1-motion +MODEL=$(python -c 'import json; print(json.load(open("models/g1-motion/download.json"))["checkpoint"])') +``` + +The [model bundle](https://huggingface.co/DexForceAI/embodichain_model/tree/main/neural_dynamics/motion_tracking/g1/v1) +contains actor, critic and world-model weights, normalization statistics, portable +configuration and evaluation results. The download record stores its immutable +revision and checkpoint path. Checkpoint loading uses `weights_only=True`. + +Evaluate ten seconds of walking and save the states needed for replay: + +```bash +CUDA_VISIBLE_DEVICES=0 embodichain run-example learning/motion_tracking/evaluate_online.py "$MODEL" \ + --clips walk1_subject1 --max-steps 500 \ + --output outputs/pretrained-walk/evaluation.json \ + --trajectories outputs/pretrained-walk/trajectory.npz +``` + +For the complete 40-clip evaluation, omit `--clips` and `--max-steps`: + +```bash +CUDA_VISIBLE_DEVICES=0 embodichain run-example learning/motion_tracking/evaluate_online.py "$MODEL" \ + --output outputs/pretrained-full40/evaluation.json \ + --trajectories outputs/pretrained-full40/trajectory.npz +``` + +The historical seed-0 evaluation completed 40/40 full clips with a macro +normalized return of 0.94054. These results predate the reference-heading +correction; evaluation with the corrected heading remains pending. The model +bundle's `evaluation.json` records the original per-clip results and environment. + +Ordinary evaluation uses `--mode quality`. Each clip starts at its first frame +with nominal physical parameters and deterministic FP32 policy actions, and runs +to reference completion or physical failure. Normalized return divides cumulative +reward by the full reference length times the maximum step reward; the macro score +weights clips equally. Six tracking RMSE values cover usable successor frames, +including the terminal frame. Body pose errors align reference heading and +horizontal root position; root pose and body velocity errors use world coordinates. +`measured_steps` gives each error's denominator. Short evaluations record +`full_horizon: false`. `--mode physics-regression` enables deterministic Warp +execution for physical regression checks. + +## Render the recorded policy with FastRT + +```bash +CUDA_VISIBLE_DEVICES=0 embodichain run-example learning/motion_tracking/replay.py \ + outputs/pretrained-walk/trajectory.npz \ + --clips walk1_subject1 --seconds 10 \ + --output outputs/pretrained-walk/replay.mp4 +``` + +Replay displays the recorded policy states with a scene robot, a green reference +robot marker with scene lighting, red policy tracking points and green reference +tracking points. The trajectory archive stores both states and their matched +reference, joint names and tracked-link names. +Playback writes the recorded root and joint states through the robot API and +synchronizes rendering without advancing physics. The opaque reference robot uses +scene lighting; tracking points and floor traces use the overlay layer. On Linux +without a display, the entry selects SDL offscreen +initialization; an explicitly configured SDL video driver is preserved. + +Create a labelled multi-clip video from the full evaluation archive: + +```bash +CUDA_VISIBLE_DEVICES=0 embodichain run-example learning/motion_tracking/replay.py \ + outputs/pretrained-full40/trajectory.npz \ + --clips walk1_subject1,dance1_subject1 --seconds 10 \ + --output outputs/pretrained-full40/playlist.mp4 +``` + +`--resolution 1920 1080`, `--spp 8` and `--denoising dlss` control rendering. +The playlist concatenates independently evaluated clips; each segment shows its +clip name, reference time and the next clip countdown. The upper-left World XY +panel uses fixed bounds for each recorded clip and occupies 300 × 350 pixels at +1080p. The panel and ground traces show three seconds of executed motion and two +seconds of future reference in world coordinates. The green robot and reference +tracking points align heading and horizontal root position for body-pose comparison. +The panel reports world root XY error and aligned tracked-link RMS position error +in centimetres. + +## Check world-model predictions + +```bash +CUDA_VISIBLE_DEVICES=0 embodichain run-example learning/motion_tracking/evaluate_world_model.py "$MODEL" \ + --control policy --num-envs 80 --collect-steps 128 --batch-size 128 \ + --horizons 1 8 16 32 --seed 10042 \ + --output outputs/pretrained-world-model +``` + +The evaluator collects fresh physical episodes, samples windows within each +episode, and supplies measured actions to open-loop model rollouts. It reports +state errors, reward errors and termination disagreement at each horizon, alongside +a state-hold predictor using the same windows. `--control reference` samples +reference-PD transitions. The output records the selected motion data, hardware, +software, sample counts and checks that model weights and normalization statistics +remain unchanged. These measurements cover fresh trajectories from the training +motion library. + +## Train and resume + +`MotionTraining` is the G1/SuperMimic composition entry point. Its recipe +configures the following supported components: + +| Selection | Supported component | +| --- | --- | +| `workflow.collection_env` / `workflow.evaluation_env` | `UnitreeG1MotionRL-v1` task configurations | +| `trainer.learning_env.name` | `MotionTrackingModelRL` | +| `policy.name` | `motion_tracking` goal-conditioned actor and scalar critic | +| `algorithm.name` | `apg_actor_critic`, with separate actor and critic optimizers | +| Model and fitting objective | `KinematicWorldModel` and `RobotWorldModelObjective` | +| Replay | `TransitionReplay` with frame, previous action, policy action and usability fields | + +The entry validates the named selections before constructing the physical +environment. Policy dimensions derive from the G1 task and world-model features; +explicit dimensions must match. It constructs the WM optimizer and manages the +learning rates and recovery of all three optimizers. Alternative model families +or algorithms require a corresponding composition entry and resource adapters. +The reusable interfaces are the environment protocols and registries, +`DifferentiableTrainer.update_once()`, `SupervisedLearner`, and +`OnlineModelBasedSchedule` in `embodichain.learning.dynamics.schedule`. + +The workflow injects its live WM, motion library, task and replay-start provider +into the registered model environment. The model is frozen throughout each +policy update, while gradients propagate through its predicted states to the actor. + +When collection and evaluation resolve to the same environment file, evaluation +uses the resolved collection motion settings, including workflow and CLI overrides. +For a separate evaluation file, explicit motion-tracking settings take precedence; +omitted settings inherit collection defaults, except `clips`. An omitted `clips` +field or explicit `clips: null` evaluates all clips in the selected motion archive. +The resolved evaluation configuration is saved in the checkpoint. + +`workflow` also owns measured batch size, replay, WM fitting, update ratio and run +budget. `trainer.num_envs` is the imagined rollout batch size. The saved +`config.yaml` contains the complete resolved recipe; `run-manifest.json` records +the execution environment. Existing checkpoints with flat recipe fields are +normalized when loading their learning state. Resume also compares a cached +content identity of the selected motion clips, including positions, velocities, +clip layout, timing and joint order, before restoring learning state. A changed +motion archive or an older checkpoint without this identity is rejected; start +a new run for those inputs. + +### Replay and ODS ownership + +| Component | Responsibility | +| --- | --- | +| ODS `OnlineDataEngine` and data sources | Production, shared storage, sample selection, validity and producer lifecycle | +| ODS `ODSFlowController` | Refill, bounded waits, delivery limits and data-supply pacing | +| `OnlineModelBasedSchedule` | Collection/model/policy ordering and ratios, model-only warmup, update credit and progress | +| `TransitionReplay` | Device-resident measured transitions, episode-local windows/history and imagination starts | + +The synchronous G1 path uses device-resident replay. Append copies detached states, +controls and successors before physical reset; sampled tensors own their storage. +`terminated` and `truncated` close an episode after its final stored transition. +Windows repeat the final transition beyond that boundary with `valid=False`. +Oldest-first history repeats the earliest state within the episode; +`history_valid` identifies padding, and `history_complete` distinguishes real +reset padding from history lost to ring overwrite. G1 start sampling accepts reset +padding and rejects overwritten or unusable history. Model fitting selects complete, +usable windows before computing its loss. + +Replay `action` is the normalized joint setpoint after reference blending; +`policy_action` is the executed policy control including collection noise. With +resting position `q0`, action scale `s` and encoder bias `b`, the physical target is +`q0 + s * action - b`. Context records friction, encoder bias and COM offset; +frame and previous action retain reference progress and observation history. +The supplied G1 configurations use a 0.02 s control period (50 Hz), checked against +the task and motion library. Imagined clip suffixes expose `valid` and `next_valid` +separately from differentiable continuation probabilities. + +A future ODS-fed model learner can adapt `BatchSource` / `SequenceBatchSource` to +`SupervisedBatch` with these sequence and control semantics. The schedule remains +the owner of collection triggers and update accounting; ODS supply pacing must use +that trigger rather than schedule an additional collection. Learners and trainer +callbacks own optimizer steps and complete backward passes before returning. + +Configuration paths below resolve through the installed task package: + +```bash +CONFIGS=extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents +CUDA_VISIBLE_DEVICES=0 embodichain run-example learning/motion_tracking/train.py \ + --config "$CONFIGS/model_based.yaml" --output outputs/functional +``` + +The functional recipe runs two clips with 128 physical environments. It completes +200 collection iterations, 210 world-model updates and 80 actor updates, and writes +an independent evaluation. Inspect `invalid_transitions`, `apg/skipped_update` and +`apg/critic_skipped_updates` alongside the optimizer counters in `result.json`. + +Continue the same run to a total of 220 collection iterations: + +```bash +CUDA_VISIBLE_DEVICES=0 embodichain run-example learning/motion_tracking/train.py \ + --config "$CONFIGS/model_based.yaml" --output outputs/functional \ + --iterations 220 --resume outputs/functional/latest.pt +``` + +`latest.pt` restores networks, normalizers, optimizers, schedule counters, random +state and adaptive start-sampling statistics. Recovery constructs a physical scene +and refills replay before continuing. The published inference bundle contains +weights and configuration; optimizer recovery uses a training run's `latest.pt`. + +| Configuration | Purpose | Clips | Environments | Collection iterations | WM updates per actor update | +|---|---|---:|---:|---:|---:| +| `model_based.yaml` | Functional run | 2 | 128 | 200 | 2 | +| `model_based_development.yaml` | Development training | 4 | 512 | 2,000 | 10 | +| `model_based_full.yaml` | Complete training | 40 | 4,096 | 60,000 | 10 | + +Each iteration collects one vector step. `utd: 0.4` schedules two actor updates per +five collection steps. All recipes prefill replay for 64 steps; functional and +development recipes then perform 50 and 500 model-only warmup updates. The full +recipe starts joint online updates after prefill and completes 240,000 WM, +24,000 actor and 480,000 critic updates. Each actor update uses a 32-step imagined +trajectory. The full recipe compiles the actor, WM objective and imagined transition; +first calls include compilation and CUDA graph preparation. + +Run complete training with a new output directory and explicit seed: + +```bash +CUDA_VISIBLE_DEVICES=0 embodichain run-example learning/motion_tracking/train.py \ + --config "$CONFIGS/model_based_full.yaml" --seed 0 --output outputs/full-seed0 +``` + +To train a new policy through the published, fixed WM: + +```bash +CUDA_VISIBLE_DEVICES=0 embodichain run-example learning/motion_tracking/train.py \ + --config "$CONFIGS/model_based_fixed_wm.yaml" --wm-checkpoint "$MODEL" \ + --output outputs/fixed-world-model +``` + +This configuration updates the actor and critic while holding the world-model +weights and statistics fixed. The model architecture and robot must match the +selected checkpoint. + +## Run records + +| Output | Contents | +|---|---| +| `config.yaml` | Resolved recipe and seed | +| `metrics.jsonl` | Phase, update counts, WM/APG metrics, physical reward and failures, timing and resource samples | +| `latest.pt` | Complete learning state; replay is reconstructed on resume | +| `policy-NNNNNN.pt` | Evaluation weights and task configuration | +| `evaluation-*.json` | Per-clip return, failures, tracking errors and macro score | +| `environment-*.json` | Dataset selection, hardware, imported package versions, precision and source checkout | +| `result.json` | Actual optimizer counters, phase times, replay storage, Torch peaks, native GPU samples and CPU peak RSS | + +Timing records identify their process invocation. `apg` includes actor rollout and +critic work; its subtimings are components of that total. Torch allocator peaks and +sampled native process GPU allocation describe separate memory measurements. +`optimizer_steps` records minimum and maximum Adam step counters across parameters. + +## Components and gradients + +| Component | Responsibility | +|---|---| +| `MotionLibrary` | Reference queries, clip boundaries and adaptive start sampling | +| `UnitreeG1MotionRL-v1` | Robot/control mapping, physical reset and measured transitions | +| Shared tracking task | Observations, future goals, rewards and termination from tensor states | +| `KinematicWorldModel` | Kinematic features, normalized state increments and integration | +| `RobotWorldModelObjective` and `SupervisedLearner` | Eight-step measured windows and WM optimization | +| Model environment | Replay starts, history, reference progress and differentiable stepping | +| Motion-tracking policy | Goal encoder, action decoder and scalar critic | +| APG Actor-Critic | Differentiable rollout, actor objective, lambda-return targets and critic regression | +| Online schedule and `train.py` | Collection/update order, normalization cadence, records and recovery | + +The full model has 9,166,918 WM parameters, 9,214,877 actor parameters and 291,585 +critic parameters. The actor encodes 730 goal features into a 128-dimensional latent, +then combines it with 930 history features to produce 29 actions. Its encoder and +decoder use hidden widths 2048/1024/512/256. The critic maps 1,009 privileged features +through 256/128 hidden units to one value. + +Replay stores detached physical transitions. WM fitting differentiates through +eight predicted steps. During the actor's 32-step rollout, WM and critic parameters +and normalization statistics are fixed, while derivatives through actions, states, +rewards, continuation and terminal value remain connected. Actor backward finishes +before critic normalization changes. Critic inputs and lambda-return targets are +detached for value regression. This recipe uses APG pathwise gradients; `ppo.yaml` +provides the separate PPO task recipe. + +Control uses unclamped PD position targets with physical joint and effort limits. +The legs use absolute setpoints; the upper body uses reference residuals. The +policy receives robot-relative root position and orientation goals, and adaptive +sampling caps individual difficult-bin probabilities. diff --git a/examples/learning/motion_tracking/_configuration.py b/examples/learning/motion_tracking/_configuration.py new file mode 100644 index 0000000000..01193da705 --- /dev/null +++ b/examples/learning/motion_tracking/_configuration.py @@ -0,0 +1,119 @@ +# ---------------------------------------------------------------------------- +# 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 the online recipe and its saved pre-configuration checkpoints.""" + +from __future__ import annotations + +from copy import deepcopy +from typing import Any + + +def normalize_recipe(config: dict[str, Any]) -> dict[str, Any]: + """Return trainer/policy/algorithm blocks and task-owned workflow settings.""" + result = deepcopy(config) + if "trainer" in result: + if set(result) != {"trainer", "policy", "algorithm", "workflow"}: + raise ValueError( + "Motion recipes require trainer, policy, algorithm and workflow." + ) + return result + workflow = result + collection = workflow.pop("gym_config") + trainer = { + "learning_env": { + "name": "MotionTrackingModelRL", + "cfg": {"horizon": workflow.pop("horizon")}, + }, + "num_envs": workflow.pop("pi_batch"), + "rollout_mode": "complete", + "deterministic_actions": True, + } + actor_hidden = workflow.pop("actor_hidden") + policy = { + "name": "motion_tracking", + "cfg": { + "latent_dim": workflow.pop("latent"), + "encoder_hidden": actor_hidden, + "decoder_hidden": list(actor_hidden), + "critic_hidden": workflow.pop("critic_hidden"), + "amp_bfloat16": workflow["amp"], + }, + } + algorithm = { + "name": "apg_actor_critic", + "cfg": { + "gamma": workflow.pop("gamma"), + "gae_lambda": workflow.pop("lambda"), + "max_grad_norm": 1.0, + "optimizer": { + "learning_rate": workflow["lr"], + "kwargs": {"betas": [0.9, 0.95], "fused": True}, + }, + "critic_optimizer": { + "learning_rate": workflow["lr"], + "kwargs": {"fused": True}, + }, + "critic_epochs": workflow.pop("critic_epochs"), + "critic_minibatches": workflow.pop("critic_minibatches"), + "actor_grad_value_clip": 0.1, + "update_critic_normalization": True, + }, + } + workflow["collection_env"] = collection + workflow["evaluation_env"] = collection + return dict(trainer=trainer, policy=policy, algorithm=algorithm, workflow=workflow) + + +def validate_recipe(config: dict[str, Any]) -> None: + """Check the G1/SuperMimic composition before allocating its environment.""" + for path, block, expected in ( + ( + "trainer.learning_env", + config["trainer"].get("learning_env"), + "MotionTrackingModelRL", + ), + ("policy", config["policy"], "motion_tracking"), + ("algorithm", config["algorithm"], "apg_actor_critic"), + ): + name = block.get("name") if isinstance(block, dict) else None + if not isinstance(name, str) or name.lower() != expected.lower(): + raise ValueError( + f"G1 motion training requires {path}.name={expected!r}; got {name!r}." + ) + + +def recipe_identity(config: dict[str, Any]) -> dict[str, Any]: + """Select learning settings that must match before restoring any live state.""" + result = normalize_recipe(config) + workflow = result["workflow"] + for key in ( + "iterations", + "eval_every", + "evaluate_final", + "save_every", + "log_every", + "compile_mode", + "episode_context_in_info", + "wm_checkpoint", + "collection_env", + "evaluation_env", + ): + workflow.pop(key, None) + result["trainer"]["learning_env"].get("cfg", {}).pop("compile_mode", None) + # The policy's resolved dimensions and numerical settings have their own identity. + result.pop("policy") + return result diff --git a/examples/learning/motion_tracking/_replay_hud.py b/examples/learning/motion_tracking/_replay_hud.py new file mode 100644 index 0000000000..75a06f1388 --- /dev/null +++ b/examples/learning/motion_tracking/_replay_hud.py @@ -0,0 +1,185 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Compact world-space trajectory panel for recorded motion playback.""" + +from __future__ import annotations + +from functools import lru_cache + +import numpy as np +from PIL import Image, ImageDraw, ImageFont + + +@lru_cache(maxsize=16) +def load_font(size: int) -> ImageFont.FreeTypeFont | ImageFont.ImageFont: + """Load the playback font with Pillow's bundled fallback.""" + try: + return ImageFont.truetype("DejaVuSans.ttf", size) + except OSError: + return ImageFont.load_default(size=size) + + +class TrajectoryHUD: + """Draw a fixed-scale XY map with recent execution and future reference.""" + + size = (300, 350) + pale = (174, 211, 188) + green = (19, 115, 76) + red = (215, 65, 65) + ink = (36, 53, 65) + muted = (105, 123, 130) + background = (249, 252, 251) + + def __init__(self, actual: np.ndarray, reference: np.ndarray, *, dt: float) -> None: + self.actual = np.asarray(actual)[:, :2] + self.reference = np.asarray(reference)[:, :2] + bounds = np.concatenate((self.actual, self.reference)) + self.center = (bounds.min(0) + bounds.max(0)) / 2 + self.width = max(float(np.ptp(bounds, axis=0).max()) * 1.15, 1.0) + self.dt = dt + self.trail = round(3 / dt) + self.lead = round(2 / dt) + self.title_font = load_font(17) + self.font = load_font(14) + self.small = load_font(12) + self.plot_size = 224 + self.plot_origin = (54, 64) + self.base = Image.new("RGB", (self.plot_size, self.plot_size), self.background) + draw = ImageDraw.Draw(self.base) + for fraction in (0, 0.25, 0.5, 0.75, 1): + xy = round(fraction * (self.plot_size - 1)) + draw.line(((xy, 0), (xy, self.plot_size - 1)), fill=(224, 234, 229)) + draw.line(((0, xy), (self.plot_size - 1, xy)), fill=(224, 234, 229)) + context = self._points(self.reference[:: max(1, len(self.reference) // 2000)]) + if len(context) > 1: + draw.line([tuple(p) for p in context], fill=self.pale, width=2) + + def _points(self, xy: np.ndarray) -> np.ndarray: + return (np.asarray(xy) - self.center) / self.width * (self.plot_size - 1) * [ + 1, + -1, + ] + (self.plot_size - 1) / 2 + + def draw(self, image: Image.Image, frame: int, body_error_m: float) -> None: + """Overlay the map; body error is RMS tracked-link error after alignment.""" + panel = Image.new("RGBA", self.size) + draw = ImageDraw.Draw(panel) + draw.rounded_rectangle( + (0, 0, self.size[0] - 1, self.size[1] - 1), + radius=12, + fill=(*self.background, 246), + outline=(208, 222, 216, 255), + ) + draw.text((14, 11), "World XY (m)", font=self.title_font, fill=self.ink) + draw.text( + (286, 15), + f"{self.width:.1f} m wide", + anchor="ra", + font=self.small, + fill=self.muted, + ) + available = min(self.lead, len(self.reference) - frame - 1) * self.dt + for x, color, label in ( + (14, self.pale, "Ref path"), + (105, self.red, "Robot 3s"), + (203, self.green, f"Ref +{available:.1f}s"), + ): + draw.line(((x, 45), (x + 10, 45)), fill=color, width=3) + draw.text((x + 15, 37), label, font=self.small, fill=self.ink) + plot = self.base.copy() + pd = ImageDraw.Draw(plot) + actual = self._points(self.actual[max(0, frame - self.trail) : frame + 1]) + target = self._points(self.reference[frame : frame + self.lead + 1]) + for points, color in ((target, self.green), (actual, self.red)): + if len(points) > 1: + pd.line([tuple(p) for p in points], fill=color, width=2) + a, r = actual[-1], target[0] + for k in range(0, 10, 2): + pd.line( + (tuple(r + (a - r) * k / 10), tuple(r + (a - r) * (k + 1) / 10)), + fill=self.muted, + ) + if len(target) > 1: + direction = target[-1] - target[max(0, len(target) - 11)] + length = float(np.linalg.norm(direction)) + if length > 1: + u = direction / length + side = np.array([-u[1], u[0]]) + pd.polygon( + [ + tuple(target[-1]), + tuple(target[-1] - 7 * u + 3 * side), + tuple(target[-1] - 7 * u - 3 * side), + ], + fill=self.green, + ) + pd.ellipse( + (r[0] - 4, r[1] - 4, r[0] + 4, r[1] + 4), + fill=self.background, + outline=self.green, + width=2, + ) + pd.ellipse( + (a[0] - 3, a[1] - 3, a[0] + 3, a[1] + 3), fill=self.red, outline="white" + ) + panel.paste(plot, self.plot_origin) + px, py = self.plot_origin + for fraction in (0, 0.5, 1): + x = self.center[0] + (fraction - 0.5) * self.width + y = self.center[1] + (0.5 - fraction) * self.width + draw.text( + (px + fraction * (self.plot_size - 1), py + self.plot_size + 5), + f"{x:.1f}", + anchor="mt", + font=self.small, + fill=self.muted, + ) + draw.text( + (px - 7, py + fraction * (self.plot_size - 1)), + f"{y:.1f}", + anchor="rm", + font=self.small, + fill=self.muted, + ) + draw.line(((14, 313), (286, 313)), fill=(221, 231, 226)) + xy_error = float(np.linalg.norm(self.actual[frame] - self.reference[frame])) + draw.text( + (14, 324), f"XY {xy_error * 100:.1f} cm", font=self.font, fill=self.ink + ) + draw.text( + (157, 324), + f"Body {body_error_m * 100:.1f} cm", + font=self.font, + fill=self.ink, + ) + scale = min(image.width / 1920, image.height / 1080) + panel = panel.resize( + tuple(max(1, round(n * scale)) for n in self.size), Image.Resampling.LANCZOS + ) + image.paste(panel, (round(18 * scale), round(72 * scale)), panel) + + def metadata(self) -> dict: + """Return map dimensions, coordinate bounds and time-window semantics.""" + return { + "panel_pixels_at_1080p": list(self.size), + "map_center_xy_m": self.center.tolist(), + "map_width_m": self.width, + "history_seconds": 3, + "future_seconds": 2, + "coordinates": "unaligned world root XY", + "body_error": "RMS tracked-link position error after root heading/XY alignment", + } diff --git a/examples/learning/motion_tracking/_replay_visuals.py b/examples/learning/motion_tracking/_replay_visuals.py new file mode 100644 index 0000000000..54fac7c328 --- /dev/null +++ b/examples/learning/motion_tracking/_replay_visuals.py @@ -0,0 +1,224 @@ +# ---------------------------------------------------------------------------- +# 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-space floor traces for recorded motion-tracking playback.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np +from numpy.typing import ArrayLike, NDArray + +if TYPE_CHECKING: + from embodichain.lab.sim import SimulationManager + from embodichain.lab.visualization.markers import VisualizationMarkers + +__all__ = ["FloorTrajectory"] + + +class FloorTrajectory: + """Draw one clip's recent motion and future reference on the ground. + + Args: + sim: Prepared simulation owning the render-only markers. + reference_xy: Recorded reference root positions shaped ``(T, 2)``. + actual_xy: Recorded policy root positions shaped ``(T, 2)``. + dt: Time between recorded frames, in seconds. + + Traces use the original world coordinates. Reference-robot display alignment + and ghost offsets are independent of these positions. Each update publishes + 159 marker instances; the caller synchronizes rendering before capture. + """ + + def __init__( + self, + sim: SimulationManager, + reference_xy: ArrayLike, + actual_xy: ArrayLike, + *, + dt: float = 0.02, + ) -> None: + from embodichain.lab.visualization.markers import ( + MarkerPrototypeCfg, + VisualizationMarkers, + VisualizationMarkersCfg, + ) + + self._reference = np.array(reference_xy, dtype=np.float64, copy=True) + self._actual = np.array(actual_xy, dtype=np.float64, copy=True) + if ( + self._reference.ndim != 2 + or self._reference.shape[1:] != (2,) + or len(self._reference) == 0 + or self._actual.shape != self._reference.shape + ): + raise ValueError("Reference and actual XY must have matching shape (T, 2).") + if ( + not np.isfinite(self._reference).all() + or not np.isfinite(self._actual).all() + ): + raise ValueError("Recorded XY positions must be finite.") + if not np.isfinite(dt) or dt <= 0: + raise ValueError("The recorded frame interval must be finite and positive.") + + self._past_offsets = np.rint(np.linspace(-round(3 / dt), 0, 51)).astype( + np.int64 + ) + self._future_offsets = np.rint(np.linspace(0, round(2 / dt), 51)).astype( + np.int64 + ) + theta = np.arange(48) * (2 * np.pi / 48) + vertices = np.concatenate( + [ + np.column_stack([r * np.cos(theta), r * np.sin(theta), np.zeros(48)]) + for r in (0.085, 0.06) + ] + ) + faces = [] + for i in range(48): + following = (i + 1) % 48 + faces.extend([(i, following, 48 + i), (following, 48 + following, 48 + i)]) + colors = ( + (0.86, 0.12, 0.10, 1), + (0.02, 0.37, 0.16, 1), + (0.57, 0.76, 0.61, 1), + (0.25, 0.29, 0.28, 1), + ) + prototypes = { + f"line_{i}": MarkerPrototypeCfg(shape="box", color=color) + for i, color in enumerate(colors) + } + prototypes["actual"] = MarkerPrototypeCfg(shape="cylinder", color=colors[0]) + prototypes["reference"] = MarkerPrototypeCfg( + shape="mesh", vertices=vertices, faces=np.array(faces), color=colors[1] + ) + prototypes["future"] = MarkerPrototypeCfg( + shape="mesh", + vertices=np.array([[0, 0, 0], [-0.16, 0.08, 0], [-0.16, -0.08, 0]]), + faces=np.array([[0, 1, 2]]), + color=colors[1], + ) + self._markers: VisualizationMarkers | None = VisualizationMarkers( + VisualizationMarkersCfg( + prim_path="replay_floor_trajectory", markers=prototypes + ), + sim, + ) + self._positions = np.zeros((159, 3), dtype=np.float64) + self._rotations = np.tile([0.0, 0.0, 0.0, 1.0], (159, 1)) + self._scales = np.ones((159, 3), dtype=np.float64) + self._kinds = np.concatenate( + [np.repeat([2, 1, 0], 50), np.full(6, 3), [4, 5, 6]] + ).astype(np.int64) + + @staticmethod + def _trim_arrow( + points: NDArray[np.float64], + ) -> tuple[NDArray[np.float64], NDArray[np.float64], float]: + """End the shaft at the arrowhead base, preserving its original tip.""" + lengths = np.linalg.norm(np.diff(points, axis=0), axis=1) + distance = np.r_[0.0, np.cumsum(lengths)] + if distance[-1] < 0.12: + return points, np.zeros(2), 0.0 + cut = distance[-1] - min(0.16, distance[-1] * 0.5) + index = min( + int(np.searchsorted(distance, cut, side="right")) - 1, len(points) - 2 + ) + fraction = (cut - distance[index]) / lengths[index] + base = points[index] + fraction * (points[index + 1] - points[index]) + direction = points[-1] - base + head_length = float(np.linalg.norm(direction)) + if head_length < 0.04: + return points, np.zeros(2), 0.0 + shaft = points.copy() + shaft[index + 1 :] = base + return shaft, direction, head_length + + def _write_lines( + self, + start: int, + points: NDArray[np.float64], + width: float, + z: float, + ) -> None: + """Write a polyline into the existing instance arrays.""" + delta = np.diff(points, axis=0) + length = np.linalg.norm(delta, axis=1) + angle = np.arctan2(delta[:, 1], delta[:, 0]) + rows = slice(start, start + len(delta)) + self._positions[rows, :2] = (points[:-1] + points[1:]) / 2 + self._positions[rows, 2] = z + self._rotations[rows, 2] = np.sin(angle / 2) + self._rotations[rows, 3] = np.cos(angle / 2) + self._scales[rows, 0] = np.maximum(length, 1e-5) + self._scales[rows, 1] = np.where(length > 1e-6, width, 1e-5) + self._scales[rows, 2] = 0.001 + + def update(self, frame: int) -> None: + """Publish three seconds of history and two seconds of reference. + + Args: + frame: Clip-relative frame index, clamped to the recorded interval. + """ + if self._markers is None: + raise RuntimeError("Floor trajectory markers have been closed.") + frame = min(max(0, frame), len(self._actual) - 1) + past = np.clip(frame + self._past_offsets, 0, len(self._actual) - 1) + future = np.clip(frame + self._future_offsets, 0, len(self._reference) - 1) + self._write_lines(0, self._reference[past], 0.042, 0.001) + shaft, direction, head_length = self._trim_arrow(self._reference[future]) + self._write_lines(50, shaft, 0.032, 0.003) + self._write_lines(100, self._actual[past], 0.028, 0.005) + + current, goal = self._actual[frame], self._reference[frame] + for i, offset in enumerate(range(0, 12, 2)): + self._write_lines( + 150 + i, + np.array( + [ + goal + (current - goal) * offset / 12, + goal + (current - goal) * (offset + 1) / 12, + ] + ), + 0.018, + 0.007, + ) + end = self._reference[future[-1]] + angle = np.arctan2(direction[1], direction[0]) + self._positions[156:] = [[*current, 0.011], [*goal, 0.009], [*end, 0.0035]] + self._rotations[158] = [0, 0, np.sin(angle / 2), np.cos(angle / 2)] + arrow_length = max(head_length / 0.16, 1e-4) + arrow_width = ( + max(max(0.035, head_length / 2) / 0.08, 1e-4) if head_length else 1e-4 + ) + self._scales[156:] = [ + [0.11, 0.11, 0.002], + [1, 1, 1], + [arrow_length, arrow_width, 1], + ] + self._markers.visualize( + translations=self._positions, + orientations=self._rotations, + scales=self._scales, + marker_indices=self._kinds, + ) + + def close(self) -> None: + """Release this clip's markers before starting the next clip.""" + if self._markers is not None: + self._markers.remove() + self._markers = None diff --git a/examples/learning/motion_tracking/download_pretrained.py b/examples/learning/motion_tracking/download_pretrained.py new file mode 100644 index 0000000000..3703c327cf --- /dev/null +++ b/examples/learning/motion_tracking/download_pretrained.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. +# ---------------------------------------------------------------------------- + +"""Download the G1 motion-tracking policy and world-model inference bundle.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path +import re + +from huggingface_hub import HfApi, snapshot_download + +_REPOSITORY = "DexForceAI/embodichain_model" +_BUNDLE = "neural_dynamics/motion_tracking/g1/v1" +_FILES = ( + "weights.pt", + "manifest.json", + "evaluation.json", + "README.md", + "NOTICE", + "robot-LICENSE.txt", + "configs/env.yaml", + "configs/train.yaml", +) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument( + "--revision", + default="main", + help="Hub commit, tag or branch; resolved once to an immutable revision.", + ) + args = parser.parse_args() + revision = args.revision + if re.fullmatch(r"[0-9a-f]{40}", revision) is None: + revision = ( + HfApi(endpoint="https://huggingface.co", token=False) + .model_info(_REPOSITORY, revision=revision) + .sha + ) + if not isinstance(revision, str) or re.fullmatch(r"[0-9a-f]{40}", revision) is None: + raise ValueError("The requested model revision did not resolve to a commit.") + destination = args.output.expanduser().resolve() / revision + snapshot_download( + repo_id=_REPOSITORY, + revision=revision, + allow_patterns=[f"{_BUNDLE}/{name}" for name in _FILES], + local_dir=destination, + endpoint="https://huggingface.co", + token=False, + ) + bundle = destination / _BUNDLE + missing = [name for name in _FILES if not (bundle / name).is_file()] + if missing: + raise FileNotFoundError(f"Incomplete model bundle: {missing}") + manifest = json.loads((bundle / "manifest.json").read_text()) + if manifest.get("format") != "embodichain-motion-inference-v1": + raise ValueError("Unsupported motion-tracking inference bundle format.") + source = { + "repo_id": _REPOSITORY, + "revision": revision, + "bundle": _BUNDLE, + "checkpoint": str(bundle / "weights.pt"), + } + (args.output.expanduser() / "download.json").write_text( + json.dumps(source, indent=2) + "\n" + ) + print(json.dumps(source, indent=2)) + + +if __name__ == "__main__": + main() diff --git a/examples/learning/motion_tracking/evaluate.py b/examples/learning/motion_tracking/evaluate.py new file mode 100644 index 0000000000..8847f5221c --- /dev/null +++ b/examples/learning/motion_tracking/evaluate.py @@ -0,0 +1,109 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Evaluate a tracking policy once from the start of every motion clip.""" + +from __future__ import annotations +import argparse +import json +from pathlib import Path +import torch +from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, +) +from embodichain.learning.rl.evaluation import infer_policy_action +from embodichain.learning.rl.policy_evaluation.manifest import RunManifest +from embodichain.learning.rl.runtime import build_gym_policy_runtime +from embodichain.utils.utility import load_config + + +@torch.no_grad() +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("run", type=Path) + parser.add_argument("--device", default="cuda:0") + args = parser.parse_args() + manifest = RunManifest.load(args.run) + _, checkpoint = manifest.select_checkpoint("latest") + config = load_config(manifest.configs["train"]) + config["trainer"]["gym_config"] = str(manifest.configs["gym"]) + discover_task_packages() + execute_init_hooks() + runtime = build_gym_policy_runtime( + config, + device=torch.device(args.device), + headless=True, + renderer="no-render", + num_envs=1, + gpu_id=0, + ) + try: + env = runtime.env.unwrapped + runtime.policy.load_state_dict( + torch.load(checkpoint, map_location="cpu", weights_only=True)["policy"] + ) + runtime.policy.eval() + env.motion_cfg.observation_noise = False + env.motion_cfg.random_start = False + env.max_episode_steps = 2**31 - 1 + results = [] + for clip, name in enumerate(env.motion.names): + obs, _ = env.reset( + seed=0, options={"motion_frames": env.motion.starts[clip : clip + 1]} + ) + total = 0.0 + failed = False + steps = 0 + length = int(env.motion.lengths[clip]) - 1 + for _ in range(length): + action = infer_policy_action( + runtime.policy, obs, device=runtime.device, num_envs=1 + ) + obs, reward, terminated, truncated, _ = env.step(action) + total += float(reward[0]) + steps += 1 + if bool(terminated[0] | truncated[0]): + failed = bool(terminated[0]) + break + results.append( + { + "clip": name, + "steps": steps, + "reference_steps": length, + "failed": failed, + "return": total, + "normalized_return": total / (length * env.task.max_step_reward), + } + ) + result = { + "checkpoint": str(checkpoint), + "clips": results, + "failures": sum(x["failed"] for x in results), + "macro_normalized_return": sum(x["normalized_return"] for x in results) + / len(results), + } + output = manifest.root / "evaluations/motion-clips.json" + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(json.dumps(result, indent=2) + "\n") + print(f"Evaluation report: {output}") + print(json.dumps({k: v for k, v in result.items() if k != "clips"}, indent=2)) + finally: + runtime.close() + + +if __name__ == "__main__": + main() diff --git a/examples/learning/motion_tracking/evaluate_online.py b/examples/learning/motion_tracking/evaluate_online.py new file mode 100644 index 0000000000..41cb59d48e --- /dev/null +++ b/examples/learning/motion_tracking/evaluate_online.py @@ -0,0 +1,282 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Evaluate an online-training checkpoint from every clip's first frame.""" + +from __future__ import annotations + +import argparse +from copy import deepcopy +import json +from pathlib import Path +import time +import numpy as np +import torch +import warp as wp +from datetime import datetime, timezone +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain_models.policies.motion_tracking import ( + MotionTrackingPolicy, + MotionTrackingPolicyCfg, +) +from embodichain_models.dynamics.rwm.robot_state import pack_robot_state +from embodichain.lab.gym.utils.registration import build_env +from embodichain_tasks.locomotion.tracking import g1_motion # Register physical task. +from run_metadata import write_run_metadata +from _configuration import normalize_recipe + + +@torch.no_grad() +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("checkpoint", type=Path) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--trajectories", type=Path) + parser.add_argument("--motion-path", type=Path) + parser.add_argument( + "--clips", + help="Comma-separated evaluation clips; defaults to the checkpoint selection.", + ) + parser.add_argument("--quaternion-order", choices=("xyzw", "wxyz")) + parser.add_argument("--seed", type=int, default=10000) + parser.add_argument( + "--mode", choices=("quality", "physics-regression"), default="quality" + ) + parser.add_argument( + "--max-steps", + type=int, + help="Bound a functional evaluation; results record the shortened horizon.", + ) + args = parser.parse_args() + torch.set_num_threads(4) + torch.manual_seed(args.seed) + torch.backends.cuda.matmul.allow_tf32 = False + torch.backends.cudnn.allow_tf32 = False + wp.config.deterministic = ( + wp.DeterministicMode.RUN_TO_RUN + if args.mode == "physics-regression" + else wp.DeterministicMode.NOT_GUARANTEED + ) + if args.max_steps is not None and args.max_steps < 1: + parser.error("--max-steps must be positive") + state = torch.load(args.checkpoint, map_location="cpu", weights_only=True) + training_config = normalize_recipe(state["config"])["workflow"] + gym = deepcopy(state.get("evaluation_gym_config", state["gym_config"])) + gym["num_envs"] = 1 + options = gym["env"]["extensions"]["motion_tracking"] + if args.motion_path is not None: + options["motion_path"] = str(args.motion_path) + if args.clips is not None: + options["clips"] = args.clips.split(",") + if args.quaternion_order is not None: + options["quaternion_order"] = args.quaternion_order + options["observation_noise"] = False + options["random_start"] = False + options["adaptive_sampling"] = False + options["fixed_clip_assignment"] = True + options["reset_noise"] = False + options["domain_randomization"] = False + # Read clip count before constructing the single physical scene. + from embodichain.data import get_data_path + from embodichain.data_pipeline.datasets.motion import MotionLibrary + + joints = state["identity"]["wm"]["joint_names"] + motion = MotionLibrary.from_npz( + get_data_path(options["motion_path"]), + joints, + quaternion_order=options["quaternion_order"], + clips=options.get("clips"), + ) + gym["num_envs"] = motion.num_clips + gym["max_episode_steps"] = 2**31 - 1 + cfg = config_to_cfg(gym) + cfg.init_rollout_buffer = False + env = build_env(gym["id"], base_env_cfg=cfg) + try: + args.output.parent.mkdir(parents=True, exist_ok=True) + metadata = write_run_metadata( + args.output.parent, + invocation_started_at=datetime.now(timezone.utc).isoformat(), + config={ + "kind": "evaluation", + "mode": args.mode, + "seed": args.seed, + "amp": False, + }, + gym_config=gym, + motion={ + "names": env.motion.names, + "lengths": env.motion.lengths.cpu().tolist(), + "fps": env.motion.fps, + }, + motion_path=Path(get_data_path(options["motion_path"])), + device=env.device, + resume_checkpoint=args.checkpoint.resolve(), + ) + pcfg = MotionTrackingPolicyCfg(**state["identity"]["policy"]) + pcfg.amp_bfloat16 = False + policy = MotionTrackingPolicy(pcfg, env.device) + policy.load_state_dict(state["policy"]) + policy.eval() + tracking_errors = env.task.tracking_errors + if env.motion_cfg.compile_mode is not None: + # Fuse metric FK without adding a CUDA graph output lifetime to evaluation. + tracking_errors = torch.compile( + tracking_errors, + fullgraph=True, + options={"triton.cudagraphs": False}, + ) + obs, _ = env.reset(seed=args.seed, options={"motion_frames": env.motion.starts}) + active = torch.ones(env.num_envs, dtype=torch.bool, device=env.device) + total = torch.zeros(env.num_envs, device=env.device) + steps = torch.zeros_like(total, dtype=torch.int64) + failed = torch.zeros_like(active) + measured_steps = torch.zeros_like(steps) + error_sums = { + name: torch.zeros_like(total) for name in env.task.reward_names[:6] + } + failure_reasons = { + name: torch.zeros_like(active) for name in ("height", "tilt", "non_finite") + } + trajectories = [] + references = [] + alive = [] + start = time.perf_counter() + full_steps = int(env.motion.lengths.max()) - 1 + horizon = min(full_steps, args.max_steps) if args.max_steps else full_steps + for _ in range(horizon): + if not bool(active.any()): + break + action = policy.actor(obs["policy"]) + obs, reward, terminated, truncated, info = env.step(action) + total += torch.where(active, reward, 0) + steps += active + transition = info["transition"] + reference = env.motion.frame(transition["frame"]) + measured = active & transition["usable"] + measured_steps += measured + for name, error in tracking_errors(transition["state"], reference).items(): + error_sums[name] += torch.where(measured, error, 0) + for name, mask in env.task.failure_reasons( + transition["state"], reference + ).items(): + failure_reasons[name] |= ( + active & transition["usable"] & terminated & mask + ) + failure_reasons["non_finite"] |= active & ~transition["usable"] + if args.trajectories: + trajectories.append( + pack_robot_state(info["transition"]["state"]).cpu().numpy() + ) + references.append(pack_robot_state(reference).cpu().numpy()) + alive.append(active.cpu().numpy()) + failed |= terminated & active + active &= ~(terminated | truncated) + result = { + "checkpoint": str(args.checkpoint), + "iteration": state["schedule"]["iterations"], + "kind": "evaluation", + "mode": args.mode, + "environment_metadata": metadata.name, + "full_horizon": horizon == full_steps, + "max_steps": args.max_steps, + "training_kind": training_config["kind"], + "evaluation": "full-clip physical rollout; fixed reference starts; no observation/action noise", + "seed": args.seed, + "evaluation_dataset": { + "path": str(get_data_path(options["motion_path"])), + "quaternion_order": options["quaternion_order"], + "clips": env.motion.names, + "frames": env.motion.lengths.cpu().tolist(), + "fps": env.motion.fps, + }, + "reset": { + "fixed_clip_assignment": True, + "assigned_start_frames": env.motion.starts.cpu().tolist(), + "final_episode_start_frames": env._episode_start.cpu().tolist(), + }, + "precision": { + "policy": "float32", + "matmul_tf32": torch.backends.cuda.matmul.allow_tf32, + "cudnn_tf32": torch.backends.cudnn.allow_tf32, + }, + "tracking_error_definition": "RMSE over usable physical successor frames, including the terminal frame; body pose errors average tracked links after heading/horizontal-root alignment; root pose and body velocities use world coordinates; position in m, angle in rad, linear velocity in m/s, angular velocity in rad/s", + "seconds": time.perf_counter() - start, + "clips": [ + { + "clip": name, + "steps": int(steps[i]), + "reference_steps": int(env.motion.lengths[i]) - 1, + "failed": bool(failed[i]), + "end_reasons": ( + [ + name + for name, mask in failure_reasons.items() + if bool(mask[i]) + ] + if bool(failed[i]) + else [ + "evaluation_limit" if bool(active[i]) else "reference_end" + ] + ), + "measured_steps": int(measured_steps[i]), + "tracking_rmse": { + name: ( + float((value[i] / measured_steps[i]).sqrt()) + if int(measured_steps[i]) + else None + ) + for name, value in error_sums.items() + }, + "return": float(total[i]), + "normalized_return": float( + total[i] + / ((env.motion.lengths[i] - 1) * env.task.max_step_reward) + ), + } + for i, name in enumerate(env.motion.names) + ], + } + result["failures"] = sum(x["failed"] for x in result["clips"]) + result["macro_normalized_return"] = sum( + x["normalized_return"] for x in result["clips"] + ) / len(result["clips"]) + args.output.parent.mkdir(parents=True, exist_ok=True) + args.output.write_text(json.dumps(result, indent=2) + "\n") + if args.trajectories: + args.trajectories.parent.mkdir(parents=True, exist_ok=True) + np.savez_compressed( + args.trajectories, + format_version=1, + state=np.asarray(trajectories), + reference=np.asarray(references), + active=np.asarray(alive), + names=env.motion.names, + joint_names=env.motion.joint_names, + tracked_link_names=[ + env.task.kinematics.link_names[i] + for i in env.task.tracked.cpu().tolist() + ], + state_layout="root_position[3],root_rotation_row_major[9],root_linear_velocity[3],root_angular_velocity[3],joint_position[dof],joint_velocity[dof]", + dt=env.step_dt, + ) + print(json.dumps(result, indent=2), flush=True) + finally: + env.close(exit_process=False) + + +if __name__ == "__main__": + main() diff --git a/examples/learning/motion_tracking/evaluate_world_model.py b/examples/learning/motion_tracking/evaluate_world_model.py new file mode 100644 index 0000000000..b1c4bb1a81 --- /dev/null +++ b/examples/learning/motion_tracking/evaluate_world_model.py @@ -0,0 +1,228 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Measure frozen WM prediction on newly collected, episode-local physical windows.""" + +from __future__ import annotations + +import argparse +from copy import deepcopy +from datetime import datetime, timezone +import json +from pathlib import Path +import time + +import torch + +from embodichain.data import get_data_path +from embodichain_models.dynamics.rwm.robot_state import unpack_robot_state +from run_metadata import write_run_metadata +from train import MotionTraining +from _configuration import normalize_recipe + + +@torch.no_grad() +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("checkpoint", type=Path) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--control", choices=("policy", "reference"), default="policy") + parser.add_argument("--num-envs", type=int, default=80) + parser.add_argument("--collect-steps", type=int, default=256) + parser.add_argument("--batch-size", type=int, default=512) + parser.add_argument("--horizons", type=int, nargs="+", default=[1, 8, 16, 32]) + parser.add_argument("--seed", type=int, default=10042) + parser.add_argument("--motion-path", type=Path) + parser.add_argument("--quaternion-order", choices=("xyzw", "wxyz")) + args = parser.parse_args() + horizons = sorted(set(args.horizons)) + if min(horizons) < 1 or args.collect_steps < max(64, horizons[-1]): + parser.error( + "Collect at least 64 steps and cover the longest prediction horizon." + ) + if min(args.num_envs, args.batch_size) < 1: + parser.error("Environment and batch sizes must be positive.") + args.output.mkdir(parents=True, exist_ok=False) + saved = torch.load(args.checkpoint, map_location="cpu", weights_only=True) + recipe = normalize_recipe(saved["config"]) + config = recipe["workflow"] + gym = deepcopy(saved["gym_config"]) + options = gym["env"]["extensions"]["motion_tracking"] + if args.motion_path: + options["motion_path"] = str(args.motion_path.resolve()) + if args.quaternion_order: + options["quaternion_order"] = args.quaternion_order + options["compile_mode"] = None + (args.output / "gym.json").write_text(json.dumps(gym, indent=2) + "\n") + config.update( + kind="test", + seed=args.seed, + num_envs=args.num_envs, + buffer_steps=args.collect_steps, + compile_mode=None, + collection_env=str((args.output / "gym.json").resolve()), + evaluation_env=str((args.output / "gym.json").resolve()), + ) + recipe["trainer"]["num_envs"] = args.num_envs + recipe["trainer"]["learning_env"].get("cfg", {}).pop("compile_mode", None) + config.setdefault("task_options", {}).update(options) + workflow = MotionTraining(recipe, args.output) + try: + model, policy, env = workflow.model, workflow.policy, workflow.env + model.load_state_dict(saved["model"]["model"]) + policy.load_state_dict(saved["policy"]) + model.eval() + policy.eval() + metadata = write_run_metadata( + args.output, + invocation_started_at=datetime.now(timezone.utc).isoformat(), + config=config, + gym_config=gym, + motion=workflow.identity["motion"], + motion_path=Path(get_data_path(env.motion_cfg.motion_path)), + device=env.device, + resume_checkpoint=args.checkpoint.resolve(), + ) + start = time.perf_counter() + for _ in range(args.collect_steps): + workflow.collect( + update_normalization=False, + reference_control=args.control == "reference", + ) + collection_seconds = time.perf_counter() - start + for _ in range(8): + windows = workflow.replay.sample_windows(args.batch_size * 4, horizons[-1]) + valid = windows["valid"].all(1) & windows["usable"].all(1) + if int(valid.sum()) >= args.batch_size: + windows = {k: v[valid][: args.batch_size] for k, v in windows.items()} + break + else: + raise RuntimeError( + "Insufficient complete physical windows; increase collection steps." + ) + # Preserve the actual measured controls and contexts for every horizon. + predicted = windows["state"][:, 0] + identity = predicted.clone() + metrics = {} + prediction_started = time.perf_counter() + for step in range(horizons[-1]): + predicted = model( + predicted, windows["action"][:, step], windows["context"][:, step] + ) + if not bool(torch.isfinite(predicted).all()): + raise RuntimeError(f"Non-finite WM prediction at horizon {step + 1}") + if step + 1 not in horizons: + continue + target = windows["next_state"][:, step] + reference = env.motion.frame(windows["frame"][:, step] + 1) + actual_state = unpack_robot_state(target) + actual_reward, _ = env.task.reward( + actual_state, + reference, + windows["policy_action"][:, step], + windows["previous_action"][:, step], + ) + actual_done, actual_continuation = env.task.termination( + actual_state, reference + ) + row = {} + for label, state in (("model", predicted), ("state_hold", identity)): + error = model.state_error(state, target) + joints = env.task.dof + measured = unpack_robot_state(state) + reward, _ = env.task.reward( + measured, + reference, + windows["policy_action"][:, step], + windows["previous_action"][:, step], + ) + done, continuation = env.task.termination(measured, reference) + row[label] = { + "normalized_mse": float( + (error / model.error_scale()).square().mean() + ), + "root_position_m": float(error[:, :3].norm(dim=-1).mean()), + "root_rotation_rad": float(error[:, 3:6].norm(dim=-1).mean()), + "joint_position_rmse_rad": float( + error[:, 12 : 12 + joints].square().mean().sqrt() + ), + "joint_velocity_rmse_rad_s": float( + error[:, 12 + joints :].square().mean().sqrt() + ), + "normalized_reward_mae": float( + (reward - actual_reward).abs().mean() / env.task.max_step_reward + ), + "hard_termination_disagreement": float( + (done != actual_done).float().mean() + ), + "continuation_mae": float( + (continuation - actual_continuation).abs().mean() + ), + } + row["physical_terminal_count"] = int(actual_done.sum()) + metrics[str(step + 1)] = row + unchanged = all( + torch.equal(value.cpu(), saved["model"]["model"][key]) + for key, value in model.state_dict().items() + if isinstance(value, torch.Tensor) + ) + policy_unchanged = all( + torch.equal(value.cpu(), saved["policy"][key]) + for key, value in policy.state_dict().items() + if isinstance(value, torch.Tensor) + ) + assert ( + unchanged and policy_unchanged + ), "Validation changed weights or normalization statistics." + report = { + "kind": "validation", + "checkpoint": str(args.checkpoint.resolve()), + "environment_metadata": metadata.name, + "control": args.control, + "seed": args.seed, + "collection_steps": args.collect_steps, + "num_envs": args.num_envs, + "windows": args.batch_size, + "sampled_clip_counts": { + env.motion.names[i]: int(n) + for i, n in enumerate( + torch.bincount( + env.motion.clip_ids(windows["frame"][:, 0]), + minlength=len(env.motion.names), + ) + .cpu() + .tolist() + ) + }, + "horizons": horizons, + "prediction_precision": "Checkpoint network precision; FP32 state integration and errors", + "normalization_floor": None, + "split": "Fresh physical episodes from the frozen checkpoint; no fitting or normalization updates. Complete windows respect episode boundaries.", + "model_and_statistics_unchanged": unchanged, + "policy_and_statistics_unchanged": policy_unchanged, + "invalid_transitions": workflow.invalid_transitions, + "collection_seconds": collection_seconds, + "prediction_seconds": time.perf_counter() - prediction_started, + "metrics": metrics, + } + (args.output / "result.json").write_text(json.dumps(report, indent=2) + "\n") + print(json.dumps(report, indent=2), flush=True) + finally: + workflow.close() + + +if __name__ == "__main__": + main() diff --git a/examples/learning/motion_tracking/fit_world_model.py b/examples/learning/motion_tracking/fit_world_model.py new file mode 100644 index 0000000000..3294263e1f --- /dev/null +++ b/examples/learning/motion_tracking/fit_world_model.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. +# ---------------------------------------------------------------------------- + +"""Collect G1 physical transitions and fit an eight-step world model through shared SL.""" + +from __future__ import annotations + +import argparse +import json +import time +from pathlib import Path + +import torch +import yaml + +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.learning.sl import SupervisedBatch, SupervisedLearner +from embodichain_models.dynamics.rwm import ( + KinematicWorldModel, + KinematicWorldModelCfg, + TransitionReplay, + RobotWorldModelObjective, + replay_supervised_batch, +) +from embodichain_models.dynamics.rwm.robot_state import pack_robot_state +from embodichain_tasks.locomotion.tracking.g1_motion import UnitreeG1MotionEnv + + +def sample_batch( + replay: TransitionReplay, batch_size: int, horizon: int +) -> SupervisedBatch: + """Draw complete usable windows with bounded retries and a fixed training batch.""" + for _ in range(4): + windows = replay.sample_windows(batch_size * 4, horizon) + selected = windows["valid"].all(1) & windows["usable"].all(1) + if int(selected.sum()) >= batch_size: + return replay_supervised_batch( + {k: v[selected][:batch_size] for k, v in windows.items()}, + complete_windows=True, + ) + raise RuntimeError( + "Insufficient complete windows; collect longer episodes or reduce the horizon" + ) + + +@torch.no_grad() +def prediction_metrics( + model: KinematicWorldModel, batch: SupervisedBatch, *, identity: bool = False +) -> dict[str, float]: + """Evaluate fixed held-out episodes without updating normalizer statistics.""" + state = batch.inputs["initial_state"] + values = {} + for step in range(batch.inputs["actions"].shape[1]): + if not identity: + state = model( + state, + batch.inputs["actions"][:, step], + batch.inputs["contexts"][:, step], + ) + error = model.state_error(state, batch.targets[:, step]) + if step in (0, batch.inputs["actions"].shape[1] - 1): + prefix = f"step_{step+1}" + joints = (state.shape[-1] - 18) // 2 + values.update( + { + f"{prefix}/normalized_mse": float( + (error / model.error_scale()).square().mean() + ), + f"{prefix}/root_position_m": float( + error[:, :3].norm(dim=-1).mean() + ), + f"{prefix}/root_rotation_rad": float( + error[:, 3:6].norm(dim=-1).mean() + ), + f"{prefix}/joint_position_rmse_rad": float( + error[:, 12 : 12 + joints].square().mean().sqrt() + ), + f"{prefix}/joint_velocity_rmse_rad_s": float( + error[:, 12 + joints :].square().mean().sqrt() + ), + } + ) + return values + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--config", type=Path, required=True, help="G1 Gym deployment YAML" + ) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--num-envs", type=int, default=160) + parser.add_argument("--eval-envs", type=int, default=32) + parser.add_argument("--collect-steps", type=int, default=512) + parser.add_argument("--updates", type=int, default=500) + parser.add_argument("--batch-size", type=int, default=128) + parser.add_argument("--eval-batch-size", type=int, default=512) + parser.add_argument("--horizon", type=int, default=8) + parser.add_argument( + "--hidden-dims", type=int, nargs="+", default=[2048, 2048, 2048] + ) + parser.add_argument( + "--weights", + type=Path, + default=Path(__file__).with_name("world_model_weights.yaml"), + ) + parser.add_argument("--seed", type=int, default=0) + parser.add_argument( + "--fp32", action="store_true", help="Disable BF16 network arithmetic" + ) + args = parser.parse_args() + if ( + not 0 < args.eval_envs < args.num_envs + or args.collect_steps < args.horizon + or args.updates < 1 + ): + parser.error( + "Need nonempty train/evaluation partitions, enough collection steps and positive updates" + ) + args.output.mkdir(parents=True, exist_ok=False) + torch.manual_seed(args.seed) + torch.backends.cuda.matmul.allow_tf32 = False + config = yaml.safe_load(args.config.read_text()) + config["num_envs"] = args.num_envs + cfg = config_to_cfg(config) + cfg.init_rollout_buffer = False + env = UnitreeG1MotionEnv(cfg) + try: + env.reset(seed=args.seed) + task = env.task + wm_cfg = KinematicWorldModelCfg( + joint_names=env.motion_joint_names, + resting_position=tuple(task.resting_position.cpu().tolist()), + action_dim=task.dof, + context_dim=task.dof + 4, + hidden_dims=tuple(args.hidden_dims), + step_dt=env.step_dt, + amp_bfloat16=not args.fp32, + ) + model = KinematicWorldModel(Path(cfg.robot.fpath).read_text(), wm_cfg).to( + env.device + ) + table = yaml.safe_load(args.weights.read_text()) + if set(table) != set(env.motion_joint_names): + raise ValueError( + "Joint error weights must cover the configured joint names" + ) + joint_weights = torch.tensor( + [table[name] for name in env.motion_joint_names], device=env.device + ).clamp(0.25, 4) + joint_weights = joint_weights / joint_weights.mean() + weights = torch.cat((joint_weights.new_ones(12), joint_weights, joint_weights)) + objective = RobotWorldModelObjective( + None, temporal_reduction="sum", error_weights=tuple(weights.cpu().tolist()) + ) + learner = SupervisedLearner( + model, + objective, + torch.optim.Adam(model.parameters(), lr=2e-4), + max_grad_norm=1.0, + ) + extras = { + "frame": (None, torch.int64), + "policy_action": (task.dof, torch.float32), + "previous_action": (task.dof, torch.float32), + "usable": (None, torch.bool), + } + train_envs = args.num_envs - args.eval_envs + train = TransitionReplay( + args.collect_steps, + train_envs, + model.state_dim, + task.dof, + context_dim=wm_cfg.context_dim, + device=env.device, + seed=args.seed + 1, + extra_fields=extras, + ) + holdout = TransitionReplay( + args.collect_steps, + args.eval_envs, + model.state_dim, + task.dof, + context_dim=wm_cfg.context_dim, + device=env.device, + seed=args.seed + 2, + extra_fields=extras, + ) + manifest = { + "args": { + k: str(v) if isinstance(v, Path) else v for k, v in vars(args).items() + }, + "model": wm_cfg.to_dict(), + "error_weights": weights.cpu().tolist(), + "parameter_count": sum(p.numel() for p in model.parameters()), + "collection": "reference joint-position commands with 0.1 Gaussian action noise in 75% of environments", + "holdout": "last eval_envs worlds; their episodes never enter training or normalization", + "replay_bytes": train.allocated_bytes + holdout.allocated_bytes, + } + (args.output / "run.json").write_text(json.dumps(manifest, indent=2) + "\n") + begin = time.perf_counter() + invalid = 0 + with torch.no_grad(): + for step in range(args.collect_steps): + before = env.capture_transition_start() + reference = env.motion.future( + env.motion_frame, task.future_offsets.new_ones(1) + )["q"][:, 0] + action = ( + (reference - task.resting_position) + / task.action_scale + * (1 - task.reference_weight) + ) + noise = torch.randn_like(action) * 0.1 + noise[: round(args.num_envs * 0.25)] = 0 + _, _, terminated, truncated, info = env.step(action + noise) + after = info["transition"] + state, successor = pack_robot_state(before["state"]), pack_robot_state( + after["state"] + ) + usable = after["usable"] & torch.isfinite(state).all(-1) + invalid += int((~usable).sum()) + payload = { + "frame": before["frame"], + "previous_action": before["previous_action"], + "policy_action": after["action"], + "usable": usable, + } + for replay, rows in ( + (train, slice(None, train_envs)), + (holdout, slice(train_envs, None)), + ): + replay.append( + state[rows], + after["setpoint_action"][rows], + successor[rows], + context=before["context"][rows], + terminated=terminated[rows], + truncated=truncated[rows], + extras={k: v[rows] for k, v in payload.items()}, + ) + valid_train = usable[:train_envs] + model.update_normalization( + state[:train_envs][valid_train], + after["setpoint_action"][:train_envs][valid_train], + successor[:train_envs][valid_train], + before["context"][:train_envs][valid_train], + ) + if (step + 1) % 64 == 0: + print( + json.dumps( + { + "collected_vector_steps": step + 1, + "invalid_transitions": invalid, + } + ), + flush=True, + ) + torch.cuda.synchronize() + collection_seconds = time.perf_counter() - begin + validation = sample_batch(holdout, args.eval_batch_size, args.horizon) + identity = prediction_metrics(model, validation, identity=True) + initial = prediction_metrics(model, validation) + print(json.dumps({"identity": identity, "initial": initial}), flush=True) + torch.cuda.reset_peak_memory_stats() + begin = time.perf_counter() + with (args.output / "metrics.jsonl").open("w") as stream: + for update in range(args.updates): + metrics = learner.update( + sample_batch(train, args.batch_size, args.horizon) + ) + if (update + 1) % 50 == 0 or update == 0: + metrics["evaluation"] = prediction_metrics(model, validation) + print(json.dumps(metrics), flush=True) + stream.write(json.dumps(metrics) + "\n") + stream.flush() + torch.cuda.synchronize() + training_seconds = time.perf_counter() - begin + final = prediction_metrics(model, validation) + report = { + "identity": identity, + "initial": initial, + "final": final, + "updates": learner.num_updates, + "train_transitions": args.collect_steps * train_envs, + "holdout_transitions": args.collect_steps * args.eval_envs, + "invalid_transitions": invalid, + "collection_seconds": collection_seconds, + "training_seconds": training_seconds, + "peak_allocated_bytes": torch.cuda.max_memory_allocated(), + "replay_bytes": manifest["replay_bytes"], + "normalization_count": int(model.input_normalizer.count), + "improved_over_identity": final[f"step_{args.horizon}/normalized_mse"] + < identity[f"step_{args.horizon}/normalized_mse"], + "improved_over_initial": final[f"step_{args.horizon}/normalized_mse"] + < initial[f"step_{args.horizon}/normalized_mse"], + } + torch.save( + {"learner": learner.state_dict(), "recipe": manifest}, + args.output / "world_model.pt", + ) + (args.output / "result.json").write_text(json.dumps(report, indent=2) + "\n") + print(json.dumps(report, indent=2), flush=True) + if not report["improved_over_identity"] or not report["improved_over_initial"]: + raise RuntimeError("Held-out eight-step prediction did not improve") + finally: + env.close(exit_process=False) + + +if __name__ == "__main__": + main() diff --git a/examples/learning/motion_tracking/prepare_lafan1.py b/examples/learning/motion_tracking/prepare_lafan1.py new file mode 100644 index 0000000000..c668b1cf27 --- /dev/null +++ b/examples/learning/motion_tracking/prepare_lafan1.py @@ -0,0 +1,161 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Download and prepare LAFAN1 G1 references for the official tracking task.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path +import shutil + +from embodichain.data_pipeline.datasets import MotionLibrary +from embodichain.data.constants import EMBODICHAIN_DEFAULT_DATA_ROOT +from embodichain_robots.locomotion_profiles import load_locomotion_robot_profile + +_REPOSITORY = "lvhaidong/LAFAN1_Retargeting_Dataset" +_REVISION = "ce1572906efe6157840e8474d5a0d7aa87481e74" + + +def _download_source(cache_dir: Path | None) -> Path: + from huggingface_hub import snapshot_download + + return Path( + snapshot_download( + repo_id=_REPOSITORY, + repo_type="dataset", + revision=_REVISION, + allow_patterns=[ + "g1/*.csv", + "robot_description/g1/**", + "README*", + "LICENSE*", + "license*", + "NOTICE*", + ], + cache_dir=cache_dir, + token=False, + ) + ) + + +def _preserve_sources(source: Path, output: Path, downloaded: bool) -> None: + notices = output / "sources" / "retargeting" + notices.mkdir(parents=True, exist_ok=True) + for pattern in ("README*", "LICENSE*", "license*", "NOTICE*"): + for path in source.glob(pattern): + if path.is_file(): + shutil.copy2(path, notices / path.name) + origin = ( + f"The input was downloaded from `{_REPOSITORY}` at revision `{_REVISION}`." + if downloaded + else "The input was supplied through `--source`; its notices are retained below." + ) + (output / "SOURCES.md").write_text( + f"""# LAFAN1 G1 sources + +{origin} + +## Motion trajectories + +- Original motion capture: [Ubisoft La Forge LAFAN1](https://github.com/ubisoft/ubisoft-laforge-animation-dataset). +- Original data license: [CC BY-NC-ND 4.0](https://github.com/ubisoft/ubisoft-laforge-animation-dataset/blob/master/license.txt). +- Retargeting project: [Unitree Robotics LAFAN1 Retargeting Dataset](https://www.modelscope.cn/datasets/unitreerobotics/LAFAN1_Retargeting_Dataset). +- Download mirror: [lvhaidong/LAFAN1_Retargeting_Dataset](https://huggingface.co/datasets/{_REPOSITORY}/tree/{_REVISION}). +- Pinned download revision: `{_REVISION}`. +- The retargeting README describes Interaction Mesh and inverse kinematics and identifies the original motion-data license as CC BY-NC-ND 4.0. + +Preparation resamples the 30 Hz G1 joint trajectories to 50 Hz, estimates velocities, +and stores the root orientation in XYZW order. Clip names and joint names are recorded +in `motion.npz`. + +## Robot description + +`robot/` is copied from the input's `robot_description/g1/`. The upstream root +`LICENSE` names Unitree Robotics and contains BSD 3-Clause terms. Available input +README and license files are copied unchanged into `sources/retargeting/`; robot-local +notices remain alongside the copied files. The upstream README identifies the motion-data +license separately from the robot and code notices. +""", + encoding="utf-8", + ) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--source", + type=Path, + help=( + "Use an existing root containing g1/ and robot_description/g1/. " + "Otherwise download the pinned public dataset revision." + ), + ) + parser.add_argument( + "--cache-dir", type=Path, help="Hugging Face download cache directory." + ) + parser.add_argument( + "--output", type=Path, default=Path(EMBODICHAIN_DEFAULT_DATA_ROOT) / "LAFAN1G1" + ) + args = parser.parse_args() + source = ( + args.source.expanduser() + if args.source is not None + else _download_source(args.cache_dir) + ) + args.output = args.output.expanduser() + names = load_locomotion_robot_profile("UnitreeG1")["robot"]["joint_names"] + robot = source / "robot_description/g1" + if not (robot / "g1_29dof_rev_1_0.urdf").is_file(): + parser.error(f"G1 robot description is missing from {robot}") + library = MotionLibrary.from_csv(sorted((source / "g1").glob("*.csv")), names) + args.output.mkdir(parents=True, exist_ok=True) + library.save_npz(args.output / "motion.npz") + shutil.copytree(robot, args.output / "robot", dirs_exist_ok=True) + _preserve_sources(source, args.output, args.source is None) + metadata = { + "source": ( + {"repo_id": _REPOSITORY, "revision": _REVISION} + if args.source is None + else {"kind": "local_directory"} + ), + "clips": library.num_clips, + "frames": library.num_frames, + "joints": len(names), + "source_fps": 30.0, + "fps": library.fps, + "quaternion_order": "xyzw", + "motion_file": "motion.npz", + "robot_file": "robot/g1_29dof_rev_1_0.urdf", + "source_notices": "sources/retargeting", + } + (args.output / "preparation.json").write_text( + json.dumps(metadata, indent=2) + "\n", encoding="utf-8" + ) + print( + json.dumps( + { + **metadata, + "output": str(args.output.resolve()), + }, + indent=2, + ) + ) + + +if __name__ == "__main__": + main() diff --git a/examples/learning/motion_tracking/replay.py b/examples/learning/motion_tracking/replay.py new file mode 100644 index 0000000000..245a4edc69 --- /dev/null +++ b/examples/learning/motion_tracking/replay.py @@ -0,0 +1,592 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Render saved motion-tracking evaluation states with FastRT. + +The input is the self-contained NPZ written by evaluate_online.py. Recorded +successor states and their corresponding reference states are replayed without +advancing physics. Select several clips to produce a labelled video playlist. +""" + +from __future__ import annotations + +import argparse +import importlib.metadata +import json +import os +from pathlib import Path +import platform +import time + + +def build_parser() -> argparse.ArgumentParser: + """Build the playback CLI without initializing a renderer.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("trajectory", type=Path) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument( + "--asset", + default="LAFAN1G1/robot/g1_29dof_rev_1_0.urdf", + help="Prepared robot asset ID or absolute URDF path.", + ) + parser.add_argument( + "--clips", help="Comma-separated clips in playback order; default: first clip." + ) + parser.add_argument( + "--seconds", type=float, default=10.0, help="Duration per clip." + ) + parser.add_argument("--start-seconds", type=float, default=0.0) + parser.add_argument("--resolution", type=int, nargs=2, default=(1920, 1080)) + parser.add_argument("--device", default="cuda:0") + parser.add_argument("--spp", type=int, default=8) + parser.add_argument("--denoising", choices=("off", "optix", "dlss"), default="dlss") + parser.add_argument("--warmup-frames", type=int, default=32) + parser.add_argument("--ghost-offset", type=float, nargs=2, default=(0.3, 1.65)) + return parser + + +def _load(args: argparse.Namespace) -> dict: + import numpy as np + + with np.load(args.trajectory, allow_pickle=False) as data: + required = { + "state", + "reference", + "active", + "names", + "dt", + "joint_names", + "tracked_link_names", + } + missing = required - set(data.files) + if missing: + raise ValueError( + f"Trajectory lacks {sorted(missing)}; record it with evaluate_online.py --trajectories." + ) + names = data["names"].tolist() + joints = data["joint_names"].tolist() + tracked = data["tracked_link_names"].tolist() + dt = float(data["dt"]) + state, reference, active = data["state"], data["reference"], data["active"] + if not np.isfinite(dt) or dt <= 0: + raise ValueError("Trajectory dt must be finite and positive.") + if state.ndim != 3 or state.shape[1:] != (len(names), 18 + 2 * len(joints)): + raise ValueError("Trajectory states must match clip and joint counts.") + if reference.shape != state.shape or active.shape != state.shape[:2]: + raise ValueError("Reference and active arrays must match recorded states.") + selected = args.clips.split(",") if args.clips else names[:1] + if not selected or len(set(selected)) != len(selected): + raise ValueError("Select at least one clip, without duplicate names.") + clips = [] + first = round(args.start_seconds / dt) + count = max(1, round(args.seconds / dt)) + for name in selected: + if name not in names: + raise ValueError(f"Unknown clip {name!r}; available clips: {names}") + index = names.index(name) + valid = np.asarray(active[:, index], dtype=bool) + length = int(valid.sum()) + if not np.array_equal(valid, np.arange(len(valid)) < length): + raise ValueError( + f"Clip {name!r} must contain one contiguous active prefix." + ) + if first >= length: + raise ValueError(f"Clip {name!r} ends before the requested start time.") + end = min(first + count, length) + actual = np.asarray(state[first:end, index], dtype=np.float32).copy() + target = np.asarray(reference[first:end, index], dtype=np.float32).copy() + if not np.isfinite(actual).all() or not np.isfinite(target).all(): + raise ValueError(f"Clip {name!r} contains a non-finite recorded state.") + clips.append( + { + "name": name, + "state": actual, + "reference": target, + "world_actual": np.asarray(state[:length, index, :2]).copy(), + "world_reference": np.asarray(reference[:length, index, :2]).copy(), + "start": first, + } + ) + return {"clips": clips, "joint_names": joints, "tracked_links": tracked, "dt": dt} + + +def _poses(tree, states): + import numpy as np + import torch + + x = torch.as_tensor(states, dtype=tree.dtype, device=tree.device) + dof = len(tree.joint_names) + links = tree.evaluate( + x[..., :3], + x[..., 3:12].unflatten(-1, (3, 3)), + x[..., 18 : 18 + dof], + x[..., 12:15], + x[..., 15:18], + x[..., 18 + dof :], + ) + result = np.broadcast_to(np.eye(4), (*links.position.shape[:-1], 4, 4)).copy() + result[..., :3, :3] = links.rotation.cpu().numpy() + result[..., :3, 3] = links.position.cpu().numpy() + return result + + +def _caption(image, *, clip, local_frame, segment, schedule, dt): + from PIL import ImageDraw + from _replay_hud import load_font + + draw = ImageDraw.Draw(image) + width, height = image.size + scale = min(width / 1920, height / 1080) + + def xy(x, y): + return round(x * scale), round(y * scale) + + font, small = load_font(max(1, round(23 * scale))), load_font( + max(1, round(19 * scale)) + ) + header, footer = round(54 * scale), round(86 * scale) + draw.rectangle((0, 0, width, header), fill="#18232e") + source_time = (clip["start"] + local_frame + 1) * dt + draw.text( + xy(20, 13), + f"G1 | {clip['name']} | source {source_time:.2f} s", + font=font, + fill="#f2f6f8", + ) + draw.text( + (width - xy(20, 0)[0], xy(0, 15)[1]), + "EmbodiChain + FastRT", + font=small, + fill="#b9c9d3", + anchor="ra", + ) + top = height - footer + draw.rectangle((0, top, width, height), fill="#18232e") + draw.text( + (xy(20, 0)[0], top + xy(0, 10)[1]), + f"Clip {segment + 1}/{len(schedule)}: {clip['name']}", + font=font, + fill="#f2f6f8", + ) + remaining = (schedule[segment]["frames"] - local_frame - 1) * dt + next_label = "Next clip" if segment + 1 < len(schedule) else "End" + draw.text( + (width - xy(20, 0)[0], top + xy(0, 13)[1]), + f"{next_label} in {remaining:.1f} s", + font=small, + fill="#b9c9d3", + anchor="ra", + ) + draw.text( + (xy(20, 0)[0], top + xy(0, 42)[1]), + "Tracking | independent evaluation clips", + font=small, + fill="#93d5b0", + ) + draw.text( + (width - xy(20, 0)[0], top + xy(0, 42)[1]), + "Red: policy points Green: reference points / robot", + font=small, + fill="#b9c9d3", + anchor="ra", + ) + total = sum(item["frames"] for item in schedule) + offset, margin = 0, xy(20, 0)[0] + for index, item in enumerate(schedule): + left = margin + (width - 2 * margin) * offset / total + right = margin + (width - 2 * margin) * (offset + item["frames"]) / total + draw.rectangle( + (left, height - xy(0, 12)[1], right - 2, height - xy(0, 8)[1]), + fill="#426075", + ) + completed = ( + item["frames"] + if index < segment + else local_frame + 1 if index == segment else 0 + ) + if completed: + end = left + (right - left) * completed / item["frames"] + draw.rectangle( + (left, height - xy(0, 12)[1], max(left, end - 1), height - xy(0, 8)[1]), + fill="#62c697", + ) + offset += item["frames"] + + +def main() -> None: + """Render recorded clips and save the video with its playback metadata.""" + args = build_parser().parse_args() + import math + + if not math.isfinite(args.seconds) or args.seconds <= 0: + raise ValueError("--seconds must be finite and positive.") + if not math.isfinite(args.start_seconds) or args.start_seconds < 0: + raise ValueError("--start-seconds must be finite and nonnegative.") + if min(args.resolution) < 2 or args.spp < 1 or args.warmup_frames < 0: + raise ValueError("Resolution, spp and warmup must have valid positive sizes.") + if any(value % 2 for value in args.resolution): + raise ValueError("Video resolution dimensions must be even.") + if not all(math.isfinite(value) for value in args.ghost_offset): + raise ValueError("Ghost offsets must be finite.") + recording = _load(args) + if platform.system() == "Linux" and not os.environ.get("DISPLAY"): + os.environ.setdefault("SDL_VIDEODRIVER", "offscreen") + + import imageio.v2 as imageio + import numpy as np + import torch + from PIL import Image + from _replay_hud import TrajectoryHUD + from _replay_visuals import FloorTrajectory + from dexsim import spawn + from embodichain.compute.kinematics import TreeKinematics + from embodichain.compute.kinematics.features import heading_rotation + from embodichain.data import get_data_path + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import ( + ArticulationRootPropertiesCfg, + DenoisingCfg, + DLSSCfg, + LightCfg, + NewtonPhysicsCfg, + RenderCfg, + RobotCfg, + VisualObjectCfg, + ) + from embodichain.lab.sim.material import VisualMaterialCfg + from embodichain.lab.sim.shapes import MeshCfg + from embodichain.utils.math import quat_from_matrix + from embodichain.lab.visualization import VisualizationCfg + from embodichain.lab.visualization.markers import ( + MarkerPrototypeCfg, + RobotMarkers, + RobotMarkersAppearanceCfg, + RobotMarkersCfg, + VisualizationMarkers, + VisualizationMarkersCfg, + ) + + torch.set_num_threads(4) + asset = Path(get_data_path(args.asset)) + tree = TreeKinematics(asset.read_text(), recording["joint_names"]) + tracked = [tree.link_names.index(name) for name in recording["tracked_links"]] + if not tracked: + raise ValueError("Trajectory must identify at least one tracking link.") + args.output.parent.mkdir(parents=True, exist_ok=True) + device = torch.device(args.device) + width, height = args.resolution + sim = SimulationManager( + SimulationManagerCfg( + num_envs=1, + headless=True, + device=str(device), + gpu_id=device.index, + arena_space=0.0, + width=width, + height=height, + enable_entity_gizmo=False, + visualization=VisualizationCfg(backend="none"), + render_cfg=RenderCfg( + renderer="fast-rt", + spp=args.spp, + denoising=DenoisingCfg(window=args.denoising, offscreen=args.denoising), + dlss=DLSSCfg(dlss_quality=5), + ), + physics_cfg=NewtonPhysicsCfg( + device=str(device), + sync_to_renderer=False, + solver_cfg={"solver_type": "mujoco_warp"}, + ), + ) + ) + camera = None + robot = None + robots = [] + balls = [] + floor_trajectory = None + hud_metadata = [] + started = time.perf_counter() + try: + grid_texture = args.output.with_suffix(".floor.png") + pixels = np.arange(512, dtype=np.float32) + 0.5 + minor = np.minimum(pixels % 128, 128 - pixels % 128) < 2 + major = np.minimum(pixels % 256, 256 - pixels % 256) < 3 + tile = np.empty((512, 512, 3), dtype=np.uint8) + tile[:] = (175, 182, 188) + tile[minor[:, None] | minor[None, :]] = (136, 148, 158) + tile[major[:, None] | major[None, :]] = (124, 137, 148) + Image.fromarray(tile).save(grid_texture) + floor_mesh = args.output.with_suffix(".floor.obj") + floor_mesh.write_text( + "v -100 -100 0\nv 100 -100 0\nv 100 100 0\nv -100 100 0\n" + "vt 0 0\nvt 100 0\nvt 100 100\nvt 0 100\nvn 0 0 1\n" + "f 1/1/1 2/2/1 3/3/1\nf 1/1/1 3/3/1 4/4/1\n" + ) + sim.add_visual_object( + VisualObjectCfg( + uid="replay_floor", + init_pos=(0, 0, -0.005), + shape=MeshCfg( + fpath=str(floor_mesh.resolve()), + visual_material=VisualMaterialCfg( + base_color=[1, 1, 1, 1], + roughness=1.0, + base_color_texture=str(grid_texture.resolve()), + ), + ), + ) + ) + sim.add_light( + LightCfg( + uid="replay_key", + light_type="direction", + direction=(-0.4, -0.5, -1), + intensity=3.0, + ) + ) + robot = sim.add_robot( + RobotCfg( + uid="recorded_policy", + fpath=str(asset), + root_props=ArticulationRootPropertiesCfg(fixed_base=False), + ) + ) + sim.prepare() + joint_order = [ + recording["joint_names"].index(name) for name in robot.joint_names + ] + recorded_qpos = torch.as_tensor( + np.concatenate( + [ + clip["state"][:, 18 : 18 + len(joint_order)] + for clip in recording["clips"] + ] + )[:, joint_order], + dtype=torch.float32, + device=device, + ) + # Preserve recorded excursions beyond the source's soft joint limits. + limits = robot.get_qpos_limits().clone() + limits[..., 0] = torch.minimum(limits[..., 0], recorded_qpos.amin(0) - 1e-5) + limits[..., 1] = torch.maximum(limits[..., 1], recorded_qpos.amax(0) + 1e-5) + robot.set_qpos_limits(limits) + sim.set_ground_plane_visibility(False) + robots = [ + RobotMarkers( + RobotMarkersCfg( + name="reference_ghost", + asset_path=str(asset), + appearance=RobotMarkersAppearanceCfg( + mode="override", + color=(0.08, 0.83, 0.24), + opacity=1.0, + unlit=False, + ), + ), + sim, + ) + ] + visual_links = [tree.link_names.index(name) for name in robots[0].link_names] + colors = ((1.0, 0.03, 0.03, 1.0), (0.02, 1.0, 0.08, 1.0)) + balls = [ + VisualizationMarkers( + VisualizationMarkersCfg( + prim_path=f"tracking_{index}", + markers={ + "point": MarkerPrototypeCfg( + shape="sphere", scale=(0.044, 0.044, 0.044), color=color + ) + }, + ), + sim, + ) + for index, color in enumerate(colors) + ] + camera = sim.spawn_result.add_camera( + spawn.CameraDesc(name="motion_replay", resolution=(width, height)) + ) + camera.open_camera() + camera.set_visual_overlay_enabled(True) + dt = recording["dt"] + schedule = [ + { + "clip": item["name"], + "frames": len(item["state"]), + "source_start_s": (item["start"] + 1) * dt, + } + for item in recording["clips"] + ] + frames = 0 + with imageio.get_writer( + str(args.output), + fps=1 / dt, + codec="libx264", + macro_block_size=None, + ffmpeg_params=[ + "-crf", + "16", + "-preset", + "medium", + "-threads", + "4", + "-movflags", + "+faststart", + ], + ) as writer: + for segment, clip in enumerate(recording["clips"]): + actual, target = clip["state"], clip["reference"] + poses = (_poses(tree, actual), _poses(tree, target)) + root_rotation = torch.from_numpy(actual[:, 3:12].reshape(-1, 3, 3)) + root_pose = torch.cat( + (torch.from_numpy(actual[:, :3]), quat_from_matrix(root_rotation)), + dim=-1, + ).to(device) + qpos = torch.as_tensor( + actual[:, 18 : 18 + len(joint_order)][:, joint_order], device=device + ) + target_rotation = torch.from_numpy(target[:, 3:12].reshape(-1, 3, 3)) + alignment = ( + heading_rotation(root_rotation) + @ heading_rotation(target_rotation).transpose(-1, -2) + ).numpy() + root_a, root_r = actual[:, :3].copy(), target[:, :3].copy() + root_a[:, 2] = root_r[:, 2] = 0 + translation = root_a - np.einsum("fij,fj->fi", alignment, root_r) + aligned = poses[1].copy() + aligned[..., :3, :3] = alignment[:, None] @ aligned[..., :3, :3] + aligned[..., :3, 3] = ( + np.einsum("fij,flj->fli", alignment, aligned[..., :3, 3]) + + translation[:, None] + ) + ghost = aligned.copy() + ghost[..., :2, 3] += args.ghost_offset + hud = TrajectoryHUD( + clip["world_actual"], clip["world_reference"], dt=dt + ) + hud_metadata.append({"clip": clip["name"], **hud.metadata()}) + floor_trajectory = FloorTrajectory( + sim, clip["world_reference"], clip["world_actual"], dt=dt + ) + body_error = np.sqrt( + np.mean( + np.sum( + ( + poses[0][..., :3, 3][:, tracked] + - aligned[..., :3, 3][:, tracked] + ) + ** 2, + axis=-1, + ), + axis=-1, + ) + ) + for frame in range(len(actual)): + floor_trajectory.update(clip["start"] + frame) + robot.set_state( + root_pose=root_pose[frame : frame + 1], + qpos=qpos[frame : frame + 1], + ) + robots[0].visualize_link_poses( + ghost[frame : frame + 1, visual_links] + ) + balls[0].visualize(translations=poses[0][frame, tracked, :3, 3]) + balls[1].visualize(translations=aligned[frame, tracked, :3, 3]) + center = np.array( + [ + actual[frame, 0] + args.ghost_offset[0] / 2, + actual[frame, 1] + args.ghost_offset[1] / 2, + 0.55, + ] + ) + camera.look_at( + (center + [3.1, -3.1, 1.7]).tolist(), center.tolist(), [0, 0, 1] + ) + sim.sync_render_state() + if frame == 0: + for _ in range(args.warmup_frames): + camera.render() + camera.render() + rgb = np.array(camera.get_rgb_map(), copy=True)[..., :3] + if np.issubdtype(rgb.dtype, np.floating): + rgb = (np.clip(rgb, 0, 1) * 255).astype(np.uint8) + image = Image.fromarray(rgb) + _caption( + image, + clip=clip, + local_frame=frame, + segment=segment, + schedule=schedule, + dt=dt, + ) + hud.draw(image, clip["start"] + frame, float(body_error[frame])) + writer.append_data(np.asarray(image)) + if frames == 0: + image.save(args.output.with_suffix(".png")) + frames += 1 + floor_trajectory.close() + floor_trajectory = None + versions = {} + for name in ("embodichain", "torch", "numpy", "imageio", "warp-lang"): + versions[name] = importlib.metadata.version(name) + result = { + "video": str(args.output), + "trajectory": str(args.trajectory), + "asset": str(asset), + "frames": frames, + "fps": 1 / dt, + "resolution": [width, height], + "renderer": "fast-rt", + "denoising": args.denoising, + "spp": args.spp, + "physics_steps": 0, + "floor_grid_spacing_m": 0.5, + "mode": "saved-state playback", + "actual_renderer": "scene Robot with source materials", + "reference_renderer": "opaque green RobotMarkers with scene lighting", + "visual_style": "compact-world-xy-floor-trails-v1", + "world_xy_hud": hud_metadata, + "floor_trajectory": { + "history_seconds": 3, + "future_seconds": 2, + "coordinates": "unaligned world root XY", + }, + "schedule": schedule, + "playlist_boundary": "Each selected clip starts from its recorded evaluation state.", + "reference_alignment": "Root heading and horizontal position; display-only ghost offset.", + "ghost_offset_xy_m": args.ghost_offset, + "tracked_links": recording["tracked_links"], + "tracking_colors": {"actual": "red", "reference": "green"}, + "device": str(device), + "gpu": torch.cuda.get_device_name(device), + "python": platform.python_version(), + "software": versions, + "seconds": time.perf_counter() - started, + } + args.output.with_suffix(".json").write_text(json.dumps(result, indent=2) + "\n") + print(json.dumps(result, indent=2)) + finally: + if floor_trajectory is not None: + floor_trajectory.close() + camera = None + balls.clear() + robots.clear() + robot = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + main() diff --git a/examples/learning/motion_tracking/run_metadata.py b/examples/learning/motion_tracking/run_metadata.py new file mode 100644 index 0000000000..012c751fb4 --- /dev/null +++ b/examples/learning/motion_tracking/run_metadata.py @@ -0,0 +1,213 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Record the inputs and execution environment of one motion-training invocation.""" + +from __future__ import annotations + +import csv +from datetime import datetime +import io +import json +import os +from pathlib import Path +import platform +import subprocess +import sys +from typing import Any + +import torch + +__all__ = ["write_run_metadata"] + + +def _command(args: list[str]) -> str | None: + try: + result = subprocess.run( + args, capture_output=True, text=True, timeout=3, check=False + ) + except (OSError, subprocess.TimeoutExpired): + return None + return result.stdout.strip() if result.returncode == 0 else None + + +def _software() -> dict[str, Any]: + modules = {} + for name in ( + "embodichain", + "dexsim", + "newton", + "mujoco", + "mujoco_warp", + "warp", + "torch", + "numpy", + "tensordict", + ): + module = sys.modules.get(name) + modules[name] = { + "loaded": module is not None, + "version": ( + str(getattr(module, "__version__", "unknown")) if module else None + ), + "file": getattr(module, "__file__", None), + } + return { + "python": sys.version, + "executable": sys.executable, + "platform": platform.platform(), + "cuda_runtime": torch.version.cuda, + "modules": modules, + } + + +def _hardware(device: torch.device) -> dict[str, Any]: + cpu = platform.processor() or platform.machine() + cpuinfo = Path("/proc/cpuinfo") + if cpuinfo.exists(): + cpu = next( + ( + line.split(":", 1)[1].strip() + for line in cpuinfo.read_text().splitlines() + if line.startswith("model name") + ), + cpu, + ) + memory_bytes = None + if hasattr(os, "sysconf"): + try: + memory_bytes = os.sysconf("SC_PAGE_SIZE") * os.sysconf("SC_PHYS_PAGES") + except (OSError, ValueError): + pass + result: dict[str, Any] = { + "host": platform.node(), + "cpu": cpu, + "logical_cpus": os.cpu_count(), + "ram_bytes": memory_bytes, + "device": str(device), + "cuda_visible_devices": os.environ.get("CUDA_VISIBLE_DEVICES"), + } + if device.type == "cuda": + properties = torch.cuda.get_device_properties(device) + result["selected_gpu"] = { + "logical_device": str(device), + "name": properties.name, + "memory_bytes": properties.total_memory, + "uuid": str(getattr(properties, "uuid", "unknown")), + } + inventory = _command( + [ + "nvidia-smi", + "--query-gpu=index,uuid,name,memory.total,driver_version", + "--format=csv,noheader,nounits", + ] + ) + result["physical_gpus"] = ( + [ + dict(zip(("index", "uuid", "name", "memory_mib", "driver"), row)) + for row in csv.reader(io.StringIO(inventory), skipinitialspace=True) + ] + if inventory + else None + ) + return result + + +def _source(root: Path) -> dict[str, Any]: + result: dict[str, Any] = { + "path": str(root), + "git_available": (root / ".git").exists(), + } + if result["git_available"]: + result.update( + head=_command(["git", "-C", str(root), "rev-parse", "HEAD"]), + branch=_command(["git", "-C", str(root), "branch", "--show-current"]), + working_tree_status=_command(["git", "-C", str(root), "status", "--short"]), + ) + return result + + +def write_run_metadata( + output: Path, + *, + invocation_started_at: str, + config: dict[str, Any], + gym_config: dict[str, Any], + motion: dict[str, Any], + motion_path: Path, + device: torch.device, + resume_checkpoint: Path | None, +) -> Path: + """Save one invocation's actual inputs and loaded runtime before training. + + Args: + output: Existing training output directory. + invocation_started_at: UTC ISO timestamp shared with metric records. + config: Resolved training configuration, including budget and seed. + gym_config: Resolved physical-environment configuration. + motion: Selected motion names, lengths and sample rate. + motion_path: Resolved archive used by the physical environment. + device: Actual training device. + resume_checkpoint: Successfully restored checkpoint, or None for fresh runs. + + Returns: + Path to the new metadata file. Resumed invocations retain earlier files. + """ + timestamp = datetime.fromisoformat(invocation_started_at).strftime( + "%Y%m%dT%H%M%S.%fZ" + ) + path = output / f"environment-{timestamp}.json" + result = { + "schema": 1, + "invocation_started_at": invocation_started_at, + "kind": config["kind"], + "seed": config["seed"], + "resume_checkpoint": ( + str(resume_checkpoint.resolve()) if resume_checkpoint else None + ), + "config": config, + "gym_config": gym_config, + "dataset": { + **motion, + "path": str(motion_path.resolve()), + "total_frames": sum(motion["lengths"]), + }, + "hardware": _hardware(device), + "software": _software(), + "precision": { + "network_autocast": "bfloat16" if config["amp"] else "float32", + "state": "float32", + "matmul_tf32": torch.backends.cuda.matmul.allow_tf32, + "cudnn_tf32": torch.backends.cudnn.allow_tf32, + }, + "source": _source(Path(__file__).resolve().parents[3]), + "runtime_environment": { + name: os.environ.get(name) + for name in ( + "SOURCE_ROOT", + "REFERENCE_ROOT", + "EMBODICHAIN_DATA_ROOT", + "DEXSIM_NEWTON_ARTIFACT_MODE", + "OMP_NUM_THREADS", + "OPENBLAS_NUM_THREADS", + "MKL_NUM_THREADS", + ) + }, + } + with path.open("x") as stream: + json.dump(result, stream, indent=2) + stream.write("\n") + return path diff --git a/examples/learning/motion_tracking/train.py b/examples/learning/motion_tracking/train.py new file mode 100644 index 0000000000..80655eefc3 --- /dev/null +++ b/examples/learning/motion_tracking/train.py @@ -0,0 +1,829 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Train a G1 motion policy through measured dynamics and shared APG Actor-Critic.""" + +from __future__ import annotations + +import argparse +import fcntl +from copy import deepcopy +from datetime import datetime, timezone +import json +import math +import os +from pathlib import Path +import resource +import subprocess +import sys +import time +from typing import Any + +import torch +import yaml +from embodichain.data import get_data_path +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.learning.dynamics import ( + OnlineModelBasedScheduleCfg, + OnlineModelBasedSchedule, +) +from embodichain.learning.rl.algo import build_algo +from embodichain.learning.rl.checkpoint import _atomic_write, _capture_rng, _restore_rng +from embodichain.learning.rl.differentiable_trainer import ( + DifferentiableTrainer, + DifferentiableTrainerCfg, +) +from embodichain.learning.rl.runtime import build_learning_policy_runtime +from embodichain.learning.sl import SupervisedLearner +from embodichain.utils.config_paths import resolve_config_path +from embodichain.utils.utility import load_config +from embodichain_models.dynamics.rwm import ( + KinematicWorldModel, + KinematicWorldModelCfg, + TransitionReplay, + RobotWorldModelObjective, + replay_supervised_batch, +) +from embodichain_models.dynamics.rwm.model import frozen_world_model +from embodichain_models.dynamics.rwm.robot_state import pack_robot_state +from embodichain_models.policies import motion_tracking # Register tracking policy. +from embodichain.lab.gym.utils.registration import build_env +from embodichain_tasks.locomotion.tracking import g1_motion # Register physical task. +from embodichain_tasks.locomotion.tracking.motion_model import replay_motion_starts + +from run_metadata import write_run_metadata +from _configuration import normalize_recipe, recipe_identity, validate_recipe + + +def _optimizer_steps(optimizer: torch.optim.Optimizer) -> dict[str, int]: + """Read actual Adam step ranges, including restored optimizer history.""" + steps = [ + int(state["step"]) for state in optimizer.state.values() if "step" in state + ] + return { + "minimum": min(steps, default=0), + "maximum": max(steps, default=0), + "parameters_with_state": len(steps), + } + + +class MotionTraining: + """Compose G1/SuperMimic training with shared SL and differentiable RL. + + This entry fixes the kinematic world model, objective and replay schema, + task-derived policy dimensions, and APG Actor-Critic with separate actor and + critic optimizers. Recipe names are validated before environment allocation. + """ + + def __init__(self, config: dict[str, Any], output: Path) -> None: + self.invocation_started = time.perf_counter() + self.invocation_started_at = datetime.now(timezone.utc).isoformat() + self.sampled_peak_process_gpu_bytes = 0 + self.sampled_peak_device_used_bytes = 0 + self.config, self.output = normalize_recipe(config), output + validate_recipe(self.config) + config = self.workflow = self.config["workflow"] + trainer_block = self.config["trainer"] + if ( + config["kind"] not in ("test", "convergence") + or config.get("learning_mode", "online") + not in ("online", "fixed_world_model") + or config["wm_batches"] < 0 + or ( + config.get("learning_mode", "online") == "online" + and config["wm_batches"] == 0 + ) + or ( + config.get("learning_mode") == "fixed_world_model" + and (config["wm_batches"] or config["warmup_updates"]) + ) + or config["lr_decay_iters"] <= 0 + or config["save_every"] < 0 + or not config["buffer_steps"] + >= config["prefill_steps"] + >= max(10, config["wm_horizon"]) + ): + raise ValueError("Invalid training kind, replay capacity or update budget.") + torch.manual_seed(config["seed"]) + torch.set_num_threads(4) + torch.backends.cuda.matmul.allow_tf32 = config.get("tf32", False) + torch.backends.cudnn.allow_tf32 = config.get("tf32", False) + gym = load_config(config["collection_env"]) + self.evaluation_gym_config = load_config(config["evaluation_env"]) + for name, environment in ( + ("collection_env", gym), + ("evaluation_env", self.evaluation_gym_config), + ): + if environment.get("id") != "UnitreeG1MotionRL-v1": + raise ValueError( + f"G1 motion training requires workflow.{name} to select " + f"UnitreeG1MotionRL-v1; got {environment.get('id')!r}." + ) + gym["num_envs"] = config["num_envs"] + gym["env"]["extensions"]["motion_tracking"]["clips"] = config.get("clips") + gym["env"]["extensions"]["motion_tracking"].update( + config.get("task_options", {}) + ) + gym["env"]["extensions"]["motion_tracking"].setdefault( + "compile_mode", config.get("compile_mode") + ) + self.gym_config = gym + evaluation_options = self.evaluation_gym_config["env"]["extensions"][ + "motion_tracking" + ] + collection_options = gym["env"]["extensions"]["motion_tracking"] + if ( + resolve_config_path(config["evaluation_env"]).resolve() + == resolve_config_path(config["collection_env"]).resolve() + ): + evaluation_options.update(deepcopy(collection_options)) + else: + # Independent evaluation selects its own clips, including all clips + # when the selection is omitted. Explicit options take precedence. + for name, value in collection_options.items(): + if name != "clips": + evaluation_options.setdefault(name, deepcopy(value)) + cfg = config_to_cfg(gym) + cfg.episode_context_in_info = config.setdefault( + "episode_context_in_info", False + ) + cfg.init_rollout_buffer = False + self.env = build_env(gym["id"], base_env_cfg=cfg) + self.env.reset(seed=config["seed"]) + task = self.env.task + device = self.env.device + self.model = KinematicWorldModel( + Path(cfg.robot.fpath).read_text(), + KinematicWorldModelCfg( + joint_names=self.env.motion_joint_names, + resting_position=tuple(task.resting_position.cpu().tolist()), + action_dim=task.dof, + context_dim=task.dof + 4, + hidden_dims=tuple(config["wm_hidden"]), + amp_bfloat16=config["amp"], + ), + ).to(device) + weights = yaml.safe_load( + Path(__file__).with_name("world_model_weights.yaml").read_text() + ) + w = torch.tensor( + [weights[name] for name in self.env.motion_joint_names], device=device + ).clamp(0.25, 4) + w = w / w.mean() + weights = tuple(torch.cat((w.new_ones(12), w, w)).cpu().tolist()) + self.learner = SupervisedLearner( + self.model, + RobotWorldModelObjective( + None, + temporal_reduction="sum", + error_weights=weights, + compile_mode=config.get("compile_mode"), + ), + torch.optim.Adam( + self.model.parameters(), lr=config["lr"], fused=device.type == "cuda" + ), + max_grad_norm=1.0, + ) + self.replay = TransitionReplay( + config["buffer_steps"], + self.env.num_envs, + self.model.state_dim, + task.dof, + context_dim=task.dof + 4, + device=device, + seed=config["seed"] + 1, + extra_fields={ + "frame": (None, torch.int64), + "previous_action": (task.dof, torch.float32), + "policy_action": (task.dof, torch.float32), + "usable": (None, torch.bool), + }, + ) + policy_block = deepcopy(self.config["policy"]) + policy_dimensions = { + "history_dim": task.observation_dim * task.cfg.history_steps, + "goal_dim": task.goal_dim, + "critic_dim": self.model.feature_dim + task.goal_dim + task.dof + 5, + "action_dim": task.dof, + } + for name, value in policy_dimensions.items(): + configured = policy_block.setdefault("cfg", {}).setdefault(name, value) + if configured != value: + raise ValueError( + f"Policy {name} differs from the motion task: {configured} != {value}" + ) + runtime_config = {"trainer": deepcopy(trainer_block), "policy": policy_block} + runtime_config["trainer"]["learning_env"].setdefault("cfg", {}).setdefault( + "compile_mode", config.get("compile_mode") + ) + learning_batch = int(trainer_block["num_envs"]) + self.learning_runtime = build_learning_policy_runtime( + runtime_config, + num_envs=learning_batch, + device=device, + resources={ + "model": self.model, + "motion": self.env.motion, + "task": task, + "start_provider": lambda: replay_motion_starts( + self.replay, learning_batch, task + ), + }, + ) + self.imagined, self.policy = ( + self.learning_runtime.env, + self.learning_runtime.policy, + ) + if config.get("compile_mode") is not None: + self.policy.actor.compile(mode=config["compile_mode"], fullgraph=True) + algorithm_block = self.config["algorithm"] + algorithm_cfg = deepcopy(algorithm_block.get("cfg", {})) + for name in ("optimizer", "critic_optimizer"): + kwargs = algorithm_cfg.get(name, {}).get("kwargs", {}) + if "betas" in kwargs: + kwargs["betas"] = tuple(kwargs["betas"]) + if device.type != "cuda": + for name in ("optimizer", "critic_optimizer"): + algorithm_cfg.get(name, {}).get("kwargs", {}).pop("fused", None) + self.algorithm = build_algo( + algorithm_block["name"], algorithm_cfg, self.policy, device + ) + diff_cfg = { + key: value + for key, value in trainer_block.items() + if key not in ("learning_env", "num_envs") + } + self.trainer = DifferentiableTrainer( + DifferentiableTrainerCfg(**diff_cfg), + self.imagined, + self.policy, + self.algorithm, + ) + self._optimizer_base_lrs = [ + [group["lr"] for group in optimizer.param_groups] + for optimizer in ( + self.learner.optimizer, + self.algorithm.optimizer, + self.algorithm.critic_optimizer, + ) + ] + self.schedule = OnlineModelBasedSchedule( + OnlineModelBasedScheduleCfg( + warmup_updates=config["warmup_updates"], + updates_per_collect=config["utd"], + model_updates_per_cycle=config["wm_batches"], + ) + ) + self.collection_steps = 0 + self.first_call_seconds: dict[str, float] = {} + self.phase_times = dict.fromkeys( + ( + "collect", + "wm", + "apg", + "policy_rollout_actor", + "critic", + "checkpoint", + "evaluation", + ), + 0.0, + ) + self.construction_seconds = time.perf_counter() - self.invocation_started + self.restore_seconds = 0.0 + self.resume_checkpoint: Path | None = None + self.environment_metadata: str | None = None + self.invalid_transitions = 0 + self.latest_collection = {} + self.output.mkdir(parents=True, exist_ok=True) + self._stream = (output / "metrics.jsonl").open("a") + identity_gym = deepcopy(gym) + identity_gym["env"]["extensions"]["motion_tracking"].pop("compile_mode", None) + identity_task = self.env.motion_cfg.to_dict() + identity_task.pop("compile_mode", None) + self.identity = { + "position_target_limits": "unclamped_pd", + "gym": identity_gym, + "task": identity_task, + "motion": { + "names": self.env.motion.names, + "lengths": self.env.motion.lengths.cpu().tolist(), + "fps": self.env.motion.fps, + "content_digest": self.env.motion.content_digest, + }, + "training": recipe_identity(self.config), + "wm": self.model.cfg.to_dict(), + "policy": self.policy.cfg.to_dict(), + } + + @torch.no_grad() + def collect( + self, *, update_normalization: bool = True, reference_control: bool = False + ) -> int: + """Collect measured transitions using the policy or reference joint targets. + + Args: + update_normalization: Refresh actor statistics from measured observations. + reference_control: Use reference joint targets for model diagnostics. + + Returns: + Number of measured environment transitions. + """ + start = time.perf_counter() + # Physical data are copied into replay before the next learning phase. + if self.workflow.get("compile_mode") is not None: + torch.compiler.cudagraph_mark_step_begin() + before = self.env.capture_transition_start() + obs = self.env.get_obs()["policy"] + if update_normalization: + self.policy.update_actor_normalization(obs) + if reference_control: + task = self.env.task + reference = self.env.motion.future( + self.env.motion_frame, task.future_offsets.new_ones(1) + )["q"][:, 0] + action = ( + (reference - task.resting_position) + / task.action_scale + * (1 - task.reference_weight) + ) + else: + action = self.policy.actor(obs) + noise = torch.randn_like(action) * self.workflow["action_noise"] + noise[: round(self.env.num_envs * self.workflow["collect_det_frac"])] = 0 + _, reward, terminated, truncated, info = self.env.step(action + noise) + after = info["transition"] + state = pack_robot_state(before["state"]) + successor = pack_robot_state(after["state"]) + usable = after["usable"] & torch.isfinite(state).all(-1) + self.replay.append( + state, + after["setpoint_action"], + successor, + context=before["context"], + terminated=terminated, + truncated=truncated, + extras={ + "frame": before["frame"], + "previous_action": before["previous_action"], + "policy_action": after["action"], + "usable": usable, + }, + ) + self.invalid_transitions += int((~usable).sum()) + self.collection_steps += 1 + self.latest_collection = { + "step_reward": float(reward.mean()), + "terminated_fraction": float(terminated.float().mean()), + "invalid_transitions": self.invalid_transitions, + } + elapsed = time.perf_counter() - start + self.first_call_seconds.setdefault("collect", elapsed) + self.phase_times["collect"] += elapsed + return self.env.num_envs + + def update_model(self) -> dict[str, float]: + start = time.perf_counter() + if self.workflow.get("compile_mode") is not None: + torch.compiler.cudagraph_mark_step_begin() + for _ in range(4): + windows = self.replay.sample_windows( + self.workflow["wm_batch"] * 4, self.workflow["wm_horizon"] + ) + valid = windows["valid"].all(1) & windows["usable"].all(1) + if int(valid.sum()) >= self.workflow["wm_batch"]: + windows = { + k: v[valid][: self.workflow["wm_batch"]] for k, v in windows.items() + } + break + else: + raise RuntimeError("Not enough complete usable model windows.") + warming = self.schedule.warmup_updates < self.workflow["warmup_updates"] + refresh = ( + self.learner.num_updates % self.workflow["norm_update_every"] == 0 + if warming + else ( + self.schedule.policy_updates % self.workflow["norm_update_every"] == 0 + and (self.learner.num_updates - self.workflow["warmup_updates"]) + % self.workflow["wm_batches"] + == 0 + ) + ) + if refresh: + self.model.update_normalization( + windows["state"].flatten(0, 1), + windows["action"].flatten(0, 1), + windows["next_state"].flatten(0, 1), + windows["context"].flatten(0, 1), + ) + metrics = self.learner.update( + replay_supervised_batch(windows, complete_windows=True) + ) + elapsed = time.perf_counter() - start + self.first_call_seconds.setdefault("wm", elapsed) + self.phase_times["wm"] += elapsed + return metrics + + def update_policy(self) -> dict[str, float]: + start = time.perf_counter() + if self.workflow.get("compile_mode") is not None: + torch.compiler.cudagraph_mark_step_begin() + with frozen_world_model(self.model): + metrics = self.trainer.update_once() + elapsed = time.perf_counter() - start + self.first_call_seconds.setdefault("apg", elapsed) + self.phase_times["apg"] += elapsed + self.phase_times["critic"] += metrics["critic_seconds"] + self.phase_times["policy_rollout_actor"] += elapsed - metrics["critic_seconds"] + return metrics + + def save(self, path: Path) -> None: + """Atomically save learning state; measured replay is refilled on resume.""" + started = time.perf_counter() + state = { + "schema": 1, + "identity": self.identity, + "config": self.config, + "gym_config": self.gym_config, + "evaluation_gym_config": self.evaluation_gym_config, + "model": self.learner.state_dict(), + "policy": self.policy.state_dict(), + "algorithm": self.algorithm.state_dict(), + "schedule": self.schedule.state_dict(), + "trainer": { + "global_step": self.trainer.global_step, + "num_updates": self.trainer.num_updates, + "rollout_index": self.trainer._rollout_index, + }, + "rng": _capture_rng(self.env.device), + "replay_rng": self.replay.generator.get_state(), + "imagination_rng": self.imagined._generator.get_state(), + "sampler": self.env.start_sampler.state_dict(), + "environment_rng": { + name: gen.get_state() + for name, gen in self.env._component_generators.items() + }, + "collection_steps": self.collection_steps, + "phase_times": self.phase_times, + "invalid_transitions": self.invalid_transitions, + } + _atomic_write(path, lambda stream: torch.save(state, stream)) + self.phase_times["checkpoint"] += time.perf_counter() - started + + def restore(self, path: Path) -> None: + """Validate recipe and model state before restoring a saved learning boundary.""" + started = time.perf_counter() + state = torch.load(path, map_location=self.env.device, weights_only=False) + saved_identity, current_identity = deepcopy(state["identity"]), deepcopy( + self.identity + ) + # Normalize saved recipe fields while preserving the resolved task identity. + saved_identity["training"] = recipe_identity(state["config"]) + current_identity["training"] = recipe_identity(self.config) + if state["schema"] != 1 or saved_identity != current_identity: + raise ValueError("Training checkpoint recipe or task identity differs.") + self.schedule.validate_state_dict(state["schedule"]) + self.env.start_sampler.validate_state_dict(state["sampler"]) + self.algorithm.validate_state_dict(state["algorithm"]) + candidate = deepcopy(self.policy) + candidate.load_state_dict(state["policy"]) + del candidate + candidate = deepcopy(self.learner) + candidate.load_state_dict(state["model"]) + del candidate + self.learner.load_state_dict(state["model"]) + self.policy.load_state_dict(state["policy"]) + self.algorithm.load_state_dict(state["algorithm"]) + self.schedule.load_state_dict(state["schedule"]) + self.trainer.global_step = state["trainer"]["global_step"] + self.trainer.num_updates = state["trainer"]["num_updates"] + self.trainer._rollout_index = state["trainer"]["rollout_index"] + self.collection_steps = state["collection_steps"] + self.invalid_transitions = state["invalid_transitions"] + self.replay.generator.set_state(state["replay_rng"].cpu()) + self.imagined._generator.set_state(state["imagination_rng"].cpu()) + _restore_rng(state["rng"], self.env.device) + self.env.start_sampler.load_state_dict(state["sampler"]) + for name, rng in state["environment_rng"].items(): + self.env.get_generator(name).set_state(rng.cpu()) + self.env.reset() + self.restore_seconds = time.perf_counter() - started + self.resume_checkpoint = path.resolve() + + def save_policy(self, path: Path) -> None: + """Keep evaluation weights and task metadata for each measured checkpoint.""" + started = time.perf_counter() + state = { + "schema": 1, + "purpose": "evaluation", + "identity": self.identity, + "config": self.config, + "gym_config": self.gym_config, + "evaluation_gym_config": self.evaluation_gym_config, + "policy": self.policy.state_dict(), + "schedule": self.schedule.state_dict(), + } + _atomic_write(path, lambda stream: torch.save(state, stream)) + self.phase_times["checkpoint"] += time.perf_counter() - started + + def resource_usage(self) -> dict[str, int | None]: + """Sample device-wide usage and this process's Torch/native GPU allocation.""" + free, total = torch.cuda.mem_get_info(self.env.device) + process_bytes = None + try: + output = subprocess.check_output( + [ + "nvidia-smi", + "--query-compute-apps=pid,used_gpu_memory", + "--format=csv,noheader,nounits", + ], + text=True, + stderr=subprocess.DEVNULL, + timeout=5, + ) + values = [ + int(memory.strip()) * 1024**2 + for pid, memory in (line.split(",", 1) for line in output.splitlines()) + if int(pid.strip()) == os.getpid() and memory.strip().isdigit() + ] + if values: + process_bytes = sum(values) + except (OSError, subprocess.SubprocessError, ValueError): + pass + self.sampled_peak_process_gpu_bytes = max( + self.sampled_peak_process_gpu_bytes, process_bytes or 0 + ) + self.sampled_peak_device_used_bytes = max( + self.sampled_peak_device_used_bytes, total - free + ) + return { + "process_gpu_used_bytes": process_bytes, + "device_used_bytes": total - free, + "process_peak_rss_bytes": resource.getrusage(resource.RUSAGE_SELF).ru_maxrss + * 1024, + } + + def record(self, phase: str, metrics: dict[str, Any]) -> None: + log_resources = ( + phase != "online" + or self.schedule.iterations % self.workflow["log_every"] == 0 + ) + record = { + "phase": phase, + "kind": self.workflow["kind"], + "invocation_started_at": self.invocation_started_at, + "environment_metadata": self.environment_metadata, + "iteration": self.schedule.iterations, + "counters": self.schedule.state_dict(), + "imagined_transitions": self.trainer.global_step, + "phase_seconds": dict(self.phase_times), + "recorded_at": datetime.now(timezone.utc).isoformat(), + "invocation_seconds": time.perf_counter() - self.invocation_started, + **metrics, + } + if log_resources: + record["resources"] = self.resource_usage() + self._stream.write(json.dumps(record) + "\n") + self._stream.flush() + if log_resources: + print(json.dumps(record), flush=True) + + def evaluate(self, label: str) -> None: + """Evaluate frozen weights in a separate physical process from clip starts.""" + self.save(self.output / "latest.pt") + checkpoint = self.output / f"policy-{self.schedule.iterations:06d}.pt" + self.save_policy(checkpoint) + started = time.perf_counter() + evaluation_path = self.output / f"evaluation-{label}.json" + subprocess.run( + [ + sys.executable, + str(Path(__file__).with_name("evaluate_online.py")), + str(checkpoint), + "--output", + str(evaluation_path), + ], + check=True, + ) + elapsed = time.perf_counter() - started + self.phase_times["evaluation"] += elapsed + evaluation = json.loads(evaluation_path.read_text()) + self.record( + "evaluation", + { + "seconds": elapsed, + "macro_normalized_return": evaluation["macro_normalized_return"], + "failures": evaluation["failures"], + "path": str(evaluation_path), + }, + ) + + def run(self) -> None: + self.environment_metadata = write_run_metadata( + self.output, + invocation_started_at=self.invocation_started_at, + config=self.workflow, + gym_config=self.gym_config, + motion=self.identity["motion"], + motion_path=Path(get_data_path(self.env.motion_cfg.motion_path)), + device=self.env.device, + resume_checkpoint=self.resume_checkpoint, + ).name + config = self.workflow + run_start = time.perf_counter() + self.record( + "constructed", + { + "construction_seconds": self.construction_seconds, + "restore_seconds": self.restore_seconds, + }, + ) + while self.replay.size < config["prefill_steps"]: + self.schedule.collect(self.collect, prefill=True) + self.record( + "prefill", + {**self.latest_collection, "seconds": time.perf_counter() - run_start}, + ) + warmup_started = time.perf_counter() + while self.schedule.warmup_updates < config["warmup_updates"]: + values = self.schedule.warmup(self.update_model) + if ( + self.schedule.warmup_updates % 25 == 0 + or self.schedule.warmup_updates == config["warmup_updates"] + ): + self.record("model_warmup", values) + warmup_seconds = time.perf_counter() - warmup_started + if self.schedule.iterations == 0 and config["eval_every"]: + self.evaluate("initial") + start = time.perf_counter() + while self.schedule.iterations < config["iterations"]: + i = self.schedule.iterations + lr_factor = config["lr_final_frac"] + ( + 1 - config["lr_final_frac"] + ) * 0.5 * (1 + math.cos(math.pi * min(i / config["lr_decay_iters"], 1))) + for optimizer, base_lrs in zip( + ( + self.learner.optimizer, + self.algorithm.optimizer, + self.algorithm.critic_optimizer, + ), + self._optimizer_base_lrs, + ): + for group, base_lr in zip(optimizer.param_groups, base_lrs): + group["lr"] = base_lr * lr_factor + metrics = self.schedule.step( + self.collect, self.update_model, self.update_policy + ) + self.record( + "online", + { + **metrics, + **self.latest_collection, + "elapsed_seconds": time.perf_counter() - start, + "phase_seconds": dict(self.phase_times), + "allocated_bytes": torch.cuda.memory_allocated(), + "reserved_bytes": torch.cuda.memory_reserved(), + }, + ) + i = self.schedule.iterations + if config["save_every"] and i % config["save_every"] == 0: + self.save(self.output / "latest.pt") + self.save_policy(self.output / f"policy-{i:06d}.pt") + if config["eval_every"] and i % config["eval_every"] == 0: + self.evaluate(str(i)) + self.save(self.output / "latest.pt") + self.save_policy(self.output / f"policy-{self.schedule.iterations:06d}.pt") + if config.get("evaluate_final", bool(config["eval_every"])) and ( + not config["eval_every"] or self.schedule.iterations % config["eval_every"] + ): + self.evaluate(str(self.schedule.iterations)) + result = { + "kind": config["kind"], + "invocation_started_at": self.invocation_started_at, + "environment_metadata": self.environment_metadata, + "counters": self.schedule.state_dict(), + "imagined_transitions": self.trainer.global_step, + "phase_seconds": self.phase_times, + "optimizer_steps": { + "world_model": _optimizer_steps(self.learner.optimizer), + "actor": _optimizer_steps(self.algorithm.optimizer), + "critic": _optimizer_steps(self.algorithm.critic_optimizer), + }, + "first_call_seconds": self.first_call_seconds, + "peak_allocated_bytes": torch.cuda.max_memory_allocated(), + "peak_reserved_bytes": torch.cuda.max_memory_reserved(), + "process_peak_rss_bytes": resource.getrusage(resource.RUSAGE_SELF).ru_maxrss + * 1024, + "run_seconds": time.perf_counter() - run_start, + "warmup_seconds": warmup_seconds, + "construction_seconds": self.construction_seconds, + "restore_seconds": self.restore_seconds, + "resources_at_end": self.resource_usage(), + "sampled_peak_process_gpu_bytes": self.sampled_peak_process_gpu_bytes + or None, + "sampled_peak_device_used_bytes": self.sampled_peak_device_used_bytes, + "timing_scope": "Current invocation's host elapsed wall time; learner scalar metrics synchronize updates. APG contains policy_rollout_actor and critic. Construction and first calls include lazy runtime/kernel initialization. Group metrics by invocation_started_at to combine resumed runs.", + "memory_scope": "Torch peaks cover this invocation. Process GPU usage includes native allocations from nvidia-smi; device usage includes other processes. Sampled maxima cover phase/log boundaries, not every allocation.", + "replay_bytes": self.replay.allocated_bytes, + "invalid_transitions": self.invalid_transitions, + } + (self.output / "result.json").write_text(json.dumps(result, indent=2) + "\n") + self.record("completed", result) + + def close(self) -> None: + self._stream.close() + self.learning_runtime.close() + self.env.close(exit_process=False) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--config", type=Path, required=True) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--resume", type=Path) + parser.add_argument("--iterations", type=int) + parser.add_argument("--seed", type=int) + parser.add_argument("--motion-path", type=Path) + parser.add_argument("--quaternion-order", choices=("xyzw", "wxyz")) + parser.add_argument( + "--wm-checkpoint", + type=Path, + help="Initialize WM weights from a fitted or online-training checkpoint.", + ) + parser.add_argument( + "--evaluate-final", action=argparse.BooleanOptionalAction, default=None + ) + args = parser.parse_args() + config_path = resolve_config_path(args.config).resolve() + recipe = normalize_recipe(load_config(config_path)) + config = recipe["workflow"] + for key in ("collection_env", "evaluation_env"): + config[key] = str( + resolve_config_path(config[key], base_dir=config_path.parent).resolve() + ) + if args.evaluate_final is not None: + config["evaluate_final"] = args.evaluate_final + if args.iterations is not None: + config["iterations"] = args.iterations + if args.seed is not None: + config["seed"] = args.seed + if args.motion_path is not None: + config.setdefault("task_options", {})["motion_path"] = str( + args.motion_path.resolve() + ) + if args.quaternion_order is not None: + config.setdefault("task_options", {})[ + "quaternion_order" + ] = args.quaternion_order + if args.wm_checkpoint is not None: + config["wm_checkpoint"] = str(args.wm_checkpoint.resolve()) + if config.get("learning_mode") == "fixed_world_model" and not ( + args.resume or config.get("wm_checkpoint") + ): + parser.error("Fixed-model training requires --wm-checkpoint or --resume.") + if not args.resume and args.output.exists(): + parser.error("Use a new output directory or --resume.") + args.output.mkdir(parents=True, exist_ok=True) + with (args.output / ".run.lock").open("a") as lock: + try: + fcntl.flock(lock, fcntl.LOCK_EX | fcntl.LOCK_NB) + except BlockingIOError: + parser.error( + "This output directory already has an active training process." + ) + workflow = MotionTraining(recipe, args.output) + try: + if args.resume: + workflow.restore(args.resume) + elif config.get("wm_checkpoint"): + initial = torch.load( + config["wm_checkpoint"], + map_location=workflow.env.device, + weights_only=True, + ) + learner = ( + initial["learner"] if "learner" in initial else initial["model"] + ) + workflow.model.load_state_dict(learner["model"]) + (args.output / "config.yaml").write_text( + yaml.safe_dump(recipe, sort_keys=False) + ) + workflow.run() + finally: + workflow.close() + + +if __name__ == "__main__": + main() diff --git a/examples/learning/motion_tracking/world_model_weights.yaml b/examples/learning/motion_tracking/world_model_weights.yaml new file mode 100644 index 0000000000..deb7376c36 --- /dev/null +++ b/examples/learning/motion_tracking/world_model_weights.yaml @@ -0,0 +1,31 @@ +# Recorded joint-error weights of the frozen G1 integration baseline. +# Ordered by joint name; clamp to [0.25, 4] and normalize to mean one. +left_hip_pitch_joint: 1.3789 +left_hip_roll_joint: 1.3501 +left_hip_yaw_joint: 0.7881 +left_knee_joint: 0.8180 +left_ankle_pitch_joint: 0.3897 +left_ankle_roll_joint: 0.3896 +right_hip_pitch_joint: 1.3789 +right_hip_roll_joint: 1.3501 +right_hip_yaw_joint: 0.7881 +right_knee_joint: 0.8180 +right_ankle_pitch_joint: 0.3897 +right_ankle_roll_joint: 0.3896 +waist_yaw_joint: 2.8053 +waist_roll_joint: 2.8257 +waist_pitch_joint: 2.8090 +left_shoulder_pitch_joint: 1.2139 +left_shoulder_roll_joint: 1.1957 +left_shoulder_yaw_joint: 0.7944 +left_elbow_joint: 0.7920 +left_wrist_roll_joint: 0.3896 +left_wrist_pitch_joint: 0.3904 +left_wrist_yaw_joint: 0.3896 +right_shoulder_pitch_joint: 1.2139 +right_shoulder_roll_joint: 1.1957 +right_shoulder_yaw_joint: 0.7944 +right_elbow_joint: 0.7920 +right_wrist_roll_joint: 0.3896 +right_wrist_pitch_joint: 0.3904 +right_wrist_yaw_joint: 0.3896 diff --git a/extensions/embodichain_models/embodichain_models/dynamics/rwm/__init__.py b/extensions/embodichain_models/embodichain_models/dynamics/rwm/__init__.py index 0a1e2007a5..b2bdfbc841 100644 --- a/extensions/embodichain_models/embodichain_models/dynamics/rwm/__init__.py +++ b/extensions/embodichain_models/embodichain_models/dynamics/rwm/__init__.py @@ -22,6 +22,8 @@ from typing import Any __all__ = [ + "KinematicWorldModelCfg", + "KinematicWorldModel", "RobotWorldModelCfg", "RobotWorldModel", "frozen_world_model", @@ -33,6 +35,8 @@ ] _EXPORT_MODULES = { + "KinematicWorldModelCfg": "kinematic", + "KinematicWorldModel": "kinematic", "RobotWorldModelCfg": "model", "RobotWorldModel": "model", "frozen_world_model": "model", diff --git a/extensions/embodichain_models/embodichain_models/dynamics/rwm/kinematic.py b/extensions/embodichain_models/embodichain_models/dynamics/rwm/kinematic.py new file mode 100644 index 0000000000..1cd452324b --- /dev/null +++ b/extensions/embodichain_models/embodichain_models/dynamics/rwm/kinematic.py @@ -0,0 +1,322 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Kinematic-feature dynamics with heading-frame state increments. + +The architecture follows SuperMimic's fixed-period state prediction recipe. +Geometry and feature evaluation use EmbodiChain's differentiable tree kinematics. +""" + +from __future__ import annotations + +from contextlib import nullcontext +from typing import Any + +import torch +from torch import nn + +from embodichain.compute.kinematics import ( + TreeKinematics, + matrix_product, + matrix_vector_product, + heading_rotation, + heading_kinematic_features, +) +from embodichain.learning.models.mlp import MLP +from embodichain.learning.models.normalizer import EmpiricalNormalizer +from embodichain.utils import configclass +from .robot_state import _parts, _body, rotation_exp, rotation_log + +__all__ = [ + "KinematicWorldModelCfg", + "KinematicWorldModel", + "heading_state_increment", + "integrate_heading_state", +] + + +def heading_state_increment( + state: torch.Tensor, next_state: torch.Tensor +) -> torch.Tensor: + """Encode displacement/velocity in heading axes and angular velocity in body axes. + + Args: + state: Canonical matrix-based robot states, width ``18 + 2*J``. + next_state: Aligned states with the same shape. + + Returns: + Increments of width ``12 + 2*J``. Rotation is a right-multiplied + axis-angle increment; angular velocity compares each state's own body frame. + """ + if state.shape != next_state.shape: + raise ValueError("Current and next state shapes must match") + p, r, v, w, q, qd = _parts(state) + p1, r1, v1, w1, q1, qd1 = _parts(next_state) + h = heading_rotation(r) + return torch.cat( + ( + _body(h, p1 - p), + rotation_log(matrix_product(r.transpose(-1, -2), r1)), + _body(h, v1 - v), + _body(r1, w1) - _body(r, w), + q1 - q, + qd1 - qd, + ), + -1, + ) + + +def integrate_heading_state( + state: torch.Tensor, increment: torch.Tensor +) -> torch.Tensor: + """Integrate heading-frame increments without detaching either input. + + Args: + state: Canonical states with world root-origin velocity. + increment: Aligned increments of width ``state.shape[-1] - 6``. + + Returns: + Newly allocated canonical states with proper rotation matrices. + """ + if increment.shape != state.shape[:-1] + (state.shape[-1] - 6,): + raise ValueError("Increment shape does not match robot state") + p, r, v, w, q, qd = _parts(state) + dp, dr, dv, dw, dq, dqd = increment.split( + (3, 3, 3, 3, q.shape[-1], q.shape[-1]), -1 + ) + h = heading_rotation(r) + r1 = matrix_product(r, rotation_exp(dr)) + world = lambda rotation, value: matrix_vector_product(rotation, value) + return torch.cat( + ( + p + world(h, dp), + r1.flatten(-2), + v + world(h, dv), + world(r1, _body(r, w) + dw), + q + dq, + qd + dqd, + ), + -1, + ) + + +@configclass +class KinematicWorldModelCfg: + """Configure a fixed-period kinematic-feature dynamics network. + + Joint order, resting pose and action semantics form checkpoint identity. + The default three 2048-wide SiLU layers match the G1 integration baseline. + """ + + joint_names: tuple[str, ...] = () + resting_position: tuple[float, ...] = () + action_dim: int = 29 + context_dim: int = 33 + hidden_dims: tuple[int, ...] = (2048, 2048, 2048) + step_dt: float = 0.02 + action_semantics: str = "normalized_joint_position_setpoint" + normalization_epsilon: float = 1e-3 + amp_bfloat16: bool = False + + +class KinematicWorldModel(nn.Module): + """Predict next states from full-body features, controls and physical context. + + Args: + urdf: Robot geometry XML. Only kinematic fields enter computation. + cfg: Ordered joints, control identity and network configuration. + + Input statistics use a quadratic epsilon and clipping at ten standard + deviations. Output statistics scale the predicted 70-dimensional increment + for a 29-joint robot. Statistics change only through explicit measured-data + updates. FK geometry is cached once per device, outside repeated tensor work. + """ + + def __init__(self, urdf: str, cfg: KinematicWorldModelCfg) -> None: + super().__init__() + cfg.validate() + if ( + not cfg.joint_names + or len(set(cfg.joint_names)) != len(cfg.joint_names) + or len(cfg.resting_position) != len(cfg.joint_names) + or type(cfg.action_dim) is not int + or cfg.action_dim < 1 + or type(cfg.context_dim) is not int + or cfg.context_dim < 0 + or not cfg.hidden_dims + or any(type(w) is not int or w < 1 for w in cfg.hidden_dims) + or not 0 < cfg.step_dt < float("inf") + or not cfg.action_semantics + ): + raise ValueError("Invalid kinematic world model configuration") + self.cfg, self.urdf = cfg, urdf + self._trees: dict[torch.device, tuple[TreeKinematics, torch.Tensor]] = {} + tree, _ = self._geometry(torch.device("cpu")) + self.state_dim = 18 + 2 * len(cfg.joint_names) + self.increment_dim = self.state_dim - 6 + self.feature_dim = ( + 13 + 2 * len(cfg.joint_names) + 6 * (len(tree.moving_link_names) - 1) + ) + self.input_dim = self.feature_dim + cfg.action_dim + cfg.context_dim + self.register_buffer( + "resting_position", torch.tensor(cfg.resting_position, dtype=torch.float32) + ) + self.input_normalizer = EmpiricalNormalizer( + self.input_dim, cfg.normalization_epsilon, variance_epsilon=True, clip=10.0 + ) + self.output_normalizer = EmpiricalNormalizer( + self.increment_dim, 1e-4, variance_epsilon=True + ) + self.net = MLP( + self.input_dim, self.increment_dim, cfg.hidden_dims, activation="silu" + ) + nn.init.orthogonal_(self.net[-1].weight, gain=0.01) + nn.init.zeros_(self.net[-1].bias) + self._statistics_frozen = False + + def _geometry(self, device: torch.device) -> tuple[TreeKinematics, torch.Tensor]: + if device not in self._trees: + tree = TreeKinematics(self.urdf, self.cfg.joint_names, device=device) + self._trees[device] = tree, tree.link_indices(tree.moving_link_names[1:]) + return self._trees[device] + + def features(self, state: torch.Tensor) -> torch.Tensor: + """Return differentiable heading-frame root/joint/link features.""" + if ( + state.ndim < 2 + or state.shape[-1] != self.state_dim + or state.dtype != torch.float32 + ): + raise ValueError(f"State must be FP32 with width {self.state_dim}") + p, r, v, w, q, qd = _parts(state) + tree, moving = self._geometry(state.device) + links = tree.evaluate(p, r, q, v, w, qd) + return heading_kinematic_features( + p, + r, + v, + w, + q, + qd, + self.resting_position, + links.position.index_select(-2, moving), + links.linear_velocity.index_select(-2, moving), + ) + + def inputs( + self, + state: torch.Tensor, + action: torch.Tensor, + context: torch.Tensor | None = None, + *, + features: torch.Tensor | None = None, + ) -> torch.Tensor: + """Encode aligned states, executed setpoints and recorded physical context.""" + if context is None and self.cfg.context_dim == 0: + context = state.new_empty(state.shape[:-1] + (0,)) + for name, value, width in ( + ("action", action, self.cfg.action_dim), + ("context", context, self.cfg.context_dim), + ): + if ( + value is None + or value.shape != state.shape[:-1] + (width,) + or value.dtype != torch.float32 + or value.device != state.device + ): + raise ValueError(f"{name} must be aligned FP32 data with width {width}") + features = self.features(state) if features is None else features + if ( + features.shape != state.shape[:-1] + (self.feature_dim,) + or features.dtype != torch.float32 + or features.device != state.device + ): + raise ValueError( + "Precomputed kinematic features must be aligned FP32 tensors." + ) + return torch.cat((features, action, context), -1) + + def forward( + self, + state: torch.Tensor, + action: torch.Tensor, + context: torch.Tensor | None = None, + *, + features: torch.Tensor | None = None, + ) -> torch.Tensor: + """Advance one period; optionally reuse graph-connected task FK features. + + Precomputed features must use this model's geometry, joint order and + resting pose. Normalization and integration retain FP32 arithmetic. + """ + with torch.autocast(state.device.type, enabled=False): + normalized = self.input_normalizer( + self.inputs(state, action, context, features=features) + ) + amp = ( + torch.autocast("cuda", dtype=torch.bfloat16) + if self.cfg.amp_bfloat16 and state.is_cuda + else nullcontext() + ) + with amp: + prediction = self.net(normalized) + with torch.autocast(state.device.type, enabled=False): + increment = ( + prediction.float() * self.output_normalizer.scale + + self.output_normalizer.mean + ) + return integrate_heading_state(state, increment) + + @torch.no_grad() + def update_normalization( + self, + state: torch.Tensor, + action: torch.Tensor, + next_state: torch.Tensor, + context: torch.Tensor | None = None, + ) -> None: + """Merge measured training transitions; validation and imagined data stay excluded.""" + if self._statistics_frozen: + raise RuntimeError("World model normalization is frozen during prediction") + self.input_normalizer.update( + self.inputs(state, action, context).reshape(-1, self.input_dim) + ) + self.output_normalizer.update( + heading_state_increment(state, next_state).reshape(-1, self.increment_dim) + ) + + def state_error( + self, predicted: torch.Tensor, target: torch.Tensor + ) -> torch.Tensor: + """Measure error in target heading axes and each state's body angular axes.""" + return heading_state_increment(target, predicted) + + def error_scale(self) -> torch.Tensor: + """Return measured increment scales including the quadratic epsilon.""" + return self.output_normalizer.scale + + def get_extra_state(self) -> dict[str, Any]: + """Save geometry and state/control identity beside trainable weights.""" + return { + "format": "kinematic-world-model-v1", + "config": self.cfg.to_dict(), + "urdf": self.urdf, + } + + def set_extra_state(self, state: dict[str, Any]) -> None: + """Require the same geometry, state layout and control semantics on load.""" + if state != self.get_extra_state(): + raise ValueError("Kinematic world model checkpoint contract differs") diff --git a/extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py b/extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py index 0e6831f472..0ca3e6dbf3 100644 --- a/extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py +++ b/extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py @@ -26,7 +26,10 @@ from contextlib import contextmanager, nullcontext from collections.abc import Iterator -from typing import Any +from typing import Any, TYPE_CHECKING + +if TYPE_CHECKING: + from .kinematic import KinematicWorldModel import torch from torch import nn @@ -35,6 +38,7 @@ integrate_robot_state, robot_state_features, robot_state_increment, + robot_state_error, ) from embodichain.learning.models.mlp import MLP from embodichain.learning.models.normalizer import EmpiricalNormalizer @@ -223,6 +227,16 @@ def update_normalization( robot_state_increment(state, next_state).reshape(-1, self.increment_dim) ) + def state_error( + self, predicted: torch.Tensor, target: torch.Tensor + ) -> torch.Tensor: + """Measure prediction error in the target body's local frame.""" + return robot_state_error(predicted, target) + + def error_scale(self) -> torch.Tensor: + """Return measured increment standard deviations before the objective floor.""" + return self.output_normalizer.standard_deviation + def get_extra_state(self) -> dict[str, Any]: """Return model layout and action identity saved beside tensor weights. @@ -242,7 +256,7 @@ def set_extra_state(self, state: dict[str, Any]) -> None: @contextmanager -def frozen_world_model(model: RobotWorldModel) -> Iterator[None]: +def frozen_world_model(model: RobotWorldModel | KinematicWorldModel) -> Iterator[None]: """Freeze parameters and statistics while retaining state/action autograd. Args: diff --git a/extensions/embodichain_models/embodichain_models/dynamics/rwm/replay.py b/extensions/embodichain_models/embodichain_models/dynamics/rwm/replay.py index 5237265bc3..c6130701c3 100644 --- a/extensions/embodichain_models/embodichain_models/dynamics/rwm/replay.py +++ b/extensions/embodichain_models/embodichain_models/dynamics/rwm/replay.py @@ -44,6 +44,8 @@ class TransitionReplay: device: Storage and sampling device. max_bytes: Optional allocation ceiling, checked before allocating. seed: Replay-owned sampling RNG seed. + extra_fields: Named scalar/vector fields as ``(width, dtype)`` pairs; + ``width=None`` denotes a scalar. Values travel with their transition. Float payloads are FP32. Terminal and truncated transitions are retained; their following episode is excluded from a sampled window using a validity @@ -62,6 +64,7 @@ def __init__( device: str | torch.device = "cpu", max_bytes: int | None = None, seed: int = 0, + extra_fields: Mapping[str, tuple[int | None, torch.dtype]] | None = None, ) -> None: if any( type(value) is not int or value < 1 @@ -91,7 +94,24 @@ def __init__( "episode_id": (None, torch.int64), "episode_step": (None, torch.int64), } + self.extra_fields = dict(extra_fields or {}) + for name, (width, dtype) in self.extra_fields.items(): + if ( + not isinstance(name, str) + or not name + or name in self._fields + or name in {"valid", "history_valid", "history_complete"} + or (width is not None and (type(width) is not int or width < 1)) + or dtype not in (torch.float32, torch.int64, torch.bool) + ): + raise ValueError(f"Invalid replay extra field: {name}") + self._fields.update(self.extra_fields) scalar_bytes = 4 * (2 * state_dim + action_dim + context_dim) + 2 + 16 + scalar_bytes += sum( + (1 if width is None else width) + * torch.empty((), dtype=dtype).element_size() + for width, dtype in self.extra_fields.values() + ) self.allocated_bytes = capacity * num_envs * scalar_bytes + 2 * num_envs * 8 if max_bytes is not None and ( type(max_bytes) is not int or max_bytes < self.allocated_bytes @@ -135,6 +155,7 @@ def append( context: torch.Tensor | None = None, terminated: torch.Tensor | None = None, truncated: torch.Tensor | None = None, + extras: Mapping[str, torch.Tensor] | None = None, ) -> None: """Copy one vector step; flags end episodes after the stored transition. @@ -145,6 +166,7 @@ def append( context: Recorded FP32 physical context; required for nonzero width. terminated: Hard terminal flags, default false. truncated: Time-limit flags, default false, retained separately. + extras: All fields declared by ``extra_fields``, aligned with this step. """ if context is None: if self.context_dim: @@ -161,6 +183,10 @@ def append( terminated=terminated, truncated=truncated, ) + extras = {} if extras is None else extras + if extras.keys() != self.extra_fields.keys(): + raise ValueError("Replay extras must match the declared fields") + payload.update(extras) for name, value in payload.items(): self._check(name, value) # Validate all fields before mutating a row. copy_ under no_grad owns @@ -251,7 +277,9 @@ def sample_starts( Returns: ``start`` fields, ``history`` fields and Boolean ``history_valid``. Missing history repeats the earliest available state in that same - episode. Requests never retry or wait for more data. + episode. ``history_complete`` distinguishes padding at a real reset + from history lost to ring overwrite. Callers requiring exact history + select this mask. Requests never retry or wait for more data. """ if ( self.size == 0 @@ -276,6 +304,7 @@ def sample_starts( for name, value in self._data.items() }, "history_valid": valid, + "history_complete": (ages >= history_length - 1) | (steps <= ages), } def state_dict(self) -> dict[str, Any]: @@ -287,7 +316,8 @@ def state_dict(self) -> dict[str, Any]: recovery may instead rebuild and refill replay explicitly. """ return { - "format": "transition-replay-v1", + "format": "transition-replay-v2", + "extra_fields": self.extra_fields.copy(), "layout": ( self.capacity, self.num_envs, @@ -318,8 +348,9 @@ def load_state_dict(self, state: Mapping[str, Any]) -> None: self.context_dim, ) if ( - state.get("format") != "transition-replay-v1" + state.get("format") not in ("transition-replay-v1", "transition-replay-v2") or tuple(state.get("layout", ())) != layout + or state.get("extra_fields", {}) != self.extra_fields ): raise ValueError("Replay snapshot layout differs") if state["data"].keys() != self._data.keys(): diff --git a/extensions/embodichain_models/embodichain_models/dynamics/rwm/robot_state.py b/extensions/embodichain_models/embodichain_models/dynamics/rwm/robot_state.py index cb30e2c127..11338ca63b 100644 --- a/extensions/embodichain_models/embodichain_models/dynamics/rwm/robot_state.py +++ b/extensions/embodichain_models/embodichain_models/dynamics/rwm/robot_state.py @@ -26,9 +26,15 @@ from __future__ import annotations +from collections.abc import Mapping + import torch +from embodichain.compute.kinematics.matrix import matrix_product, matrix_vector_product + __all__ = [ + "pack_robot_state", + "unpack_robot_state", "rotation_exp", "rotation_log", "robot_state_features", @@ -73,7 +79,7 @@ def rotation_exp(vector: torch.Tensor) -> torch.Tensor: ) skew = _skew(vector) eye = torch.eye(3, dtype=vector.dtype, device=vector.device) - return eye + a.unsqueeze(-1) * skew + b.unsqueeze(-1) * (skew @ skew) + return eye + a.unsqueeze(-1) * skew + b.unsqueeze(-1) * matrix_product(skew, skew) def rotation_log(matrix: torch.Tensor) -> torch.Tensor: @@ -140,7 +146,7 @@ def _parts(state: torch.Tensor) -> tuple[torch.Tensor, ...]: def _body(rotation: torch.Tensor, vector: torch.Tensor) -> torch.Tensor: - return (rotation.transpose(-1, -2) @ vector.unsqueeze(-1)).squeeze(-1) + return matrix_vector_product(rotation.transpose(-1, -2), vector) def robot_state_features(state: torch.Tensor) -> torch.Tensor: @@ -188,7 +194,7 @@ def robot_state_increment( return torch.cat( ( _body(r, p1 - p), - rotation_log(r.transpose(-1, -2) @ r1), + rotation_log(matrix_product(r.transpose(-1, -2), r1)), _body(r, v1 - v), _body(r, w1 - w), q1 - q, @@ -213,11 +219,11 @@ def integrate_robot_state(state: torch.Tensor, increment: torch.Tensor) -> torch p, r, v, w, q, qd = _parts(state) joints = q.shape[-1] dp, dr, dv, dw, dq, dqd = increment.split((3, 3, 3, 3, joints, joints), -1) - world = lambda value: (r @ value.unsqueeze(-1)).squeeze(-1) + world = lambda value: matrix_vector_product(r, value) return torch.cat( ( p + world(dp), - (r @ rotation_exp(dr)).flatten(-2), + matrix_product(r, rotation_exp(dr)).flatten(-2), v + world(dv), w + world(dw), q + dq, @@ -238,3 +244,30 @@ def robot_state_error(predicted: torch.Tensor, target: torch.Tensor) -> torch.Te Position, angular, velocity and joint errors in increment layout. """ return robot_state_increment(target, predicted) + + +def pack_robot_state(state: Mapping[str, torch.Tensor]) -> torch.Tensor: + """Pack named root-origin state into an owned canonical matrix-based tensor. + + Args: + state: ``pos``, ``rot``, ``v``, ``w``, ``q`` and ``qd`` tensors with aligned batches. + + Returns: + Concatenated state, width ``18 + 2*J``, preserving input gradients. + """ + return torch.cat( + ( + state["pos"], + state["rot"].flatten(-2), + state["v"], + state["w"], + state["q"], + state["qd"], + ), + -1, + ) + + +def unpack_robot_state(state: torch.Tensor) -> dict[str, torch.Tensor]: + """Return named views of a canonical matrix-based state; no data is copied.""" + return dict(zip(("pos", "rot", "v", "w", "q", "qd"), _parts(state))) diff --git a/extensions/embodichain_models/embodichain_models/dynamics/rwm/supervised.py b/extensions/embodichain_models/embodichain_models/dynamics/rwm/supervised.py index a30d53be28..8de8d1f54c 100644 --- a/extensions/embodichain_models/embodichain_models/dynamics/rwm/supervised.py +++ b/extensions/embodichain_models/embodichain_models/dynamics/rwm/supervised.py @@ -23,14 +23,17 @@ import torch from embodichain.learning.sl import LossResult, SupervisedBatch -from .robot_state import robot_state_error +from .kinematic import KinematicWorldModel from .model import RobotWorldModel __all__ = ["RobotWorldModelObjective", "replay_supervised_batch"] def replay_supervised_batch( - windows: Mapping[str, torch.Tensor], *, source_id: str = "transition-replay" + windows: Mapping[str, torch.Tensor], + *, + source_id: str = "transition-replay", + complete_windows: bool = False, ) -> SupervisedBatch: """Expose an owned transition window as initial state and future supervision. @@ -38,12 +41,21 @@ def replay_supervised_batch( windows: Output of ``TransitionReplay.sample_windows``. ``state``, ``action``, ``next_state``, ``context`` and ``valid`` are required. source_id: Stable identity supplied by the replay's owning workflow. + complete_windows: Retain only full-horizon episodes; also exclude rows + with false ``usable`` flags when that auxiliary field is present. Returns: Batch with the measured starting state and actions as inputs. Future measured states enter targets only; their values never teacher-force the autoregressive rollout. """ + if complete_windows: + selected = windows["valid"].all(1) + if "usable" in windows: + selected &= windows["usable"].all(1) + if not bool(selected.any()): + raise ValueError("No complete usable replay windows") + windows = {name: value[selected] for name, value in windows.items()} return SupervisedBatch( inputs={ "initial_state": windows["state"][:, 0], @@ -60,22 +72,49 @@ class RobotWorldModelObjective: """Fit a complete autoregressive window using normalized state errors. Args: - error_floor: Minimum physical increment scale for near-constant fields. + error_floor: Minimum physical increment scale, or None to use the model scale. + temporal_reduction: ``mean`` averages valid steps; ``sum`` sums time then + averages windows, matching a fixed-horizon autoregressive objective. + error_weights: Optional nonnegative weights in increment feature order. + compile_mode: Optional Torch compilation mode for complete fixed-shape + windows. Padded windows retain the masked eager path. - Loss is the mean squared normalized error over valid transitions and state - increment features. Invalid suffixes are excluded before physical-error + Loss averages normalized squared error over increment features and applies + the configured temporal reduction. Invalid suffixes are excluded before physical-error arithmetic and may contain nonfinite padding. This supervised objective does not create rewards, continuation probabilities or policy gradients. The last-step diagnostic weights each window by its valid transition count. """ - def __init__(self, error_floor: float = 1e-2) -> None: - if not 0 < error_floor < float("inf"): + def __init__( + self, + error_floor: float | None = 1e-2, + *, + temporal_reduction: str = "mean", + error_weights: tuple[float, ...] = (), + compile_mode: str | None = None, + ) -> None: + if error_floor is not None and not 0 < error_floor < float("inf"): raise ValueError("error_floor must be finite and positive") + if temporal_reduction not in ("mean", "sum"): + raise ValueError("temporal_reduction must be mean or sum") + if error_weights and ( + any(not 0 <= w < float("inf") for w in error_weights) + or not any(error_weights) + ): + raise ValueError( + "Error weights must be finite, nonnegative and not all zero" + ) self.error_floor = error_floor + self.temporal_reduction, self.error_weights = temporal_reduction, error_weights + self._compiled_complete = ( + None + if compile_mode is None + else torch.compile(self._complete_loss, mode=compile_mode, fullgraph=True) + ) def compute_loss( - self, model: RobotWorldModel, batch: SupervisedBatch + self, model: RobotWorldModel | KinematicWorldModel, batch: SupervisedBatch ) -> LossResult: """Build an untruncated multi-step prediction graph. @@ -111,6 +150,22 @@ def compute_loss( model.cfg.context_dim, ): raise ValueError("RWM context must align with every supervised transition") + if self._compiled_complete is not None and bool(valid.all()): + loss, first, last, position = self._compiled_complete( + model, state, actions, contexts, target + ) + count = state.shape[0] * ( + actions.shape[1] if self.temporal_reduction == "mean" else 1 + ) + return LossResult( + loss, + metrics={ + "first_step_mse": first.detach(), + "last_step_mse": last.detach(), + "position_error_m": position.detach(), + }, + weight=float(count), + ) predictions = [] for index in range(actions.shape[1]): active = valid[:, index, None] @@ -132,12 +187,20 @@ def compute_loss( # A learner's autocast covers the objective, so explicitly keep error # transport, logarithms and normalization in physical FP32 arithmetic. with torch.autocast(state.device.type, enabled=False): - error = robot_state_error(predicted, target) - scale = model.output_normalizer.standard_deviation.clamp_min( - self.error_floor - ) - squared = (error / scale).square().mean(-1) - loss = squared.mean() + error = model.state_error(predicted, target) + scale = model.error_scale() + if self.error_floor is not None: + scale = scale.clamp_min(self.error_floor) + per_feature = (error / scale).square() + squared = per_feature.mean(-1) + if self.error_weights: + if len(self.error_weights) != error.shape[-1]: + raise ValueError("Error weights must match the increment width") + weighted = (per_feature * error.new_tensor(self.error_weights)).mean(-1) + else: + weighted = squared + count = valid.sum() if self.temporal_reduction == "mean" else valid.shape[0] + loss = weighted.sum() / count lengths = valid.sum(1) # Boolean selection is row-major; each prefix ends at its cumulative # valid count, so last-step diagnostics never inspect padded values. @@ -147,8 +210,53 @@ def compute_loss( return LossResult( loss, metrics={ + "first_step_mse": squared[ + torch.cat((lengths.new_zeros(1), lengths.cumsum(0)[:-1])) + ] + .mean() + .detach(), "last_step_mse": final.detach(), "position_error_m": position_error.mean().detach(), }, - weight=float(valid.sum()), + weight=float(count), ) + + def _complete_loss( + self, + model: RobotWorldModel | KinematicWorldModel, + state: torch.Tensor, + actions: torch.Tensor, + contexts: torch.Tensor | None, + target: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Compile the full autoregression and backward without masked indexing.""" + predictions = [] + for index in range(actions.shape[1]): + state = model( + state, + actions[:, index], + None if contexts is None else contexts[:, index], + ) + predictions.append(state) + predicted = torch.stack(predictions, 1).flatten(0, 1) + target = target.flatten(0, 1) + with torch.autocast(state.device.type, enabled=False): + error = model.state_error(predicted, target) + scale = model.error_scale() + if self.error_floor is not None: + scale = scale.clamp_min(self.error_floor) + per_feature = (error / scale).square() + squared = per_feature.mean(-1) + if self.error_weights: + if len(self.error_weights) != error.shape[-1]: + raise ValueError("Error weights must match the increment width") + weighted = (per_feature * error.new_tensor(self.error_weights)).mean(-1) + else: + weighted = squared + count = actions.shape[0] * ( + actions.shape[1] if self.temporal_reduction == "mean" else 1 + ) + loss = weighted.sum() / count + by_step = squared.reshape(*actions.shape[:2], *squared.shape[1:]) + position_error = (predicted[..., :3] - target[..., :3]).norm(dim=-1).mean() + return loss, by_step[:, 0].mean(), by_step[:, -1].mean(), position_error diff --git a/extensions/embodichain_models/embodichain_models/policies/__init__.py b/extensions/embodichain_models/embodichain_models/policies/__init__.py new file mode 100644 index 0000000000..847d228a65 --- /dev/null +++ b/extensions/embodichain_models/embodichain_models/policies/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Task-conditioned policy model families.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/extensions/embodichain_models/embodichain_models/policies/motion_tracking.py b/extensions/embodichain_models/embodichain_models/policies/motion_tracking.py new file mode 100644 index 0000000000..5267c3a9bb --- /dev/null +++ b/extensions/embodichain_models/embodichain_models/policies/motion_tracking.py @@ -0,0 +1,230 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Goal-conditioned motion policy and privileged scalar value model.""" + +from __future__ import annotations + +from collections.abc import Mapping +from contextlib import nullcontext +from typing import Any + +from gymnasium import spaces +import torch +from torch import nn +from tensordict import TensorDict + +from embodichain.learning.models.mlp import MLP +from embodichain.learning.models.normalizer import EmpiricalNormalizer +from embodichain.learning.rl.policies import Policy, register_policy +from embodichain.utils import configclass + +__all__ = ["MotionTrackingPolicyCfg", "MotionTrackingPolicy"] + + +@configclass +class MotionTrackingPolicyCfg: + """Dimensions, normalization and MLPs of a deterministic tracking policy.""" + + history_dim: int = 930 + goal_dim: int = 730 + critic_dim: int = 1009 + action_dim: int = 29 + latent_dim: int = 128 + encoder_hidden: tuple[int, ...] = (2048, 1024, 512, 256) + decoder_hidden: tuple[int, ...] = (2048, 1024, 512, 256) + critic_hidden: tuple[int, ...] = (256, 128) + normalization_epsilon: float = 1e-2 + amp_bfloat16: bool = False + + +class _GoalActor(nn.Module): + def __init__(self, cfg: MotionTrackingPolicyCfg) -> None: + super().__init__() + self.cfg = cfg + self.obs_norm = EmpiricalNormalizer( + cfg.history_dim, cfg.normalization_epsilon, variance_epsilon=True, clip=10.0 + ) + self.goal_norm = EmpiricalNormalizer( + cfg.goal_dim, cfg.normalization_epsilon, variance_epsilon=True, clip=10.0 + ) + self.encoder = MLP(cfg.goal_dim, cfg.latent_dim, cfg.encoder_hidden) + self.decoder = MLP( + cfg.latent_dim + cfg.history_dim, cfg.action_dim, cfg.decoder_hidden + ) + nn.init.orthogonal_(self.decoder[-1].weight, gain=0.01) + nn.init.zeros_(self.decoder[-1].bias) + + def forward(self, observation: torch.Tensor) -> torch.Tensor: + history, goal = observation.split((self.cfg.history_dim, self.cfg.goal_dim), -1) + history, goal = self.obs_norm(history), self.goal_norm(goal) + with ( + torch.autocast("cuda", dtype=torch.bfloat16) + if self.cfg.amp_bfloat16 and observation.is_cuda + else nullcontext() + ): + latent = self.encoder(goal).float().tanh() + return self.decoder(torch.cat((latent, history), -1)).float() + + +class MotionTrackingPolicy(Policy): + """Deterministic goal encoder/decoder with a separately optimized critic. + + Actor observations concatenate oldest-first proprioceptive history and future + goals. Privileged critic inputs are supplied by the task. Exploration noise + belongs to environment collection; the policy has no learned action variance. + Normalization updates are explicit and are checkpointed with network weights. + """ + + actor_obs_groups = ("policy",) + critic_obs_groups = ("critic",) + uses_separate_critic_obs = True + + def __init__( + self, cfg: MotionTrackingPolicyCfg, device: torch.device | str = "cpu" + ) -> None: + super().__init__() + cfg.validate() + if ( + min( + cfg.history_dim, + cfg.goal_dim, + cfg.critic_dim, + cfg.action_dim, + cfg.latent_dim, + ) + < 1 + ): + raise ValueError("Tracking model dimensions must be positive.") + self.cfg = cfg + self.device = torch.device(device) + self.obs_dim = self.actor_obs_dim = cfg.history_dim + cfg.goal_dim + self.critic_obs_dim, self.action_dim = cfg.critic_dim, cfg.action_dim + self.actor = _GoalActor(cfg) + self.critic_obs_normalizer = EmpiricalNormalizer( + cfg.critic_dim, cfg.normalization_epsilon, variance_epsilon=True, clip=10.0 + ) + self.critic = MLP(cfg.critic_dim, 1, cfg.critic_hidden) + self.to(self.device) + + @classmethod + def from_config( + cls, + config: Mapping[str, Any], + *, + obs_space: spaces.Space, + action_space: spaces.Space, + device: torch.device, + ) -> MotionTrackingPolicy: + """Build from task dimensions and validate observation spaces. + + Args: + config: Policy block with model dimensions and architecture in ``cfg``. + obs_space: Named actor and critic observation spaces. + action_space: Flat continuous action space. + device: Device for model parameters and normalization buffers. + + Returns: + Policy compatible with the supplied observation and action spaces. + """ + if not isinstance(obs_space, spaces.Dict) or not isinstance( + action_space, spaces.Box + ): + raise TypeError( + "Motion tracking requires Dict observations and Box actions." + ) + values = dict(config.get("cfg", {})) + for name in ("encoder_hidden", "decoder_hidden", "critic_hidden"): + if name in values: + values[name] = tuple(values[name]) + cfg = MotionTrackingPolicyCfg(**values) + cfg.validate() + expected = { + "policy": cfg.history_dim + cfg.goal_dim, + "critic": cfg.critic_dim, + } + if any( + key not in obs_space.spaces or obs_space[key].shape != (size,) + for key, size in expected.items() + ) or action_space.shape != (cfg.action_dim,): + raise ValueError( + "Tracking policy dimensions differ from environment spaces." + ) + return cls(cfg, device) + + def optimization_parameter_groups( + self, + ) -> tuple[tuple[nn.Parameter, ...], tuple[nn.Parameter, ...]]: + """Return disjoint actor and critic weights.""" + return tuple(self.actor.parameters()), tuple(self.critic.parameters()) + + def forward( + self, tensordict: TensorDict, deterministic: bool = False + ) -> TensorDict: + """Write deterministic actions; noise is supplied by the environment.""" + tensordict["action"] = self.actor(tensordict["obs"]) + return tensordict + + def get_differentiable_action( + self, tensordict: TensorDict, deterministic: bool = False + ) -> TensorDict: + """Evaluate the actor with its state and parameter graph intact.""" + return self.forward(tensordict, deterministic) + + def get_value(self, tensordict: TensorDict) -> TensorDict: + """Evaluate a scalar value from the task's privileged features.""" + value = self.critic_obs_normalizer(tensordict["critic_obs"]) + with ( + torch.autocast("cuda", dtype=torch.bfloat16) + if self.cfg.amp_bfloat16 and value.is_cuda + else nullcontext() + ): + tensordict["value"] = self.critic(value).float().squeeze(-1) + return tensordict + + def evaluate_actions(self, tensordict: TensorDict) -> TensorDict: + """Reject likelihood-based updates for this deterministic policy.""" + raise NotImplementedError("MotionTrackingPolicy uses pathwise APG updates.") + + @torch.no_grad() + def update_actor_normalization(self, observation: torch.Tensor) -> None: + """Merge measured history and clean goals before collecting actions.""" + history, goal = observation.reshape(-1, self.obs_dim).split( + (self.cfg.history_dim, self.cfg.goal_dim), -1 + ) + self.actor.obs_norm.update(history) + self.actor.goal_norm.update(goal) + + @torch.no_grad() + def update_critic_normalization(self, observation: torch.Tensor) -> None: + """Merge detached imagined critic inputs after actor backpropagation.""" + self.critic_obs_normalizer.update(observation.reshape(-1, self.critic_obs_dim)) + + def get_extra_state(self) -> dict[str, Any]: + """Save the model configuration used to interpret network weights.""" + return self.cfg.to_dict() + + def set_extra_state(self, state: dict[str, Any]) -> None: + """Reject checkpoints with a different model/normalization identity.""" + identity = dict(state) + current = self.cfg.to_dict() + identity.pop("amp_bfloat16", None) + current.pop("amp_bfloat16", None) + if identity != current: + raise ValueError("Tracking policy checkpoint configuration differs.") + + +register_policy("motion_tracking", MotionTrackingPolicy) diff --git a/extensions/embodichain_tasks/configs/tasks/classic_control/point_mass/agents/apg_actor_critic.yaml b/extensions/embodichain_tasks/configs/tasks/classic_control/point_mass/agents/apg_actor_critic.yaml new file mode 100644 index 0000000000..f7131482ec --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/classic_control/point_mass/agents/apg_actor_critic.yaml @@ -0,0 +1,66 @@ +trainer: + exp_name: point_mass_apg_actor_critic + learning_env: + name: PointMassRL + cfg: + max_episode_steps: 100 + success_threshold: 0.03 + seed: 42 + device: cuda:0 + num_envs: 64 + iterations: 800 + segment_length: 32 + update_horizon: 32 + enable_eval: true + eval_freq: 32000 + num_eval_envs: 32 + num_eval_episodes: 128 + eval_seed: 10042 + best_eval_metric: eval/success_rate + best_eval_mode: max + save_frequency_updates: 0 + use_wandb: false + wandb_project_name: embodichain-point-mass + deterministic_actions: true +policy: + name: actor_critic + initial_log_std: -1.0 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 64 + - 64 + activation: tanh + last_activation: tanh + orthogonal_init: + - 1.414 + - 1.414 + - 0.01 + critic: + type: mlp + network_cfg: + hidden_sizes: + - 64 + - 64 + activation: tanh + orthogonal_init: + - 1.414 + - 1.414 + - 1.0 +algorithm: + name: apg_actor_critic + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + critic_optimizer: + name: adam + learning_rate: 0.001 + critic_epochs: 4 + critic_minibatches: 4 + gamma: 0.99 + gae_lambda: 0.95 + max_grad_norm: 1.0 + critic_max_grad_norm: 1.0 + normalize_return: true diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/README.md b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/README.md new file mode 100644 index 0000000000..e329f9d07e --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/README.md @@ -0,0 +1,104 @@ +# G1 motion tracking + +`UnitreeG1MotionRL-v1` tracks G1 reference motion in a DexUni environment using its rigid-body MuJoCo-Warp route. The deployment advances four 5 ms substeps per 20 ms control interval. The supplied PPO configuration runs 32 environments for 20 updates to check task and trainer integration. + +## Prepare motion data + +Place the LAFAN1 G1 retargeting source under `data/LAFAN1_Retargeting`, with CSV files in `g1/` and the matching URDF and meshes in `robot_description/g1/`. From the repository root, run: + +```bash +python examples/learning/motion_tracking/prepare_lafan1.py --source data/LAFAN1_Retargeting +``` + +The command resamples the 30 Hz CSV files to 50 Hz and writes `LAFAN1G1/motion.npz` and `LAFAN1G1/robot/` under the EmbodiChain data root. Set `EMBODICHAIN_DATA_ROOT` before preparation and training to select another data location. The complete input contains 40 clips and 441,140 resampled frames. The archive records the joint order and uses XYZW quaternions. + +## Train and evaluate + +```bash +python -m embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/ppo.yaml +``` + +Training writes a timestamped `outputs/g1_motion_ppo_smoke_*` directory containing configuration snapshots, TensorBoard metrics, a checkpoint and `run-manifest.json`. Evaluate the newest run from the start of each reference clip: + +```bash +RUN_DIR=$(ls -dt outputs/g1_motion_ppo_smoke_* | head -n 1) +python examples/learning/motion_tracking/evaluate.py "$RUN_DIR" +``` + +Evaluation disables observation noise and removes the 500-step training limit. Each clip runs until physical failure or its final reference frame. `evaluations/motion-clips.json` records per-clip returns and failure status. Normalized return divides accumulated reward by the full reference length times the maximum per-step tracking reward, including the unexecuted tail after a failure in the denominator. + +## Task interfaces + +| Interface | Content | +|---|---| +| Policy input | 930 history features followed by 730 future reference features | +| Critic input | 245 state features, 730 goal features, 33 physical parameters and one remaining-time feature | +| Policy action | 29 values: leg setpoints relative to the resting pose; upper-body residuals relative to the next reference frame | +| Executed target | Resting pose plus scaled setpoint action minus encoder bias | +| Reset | Reference position and velocity; selected rows receive repeated initial observations in their history | +| Failure | Root-height or tilt tracking error above the configured threshold | +| Truncation | Reference clip end or the training step limit; hard failure takes precedence | +| Pre-action snapshot | `capture_transition_start()` returns owned state, frame, physical context and previous policy action | +| Transition snapshot | Owned post-step state, frame, history, goal, policy action, normalized setpoint, target position, continuation probability and `usable` flag | +| Divergent state | Hard termination, finite observation replacement and `usable=False`; excluded from WM supervision | + +The base physical deployment uses nominal physical parameters, uniform clip/start selection and observation noise. The online model-based configs additionally enable adaptive time-bin sampling, reset perturbations, friction randomization, encoder bias and torso center-of-mass offsets. Each measured transition carries the physical context used by its environment. + +The task reuses `MotionLibrary` for reference data and `TreeKinematics` for differentiable body states. Tensor tracking calculations in `locomotion/tracking/_task.py` provide the observation, action mapping, goals, reward and continuation used by the physical environment and the model environment. + +## Fit a world model from physical transitions + +Run the collection and supervised fitting example after preparing the same data: + +```bash +python examples/learning/motion_tracking/fit_world_model.py \ + --config extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml \ + --output outputs/g1_world_model_seed0 +``` + +The default run uses 160 physical environments and collects 512 vector steps. +The first 128 environments supply 65,536 training transitions. The last 32 supply +16,384 held-out transitions; neither their states nor their normalizer statistics +enter training. Collection uses reference joint-position commands, with Gaussian +action noise of standard deviation 0.1 in 75% of environments. This supervised +experiment measures dynamics fitting before connecting learned-policy collection. + +The model has three 2048-wide SiLU layers and 9,166,918 parameters. It predicts +heading-frame increments from 245 state features, 29 executed setpoints and 33 +physical parameters. Training uses shared SL, Adam at 2e-4, a gradient norm limit +of one, 500 updates and complete eight-step windows of batch size 128. Network +arithmetic uses BF16 by default; `--fp32` disables it. FK and state integration +remain FP32. `world_model_weights.yaml` records the frozen G1 recipe's joint error +weights so changes to geometry or weighting-pose selection do not change the loss. + +The autoregressive objective sums the eight per-step errors and averages over +windows. Each per-step error is the feature mean of squared state error divided +by measured increment variance, with the configured joint weights. No predicted +state is detached within the window. Only training transitions update moments. + +`run.json` records the recipe; `metrics.jsonl` records every optimizer update; +`result.json` compares the final model with its initialization and a state-hold +baseline on fixed held-out windows. `world_model.pt` stores final model, +normalizers and optimizer state. Replay remains in memory during the run. +The command fails if final eight-step normalized error does not improve over +both baselines. A run takes about two minutes after simulator initialization on +a single RTX PRO 5000 in the recorded 500-update experiment. + +## Online world-model and APG training + +The model-based recipe alternates measured collection, shared supervised updates +and imagined APG Actor-Critic updates. Its functional, four-clip development and +40-clip convergence configurations are `agents/model_based.yaml`, +`agents/model_based_development.yaml` and `agents/model_based_full.yaml`. + +See the [online training guide](../../../../../../../examples/learning/motion_tracking/README.md) +for the fixed data source, complete commands, network sizes, gradient boundaries, +checkpoint recovery, output schema and physical convergence criteria. + +## State synchronization and complete-clip evaluation + +The MuJoCo-Warp configuration uses `update_data_interval: 0` so that native state advances continuously across the four substeps. This avoids converting floating-base velocity between world and body coordinates before each substep. After `write_state()` writes selected robot rows, the task calls the public `Scene.reset(arenas=..., reset_clock=False)` API to publish their new starting state and clear their integration history. This covers episode initialization, automatic resets and explicit state injection. Other arenas and physical parameters remain unchanged; shared simulation time continues, and the next step recaptures the CUDA Graph. + +Setting `update_data_interval: 1` retains per-substep synchronization for diagnostic comparisons and does not add a Scene reset after task state writes. Evaluation preserves the checkpoint's original configuration unless an explicit evaluation configuration selects another interval; the checkpoint metadata continues to describe the original training run. Continuous native integration requires DexSim's selected-world DexUni reset and child-solver CUDA Graph state-preservation fixes. + +For complete-clip evaluation, `fixed_clip_assignment: true` assigns world `i` to motion clip `i` on every automatic reset. It requires one world per clip and disables random starts and adaptive sampling. The evaluation entry point enables this option and records the assigned and final episode start frames. diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based.yaml new file mode 100644 index 0000000000..412aaabec0 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based.yaml @@ -0,0 +1,79 @@ +trainer: + learning_env: + name: MotionTrackingModelRL + cfg: + horizon: 32 + num_envs: 128 + rollout_mode: complete + deterministic_actions: true +policy: + name: motion_tracking + cfg: + latent_dim: 128 + encoder_hidden: + - 256 + - 128 + decoder_hidden: + - 256 + - 128 + critic_hidden: + - 128 + - 64 + amp_bfloat16: true +algorithm: + name: apg_actor_critic + cfg: + gamma: 0.99 + gae_lambda: 0.95 + max_grad_norm: 1.0 + optimizer: + learning_rate: 0.0002 + kwargs: + betas: + - 0.9 + - 0.95 + fused: true + critic_optimizer: + learning_rate: 0.0002 + kwargs: + fused: true + critic_epochs: 5 + critic_minibatches: 4 + actor_grad_value_clip: 0.1 + update_critic_normalization: true +workflow: + kind: test + seed: 0 + clips: + - walk1_subject1 + - dance1_subject1 + num_envs: 128 + buffer_steps: 256 + prefill_steps: 64 + warmup_updates: 50 + iterations: 200 + utd: 0.4 + wm_batches: 2 + wm_batch: 128 + wm_horizon: 8 + wm_hidden: + - 256 + - 256 + - 256 + lr: 0.0002 + lr_final_frac: 0.03 + lr_decay_iters: 250000 + amp: true + tf32: false + norm_update_every: 5 + action_noise: 0.1 + collect_det_frac: 0.25 + eval_every: 200 + save_every: 200 + log_every: 25 + task_options: + adaptive_sampling: true + reset_noise: true + domain_randomization: true + collection_env: extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml + evaluation_env: extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based_development.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based_development.yaml new file mode 100644 index 0000000000..5280526014 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based_development.yaml @@ -0,0 +1,85 @@ +trainer: + learning_env: + name: MotionTrackingModelRL + cfg: + horizon: 32 + num_envs: 512 + rollout_mode: complete + deterministic_actions: true +policy: + name: motion_tracking + cfg: + latent_dim: 128 + encoder_hidden: + - 2048 + - 1024 + - 512 + - 256 + decoder_hidden: + - 2048 + - 1024 + - 512 + - 256 + critic_hidden: + - 256 + - 128 + amp_bfloat16: true +algorithm: + name: apg_actor_critic + cfg: + gamma: 0.99 + gae_lambda: 0.95 + max_grad_norm: 1.0 + optimizer: + learning_rate: 0.0002 + kwargs: + betas: + - 0.9 + - 0.95 + fused: true + critic_optimizer: + learning_rate: 0.0002 + kwargs: + fused: true + critic_epochs: 5 + critic_minibatches: 4 + actor_grad_value_clip: 0.1 + update_critic_normalization: true +workflow: + kind: convergence + seed: 0 + clips: + - walk1_subject1 + - dance1_subject1 + - jumps1_subject1 + - fallAndGetUp1_subject1 + num_envs: 512 + buffer_steps: 1000 + prefill_steps: 64 + warmup_updates: 500 + iterations: 2000 + utd: 0.4 + wm_batches: 10 + wm_batch: 512 + wm_horizon: 8 + wm_hidden: + - 2048 + - 2048 + - 2048 + lr: 0.0002 + lr_final_frac: 0.03 + lr_decay_iters: 250000 + amp: true + tf32: false + norm_update_every: 5 + action_noise: 0.1 + collect_det_frac: 0.25 + eval_every: 500 + save_every: 500 + log_every: 100 + task_options: + adaptive_sampling: true + reset_noise: true + domain_randomization: true + collection_env: extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml + evaluation_env: extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based_fixed_wm.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based_fixed_wm.yaml new file mode 100644 index 0000000000..abc055bef2 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based_fixed_wm.yaml @@ -0,0 +1,85 @@ +trainer: + learning_env: + name: MotionTrackingModelRL + cfg: + horizon: 32 + num_envs: 128 + rollout_mode: complete + deterministic_actions: true +policy: + name: motion_tracking + cfg: + latent_dim: 128 + encoder_hidden: + - 2048 + - 1024 + - 512 + - 256 + decoder_hidden: + - 2048 + - 1024 + - 512 + - 256 + critic_hidden: + - 256 + - 128 + amp_bfloat16: true +algorithm: + name: apg_actor_critic + cfg: + gamma: 0.99 + gae_lambda: 0.95 + max_grad_norm: 1.0 + optimizer: + learning_rate: 0.0002 + kwargs: + betas: + - 0.9 + - 0.95 + fused: true + critic_optimizer: + learning_rate: 0.0002 + kwargs: + fused: true + critic_epochs: 5 + critic_minibatches: 4 + actor_grad_value_clip: 0.1 + update_critic_normalization: true +workflow: + kind: test + seed: 0 + clips: null + num_envs: 128 + buffer_steps: 128 + prefill_steps: 64 + warmup_updates: 0 + iterations: 200 + utd: 0.4 + wm_batches: 0 + wm_batch: 2048 + wm_horizon: 8 + wm_hidden: + - 2048 + - 2048 + - 2048 + lr: 0.0002 + lr_final_frac: 0.03 + lr_decay_iters: 250000 + amp: true + tf32: false + compile_mode: reduce-overhead + episode_context_in_info: false + norm_update_every: 5 + action_noise: 0.1 + collect_det_frac: 0.25 + eval_every: 0 + save_every: 0 + log_every: 50 + task_options: + adaptive_sampling: true + reset_noise: true + domain_randomization: true + evaluate_final: true + learning_mode: fixed_world_model + collection_env: extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml + evaluation_env: extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based_full.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based_full.yaml new file mode 100644 index 0000000000..cb4e680e20 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/model_based_full.yaml @@ -0,0 +1,84 @@ +trainer: + learning_env: + name: MotionTrackingModelRL + cfg: + horizon: 32 + num_envs: 4096 + rollout_mode: complete + deterministic_actions: true +policy: + name: motion_tracking + cfg: + latent_dim: 128 + encoder_hidden: + - 2048 + - 1024 + - 512 + - 256 + decoder_hidden: + - 2048 + - 1024 + - 512 + - 256 + critic_hidden: + - 256 + - 128 + amp_bfloat16: true +algorithm: + name: apg_actor_critic + cfg: + gamma: 0.99 + gae_lambda: 0.95 + max_grad_norm: 1.0 + optimizer: + learning_rate: 0.0002 + kwargs: + betas: + - 0.9 + - 0.95 + fused: true + critic_optimizer: + learning_rate: 0.0002 + kwargs: + fused: true + critic_epochs: 5 + critic_minibatches: 4 + actor_grad_value_clip: 0.1 + update_critic_normalization: true +workflow: + kind: convergence + seed: 0 + clips: null + num_envs: 4096 + buffer_steps: 1000 + prefill_steps: 64 + warmup_updates: 0 + iterations: 60000 + utd: 0.4 + wm_batches: 10 + wm_batch: 2048 + wm_horizon: 8 + wm_hidden: + - 2048 + - 2048 + - 2048 + lr: 0.0002 + lr_final_frac: 0.03 + lr_decay_iters: 250000 + amp: true + tf32: false + compile_mode: reduce-overhead + episode_context_in_info: false + norm_update_every: 5 + action_noise: 0.1 + collect_det_frac: 0.25 + eval_every: 0 + save_every: 0 + log_every: 50 + task_options: + adaptive_sampling: true + reset_noise: true + domain_randomization: true + evaluate_final: true + collection_env: extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml + evaluation_env: extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/ppo.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/ppo.yaml new file mode 100644 index 0000000000..d601a0c6ec --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/agents/ppo.yaml @@ -0,0 +1,44 @@ +trainer: + exp_name: g1_motion_ppo_smoke + gym_config: extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml + seed: 0 + device: cuda:0 + headless: true + num_envs: 32 + iterations: 20 + buffer_size: 16 + enable_eval: false + save_frequency_updates: 20 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + obs_groups: + actor: [policy] + critic: [critic] + actor: + type: mlp + network_cfg: + hidden_sizes: [256, 128] + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: [256, 128] + activation: elu + initial_action_std: 0.2 +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 2 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.0 + vf_coef: 1.0 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.dexuni.yaml new file mode 100644 index 0000000000..7d4dc3f298 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.dexuni.yaml @@ -0,0 +1,50 @@ +id: UnitreeG1MotionRL-v1 +physics: newton +num_envs: 8 +max_episode_steps: 500 +headless: true +device: cuda:0 +arena_space: 0.0 +scene_node_capacity: 131071 +renderer: no-render +physics_config: + physics_dt: 0.02 + num_substeps: 4 + sync_to_renderer: false + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + mujoco_options: + njmax: 500 + nconmax: 128 + integrator: implicitfast + cone: pyramidal + separate_worlds: true + use_mujoco_contacts: true + update_data_interval: 0 +env: + sim_steps_per_control: 1 + extensions: + motion_tracking: + motion_path: LAFAN1G1/motion.npz + quaternion_order: xyzw + observation_noise: true + random_start: true +robot: + uid: robot + fpath: LAFAN1G1/robot/g1_29dof_rev_1_0.urdf + build_pk_chain: false + init_pos: [0.0, 0.0, 0.8] +sensor: [] +light: {} +background: +- uid: floor + shape: + shape_type: Plane + body_type: static + attrs: + material_props: + dynamic_friction: 1.0 +rigid_object: [] +rigid_object_group: [] +articulation: [] diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml new file mode 100644 index 0000000000..daba77791e --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml @@ -0,0 +1,53 @@ +id: UnitreeG1MotionRL-v1 +physics: newton +num_envs: 8 +max_episode_steps: 500 +headless: true +device: cuda:0 +arena_space: 0.0 +scene_node_capacity: 131071 +renderer: no-render +physics_config: + physics_dt: 0.02 + num_substeps: 4 + sync_to_renderer: false + solver_cfg: + solver_type: mujoco_warp + rne_mode: last_substep + njmax: 500 + nconmax: 128 + integrator: implicitfast + cone: pyramidal + separate_worlds: true + use_mujoco_contacts: true + update_data_interval: 0 + use_cuda_graph: true +env: + sim_steps_per_control: 1 + extensions: + motion_tracking: + motion_path: LAFAN1G1/motion.npz + quaternion_order: xyzw + observation_noise: true + random_start: true +robot: + uid: robot + fpath: LAFAN1G1/robot/g1_29dof_rev_1_0.urdf + build_pk_chain: false + init_pos: + - 0.0 + - 0.0 + - 0.8 +sensor: [] +light: {} +background: +- uid: floor + shape: + shape_type: Plane + body_type: static + attrs: + material_props: + dynamic_friction: 1.0 +rigid_object: [] +rigid_object_group: [] +articulation: [] diff --git a/extensions/embodichain_tasks/embodichain_tasks/classic_control/point_mass.py b/extensions/embodichain_tasks/embodichain_tasks/classic_control/point_mass.py index 9fc5925f57..c5582a9f2e 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/classic_control/point_mass.py +++ b/extensions/embodichain_tasks/embodichain_tasks/classic_control/point_mass.py @@ -197,6 +197,7 @@ def step(self, action: torch.Tensor) -> tuple[ } self.last_action = action.detach() + info["terminal_observation"] = self._observation() if bool(done.any()): self._reset_ids(torch.nonzero(done, as_tuple=False).squeeze(-1)) return self._observation(), reward, terminated, truncated, info diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/managers/actions.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/managers/actions.py new file mode 100644 index 0000000000..74064b174f --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/managers/actions.py @@ -0,0 +1,62 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Reference-relative position commands for motion-tracking tasks.""" + +from __future__ import annotations +from typing import TYPE_CHECKING +import torch +from embodichain.lab.gym.envs.managers.actions import DefaultJointPositionAction + +if TYPE_CHECKING: + from embodichain.lab.gym.envs.managers.cfg import ActionTermCfg + from embodichain_tasks.locomotion.tracking.g1_motion import UnitreeG1MotionEnv +__all__ = ["ReferenceJointPositionAction"] + + +class ReferenceJointPositionAction(DefaultJointPositionAction): + """Combine policy residuals with each joint's configured reference weight.""" + + contract_id = None + representation = "reference_joint_position" + normalization = "none" + + def __init__(self, cfg: ActionTermCfg, env: UnitreeG1MotionEnv) -> None: + super().__init__(cfg, env) + self.setpoint_actions = torch.zeros_like(self.raw_actions) + self._reference_offset = torch.ones(1, dtype=torch.long, device=env.device) + + def process_actions(self, actions: torch.Tensor) -> None: + self._store_raw(actions) + env = self._env + reference = env.motion.future(env.motion_frame, self._reference_offset) + setpoint, target = env.task.control( + self.raw_actions, reference["q"][:, 0], self.position_bias + ) + self.setpoint_actions.copy_(setpoint) + self._processed_actions.copy_(target) + + def apply_actions(self) -> None: + """Write residual PD targets with the same range used by the model.""" + self._env.robot.set_qpos( + self._processed_actions, + joint_ids=list(self._joint_ids), + clamp_target_qpos=False, + ) + + def reset(self, env_ids: list[int] | torch.Tensor | None = None) -> None: + super().reset(env_ids) + self.setpoint_actions[slice(None) if env_ids is None else env_ids] = 0 diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/managers/events.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/managers/events.py new file mode 100644 index 0000000000..502193b4da --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/managers/events.py @@ -0,0 +1,53 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Reset-time physical context for G1 dynamics learning.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING +import torch + +if TYPE_CHECKING: + from embodichain_tasks.locomotion.tracking.g1_motion import UnitreeG1MotionEnv + +__all__ = ["randomize_motion_context"] + + +def randomize_motion_context(env: UnitreeG1MotionEnv, env_ids: torch.Tensor) -> None: + """Set ground friction, joint encoder bias and torso COM for reset rows. + + The sampled context is retained by the task and stored with each measured + replay transition. The physics scene applies pending property writes before + the reset state is installed. + """ + ids = env.episode_context._ids(env_ids) + if not len(ids): + return + generator = env.get_generator("motion_context") + draw = lambda width: torch.rand( + (len(ids), width), device=env.device, generator=generator + ) + env._friction[ids] = 0.3 + 0.9 * draw(1) + env.position_action.position_bias[ids] = (2 * draw(env.task.dof) - 1) * 0.01 + env._com_offset[ids] = (2 * draw(3) - 1) * env._com_offset.new_tensor( + [0.025, 0.05, 0.05] + ) + env.sim.get_rigid_object("floor").set_friction(env._friction[ids, 0], env_ids=ids) + com = env._default_torso_com[ids].clone() + com[:, :, :3] += env._com_offset[ids, None] + env.robot.set_com_pose(com, link_names=["torso_link"], env_ids=ids) + env.sim.prepare() diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/__init__.py new file mode 100644 index 0000000000..fb1c5e36bb --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Reference motion tracking tasks.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/_task.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/_task.py new file mode 100644 index 0000000000..6252298373 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/_task.py @@ -0,0 +1,387 @@ +# ---------------------------------------------------------------------------- +# 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 tracking calculations shared by physical and model environments.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +import math + +import torch + +from embodichain.compute.kinematics.matrix import matrix_product, matrix_vector_product + +from embodichain.compute.kinematics.features import ( + heading_rotation, + heading_kinematic_features, +) +from embodichain.compute.kinematics.tree import LinkKinematics, TreeKinematics +from embodichain.utils import configclass + +__all__ = ["MotionTrackingCfg", "MotionTrackingTask"] +State = Mapping[str, torch.Tensor] + + +@configclass +class MotionTrackingCfg: + """Tracking objective, history and termination parameters.""" + + control_dt: float = 0.02 + history_steps: int = 10 + future_offsets: tuple[int, ...] = tuple(range(1, 11)) + reward_weights: tuple[float, ...] = (1.0, 1.0, 1.0, 1.0, 0.5, 0.5, -0.1, -2.0) + reward_scales: tuple[float, ...] = (0.3, 0.4, 1.0, 3.14, 0.3, 0.4) + height_threshold: float = 0.25 + tilt_threshold: float = 0.8 + height_temperature: float = 0.02 + tilt_temperature: float = 0.04 + observation_noise: tuple[float, ...] = (0.01, 0.5, 0.2, 0.05) + + def __post_init__(self) -> None: + if ( + not math.isfinite(self.control_dt) + or self.control_dt <= 0 + or self.history_steps < 1 + ): + raise ValueError("Control interval and history length must be positive") + if ( + not self.future_offsets + or tuple(sorted(set(self.future_offsets))) != tuple(self.future_offsets) + or self.future_offsets[0] < 1 + ): + raise ValueError("Future offsets must be positive and strictly increasing") + if ( + len(self.reward_weights) != 8 + or len(self.reward_scales) != 6 + or any(x <= 0 for x in self.reward_scales) + ): + raise ValueError("Tracking requires eight weights and six positive scales") + if min(self.height_temperature, self.tilt_temperature) <= 0: + raise ValueError("Soft termination temperatures must be positive") + + +class MotionTrackingTask: + """Evaluate root/body tracking from tensors without owning episode state. + + State dictionaries contain world-frame ``pos``, ``rot``, ``v``, ``w``, + joint ``q`` and ``qd``. Linear velocity refers to the root body origin. + ``reference_weight`` selects each joint's reference contribution: zero + uses the configured resting pose; one adds a reference-relative residual. + """ + + reward_names = ( + "body_pos", + "body_ori", + "body_lin", + "body_ang", + "root_pos", + "root_ori", + "action_rate", + "joint_limit", + ) + + def __init__( + self, + kinematics: TreeKinematics, + resting_position: torch.Tensor, + action_scale: torch.Tensor, + soft_limits: torch.Tensor, + reference_weight: torch.Tensor, + tracked_links: Sequence[str], + cfg: MotionTrackingCfg | None = None, + ) -> None: + self.cfg = cfg or MotionTrackingCfg() + self.kinematics = kinematics + self.resting_position = resting_position + self.action_scale = action_scale + self.soft_limits = soft_limits + self.reference_weight = reference_weight + self.dof = len(kinematics.joint_names) + if any( + x.shape != (self.dof,) + for x in (resting_position, action_scale, reference_weight) + ) or soft_limits.shape != (self.dof, 2): + raise ValueError( + "Tracking parameters must match the kinematics joint order" + ) + if bool((action_scale <= 0).any()): + raise ValueError("Action scales must be positive") + self.tracked = kinematics.link_indices(tracked_links) + self.moving = kinematics.link_indices(kinematics.moving_link_names[1:]) + self.future_offsets = torch.tensor( + self.cfg.future_offsets, device=kinematics.device, dtype=torch.long + ) + nq, nv, nw, ng = self.cfg.observation_noise + self.noise_scale = resting_position.new_tensor( + [nq] * self.dof + [nv] * self.dof + [nw] * 3 + [ng] * 3 + [0.0] * self.dof + ) + + @property + def observation_dim(self) -> int: + """One proprioceptive frame's feature count.""" + return 3 * self.dof + 6 + + @property + def goal_dim(self) -> int: + """Flattened future reference feature count.""" + return len(self.cfg.future_offsets) * (15 + 2 * self.dof) + + @property + def max_step_reward(self) -> float: + """Sum of the six positive tracking weights times control duration.""" + return sum(self.cfg.reward_weights[:6]) * self.cfg.control_dt + + @staticmethod + def _local(rotation: torch.Tensor, vector: torch.Tensor) -> torch.Tensor: + return matrix_vector_product(rotation.transpose(-1, -2), vector) + + @staticmethod + def _orientation(rotation: torch.Tensor) -> torch.Tensor: + return rotation[..., :, :2].transpose(-1, -2).flatten(-2) + + _heading = staticmethod(heading_rotation) + + @staticmethod + def _angle_squared(rotation: torch.Tensor) -> torch.Tensor: + # Only the angle magnitude enters the reward. atan2 preserves the + # shortest angle at zero and pi without constructing an axis/quaternion. + vector = ( + torch.stack( + ( + rotation[..., 2, 1] - rotation[..., 1, 2], + rotation[..., 0, 2] - rotation[..., 2, 0], + rotation[..., 1, 0] - rotation[..., 0, 1], + ), + -1, + ) + * 0.5 + ) + sine = vector.norm(dim=-1) + cosine = (rotation.diagonal(dim1=-2, dim2=-1).sum(-1) - 1) * 0.5 + return torch.atan2(sine, cosine).square() + + def links(self, state: State) -> LinkKinematics: + """Compute all link poses and link-origin velocities.""" + return self.kinematics.evaluate( + state["pos"], state["rot"], state["q"], state["v"], state["w"], state["qd"] + ) + + def observation( + self, state: State, previous_action: torch.Tensor, encoder_bias: torch.Tensor + ) -> torch.Tensor: + """Joint position/velocity, local gyro/gravity and previous policy action.""" + gravity = -state["rot"][..., 2, :] + return torch.cat( + ( + state["q"] - self.resting_position + encoder_bias, + state["qd"], + self._local(state["rot"], state["w"]), + gravity, + previous_action, + ), + -1, + ) + + def control( + self, + action: torch.Tensor, + reference_qpos: torch.Tensor, + encoder_bias: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + """Return normalized setpoint and the actual encoder-corrected joint target.""" + setpoint = ( + action + + self.reference_weight + * (reference_qpos - self.resting_position) + / self.action_scale + ) + target = self.resting_position + self.action_scale * setpoint - encoder_bias + return setpoint, target + + def goal(self, state: State, future: State) -> torch.Tensor: + """Encode future root pose relative to the robot, joints and local velocities.""" + rotation = state["rot"].unsqueeze(-3) + position = self._local(rotation, future["pos"] - state["pos"].unsqueeze(-2)) + relative = matrix_product(rotation.transpose(-1, -2), future["rot"]) + return torch.cat( + ( + position, + self._orientation(relative), + future["q"], + future["qd"], + self._local(future["rot"], future["v"]), + self._local(future["rot"], future["w"]), + ), + -1, + ).flatten(-2) + + def features( + self, state: State, links: LinkKinematics | None = None + ) -> torch.Tensor: + """Heading-invariant root/joint state and movable-link positions/velocities.""" + links = links or self.links(state) + return heading_kinematic_features( + state["pos"], + state["rot"], + state["v"], + state["w"], + state["q"], + state["qd"], + self.resting_position, + links.position.index_select(-2, self.moving), + links.linear_velocity.index_select(-2, self.moving), + ) + + def tracking_errors( + self, + state: State, + reference: State, + links: LinkKinematics | None = None, + reference_links: LinkKinematics | None = None, + ) -> dict[str, torch.Tensor]: + """Return six squared errors before reward scaling. + + Body pose errors average over tracked links after aligning reference + heading and horizontal root position. Root pose and body velocities + use world coordinates. Position and velocity errors sum over axes; + orientation uses the squared rotation angle in radians. + """ + links = links or self.links(state) + reference_links = reference_links or self.links(reference) + p = links.position.index_select(-2, self.tracked) + r = links.rotation.index_select(-3, self.tracked) + rp = reference_links.position.index_select(-2, self.tracked) + rr = reference_links.rotation.index_select(-3, self.tracked) + alignment = matrix_product( + self._heading(state["rot"]), + self._heading(reference["rot"]).transpose(-1, -2), + ).unsqueeze(-3) + anchor = torch.cat( + (state["pos"][..., :2], reference["pos"][..., 2:3]), -1 + ).unsqueeze(-2) + aligned = anchor + matrix_vector_product( + alignment, rp - reference["pos"].unsqueeze(-2) + ) + errors = ( + (p - aligned).square().sum(-1).mean(-1), + self._angle_squared( + matrix_product(matrix_product(alignment, rr), r.transpose(-1, -2)) + ).mean(-1), + (links.linear_velocity - reference_links.linear_velocity) + .index_select(-2, self.tracked) + .square() + .sum(-1) + .mean(-1), + (links.angular_velocity - reference_links.angular_velocity) + .index_select(-2, self.tracked) + .square() + .sum(-1) + .mean(-1), + (state["pos"] - reference["pos"]).square().sum(-1), + self._angle_squared( + matrix_product(reference["rot"], state["rot"].transpose(-1, -2)) + ), + ) + return dict(zip(self.reward_names[:6], errors)) + + def reward( + self, + state: State, + reference: State, + action: torch.Tensor, + previous_action: torch.Tensor, + links: LinkKinematics | None = None, + reference_links: LinkKinematics | None = None, + ) -> tuple[torch.Tensor, dict[str, torch.Tensor]]: + """Compute weighted tracking terms and the duration-scaled total reward.""" + errors = self.tracking_errors(state, reference, links, reference_links) + values = [ + weight * torch.exp(-error / (scale * scale)) + for weight, scale, error in zip( + self.cfg.reward_weights, self.cfg.reward_scales, errors.values() + ) + ] + values.extend( + ( + self.cfg.reward_weights[6] + * (action - previous_action).square().sum(-1), + self.cfg.reward_weights[7] + * ( + torch.relu(self.soft_limits[:, 0] - state["q"]) + + torch.relu(state["q"] - self.soft_limits[:, 1]) + ).sum(-1), + ) + ) + return sum(values) * self.cfg.control_dt, dict(zip(self.reward_names, values)) + + def analytics( + self, + state: State, + reference: State, + future: State, + action: torch.Tensor, + previous_action: torch.Tensor, + encoder_bias: torch.Tensor, + ) -> dict[str, torch.Tensor | dict[str, torch.Tensor]]: + """Evaluate a physical transition, reusing its link kinematics.""" + links = self.links(state) + reward, terms = self.reward( + state, + reference, + action, + previous_action, + links=links, + reference_links=self.links(reference), + ) + failed, continuation = self.termination(state, reference) + return { + "reward": reward, + "reward_terms": terms, + "failed": failed, + "continuation": continuation, + "observation": self.observation(state, action, encoder_bias), + "goal": self.goal(state, future), + "features": self.features(state, links), + } + + def failure_reasons( + self, state: State, reference: State + ) -> dict[str, torch.Tensor]: + """Return height and tilt failure masks; both may apply to one state. + + Tilt is the absolute difference between the two root rotations' vertical + axis dot products with world up, matching the termination criterion. + """ + return { + "height": (state["pos"][..., 2] - reference["pos"][..., 2]).abs() + > self.cfg.height_threshold, + "tilt": (state["rot"][..., 2, 2] - reference["rot"][..., 2, 2]).abs() + > self.cfg.tilt_threshold, + } + + def termination( + self, state: State, reference: State + ) -> tuple[torch.Tensor, torch.Tensor]: + """Return hard failure and differentiable continuation probability.""" + height = (state["pos"][..., 2] - reference["pos"][..., 2]).abs() + tilt = (state["rot"][..., 2, 2] - reference["rot"][..., 2, 2]).abs() + c = self.cfg + hard = (height > c.height_threshold) | (tilt > c.tilt_threshold) + continuation = torch.sigmoid( + (c.height_threshold - height) / c.height_temperature + ) * torch.sigmoid((c.tilt_threshold - tilt) / c.tilt_temperature) + return hard, continuation diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/g1_motion.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/g1_motion.py new file mode 100644 index 0000000000..be7d2eb68e --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/g1_motion.py @@ -0,0 +1,621 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""G1 reference-motion tracking using simulator state and shared tensor objectives.""" + +from __future__ import annotations + +import copy +import math +import re +from pathlib import Path +from typing import Any +from xml.etree import ElementTree + +import torch +from tensordict import TensorDict + +from embodichain.compute.kinematics.tree import TreeKinematics +from embodichain.data_pipeline.datasets.motion import MotionLibrary +from embodichain.data_pipeline.datasets.motion_sampling import ( + MotionStartSampler, + MotionStartSamplerCfg, +) +from embodichain.data import get_data_path +from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs.managers.cfg import ActionTermCfg, EventCfg +from embodichain.lab.gym.utils.registration import register_env +from embodichain.lab.sim.cfg import ( + ArticulationRootPropertiesCfg, + CollisionPropertiesCfg, + JointDrivePropertiesCfg, + LinkPhysicsOverrideCfg, + RigidBodyPhysicsCfg, + RigidBodyMaterialCfg, +) +from embodichain.utils import configclass +from embodichain.utils.math import matrix_from_quat +from embodichain_robots.locomotion_profiles import load_locomotion_robot_profile +from embodichain_tasks.locomotion.managers.actions import ReferenceJointPositionAction +from ._task import MotionTrackingCfg, MotionTrackingTask +from embodichain_tasks.locomotion.managers.events import randomize_motion_context + +__all__ = ["G1MotionCfg", "UnitreeG1MotionEnv"] + +_TRACKED_LINKS = ( + "pelvis", + "left_hip_roll_link", + "left_knee_link", + "left_ankle_roll_link", + "right_hip_roll_link", + "right_knee_link", + "right_ankle_roll_link", + "torso_link", + "left_shoulder_roll_link", + "left_elbow_link", + "left_wrist_yaw_link", + "right_shoulder_roll_link", + "right_elbow_link", + "right_wrist_yaw_link", +) + + +@configclass +class G1MotionCfg: + """Motion source and episode options for the 29-joint G1 tracking task.""" + + motion_path: str = "LAFAN1G1/motion.npz" + quaternion_order: str = "xyzw" + clips: list[str] | None = None + observation_noise: bool = True + random_start: bool = True + fixed_clip_assignment: bool = False + adaptive_sampling: bool = False + sampler: MotionStartSamplerCfg = MotionStartSamplerCfg() + reset_noise: bool = False + domain_randomization: bool = False + compile_mode: str | None = None + gamma: float = 0.99 + task: MotionTrackingCfg = MotionTrackingCfg() + + +@register_env("UnitreeG1MotionRL-v1", max_episode_steps=500, supports_rl=True) +class UnitreeG1MotionEnv(EmbodiedEnv): + """Track reference clips with absolute leg commands and upper-body residuals. + + ``policy`` contains the ten-frame proprioceptive history followed by future + goals. ``critic`` contains state features, the same goal, physical parameters and + remaining time capped and normalized by 100 reference frames. Terminal observations and state are saved + before automatic resets in ``info['final_observation']`` and + ``info['transition']``. Their tensors own storage. + """ + + def __init__(self, cfg: EmbodiedEnvCfg, **kwargs: Any) -> None: + cfg = copy.deepcopy(cfg) + options = (cfg.extensions or {}).get("motion_tracking", {}) + self.motion_cfg = G1MotionCfg(**options) + if isinstance(self.motion_cfg.task, dict): + self.motion_cfg.task = MotionTrackingCfg(**self.motion_cfg.task) + if isinstance(self.motion_cfg.sampler, dict): + self.motion_cfg.sampler = MotionStartSamplerCfg(**self.motion_cfg.sampler) + if not 0 < self.motion_cfg.gamma < 1: + raise ValueError("Tracking discount must lie in (0, 1).") + if self.motion_cfg.domain_randomization: + events = ( + cfg.events + if isinstance(cfg.events, dict) + else vars(cfg.events) if cfg.events is not None else {} + ) + cfg.events = { + **events, + "motion_context": EventCfg(func=randomize_motion_context, mode="reset"), + } + profile = load_locomotion_robot_profile("UnitreeG1")["robot"] + self.motion_joint_names = tuple(profile["joint_names"]) + resting = { + f"{side}_{joint}_joint": value + for side in ("left", "right") + for joint, value in ( + ("hip_pitch", -0.312), + ("knee", 0.669), + ("ankle_pitch", -0.363), + ("shoulder_pitch", 0.2), + ("elbow", 0.6), + ) + } + resting.update(left_shoulder_roll_joint=0.2, right_shoulder_roll_joint=-0.2) + self._resting = [resting.get(n, 0.0) for n in self.motion_joint_names] + self._scales = [ + 0.25 * e / k for e, k in zip(profile["effort_limit"], profile["stiffness"]) + ] + self._reference_weights = [0.0] * 12 + [1.0] * 17 + robot = cfg.robot + self._urdf = Path(robot.fpath).expanduser().read_text() + robot.init_qpos = self._resting + robot.build_pk_chain = False + robot.root_props = ArticulationRootPropertiesCfg( + fixed_base=False, self_collision_enabled=False + ) + robot.attrs = RigidBodyPhysicsCfg( + material_props=RigidBodyMaterialCfg(dynamic_friction=0.3) + ) + no_collision = [ + re.escape(link.attrib["name"]) + for link in ElementTree.fromstring(self._urdf).findall("link") + if link.find("collision") is None + ] + robot.link_attrs = { + "source_without_collision": LinkPhysicsOverrideCfg( + link_names_expr=no_collision, + attrs=RigidBodyPhysicsCfg( + collision_props=CollisionPropertiesCfg(collision_enabled=False) + ), + ) + } + drive = { + key: dict(zip(self.motion_joint_names, profile[value])) + for key, value in ( + ("stiffness", "stiffness"), + ("damping", "damping"), + ("armature", "armature"), + ("max_effort", "effort_limit"), + ("max_velocity", "velocity_limit"), + ) + } + robot.joint_drive_props = JointDrivePropertiesCfg( + target_mode="position", + friction=0.0, + limit_stiffness=0.0, + limit_damping=0.0, + **drive, + ) + cfg.actions = { + "joint_position": ActionTermCfg( + func=ReferenceJointPositionAction, + params={ + "joint_names": list(self.motion_joint_names), + "preserve_order": True, + "offset": self._resting, + "scale": self._scales, + "clip": None, + }, + ) + } + super().__init__(cfg, **kwargs) + + def _init_sim_state(self, **kwargs: Any) -> None: + c = self.motion_cfg + if not math.isclose(self.step_dt, c.task.control_dt, abs_tol=1e-9): + raise ValueError( + "Motion task control interval differs from simulation timing" + ) + self.motion = MotionLibrary.from_npz( + get_data_path(c.motion_path), + self.motion_joint_names, + quaternion_order=c.quaternion_order, + device=self.device, + clips=c.clips, + ) + if c.fixed_clip_assignment and ( + self.num_envs != self.motion.num_clips + or c.random_start + or c.adaptive_sampling + ): + raise ValueError( + "Fixed clip assignment requires one environment per clip and " + "disables random starts and adaptive sampling." + ) + if not math.isclose(self.motion.dt, self.step_dt, abs_tol=1e-9): + raise ValueError( + "Motion frequency differs from environment control frequency" + ) + kin = TreeKinematics(self._urdf, self.motion_joint_names, device=self.device) + limits = kin.position_limits + mid = limits.mean(-1) + half = (limits[:, 1] - limits[:, 0]) * 0.45 + tensor = lambda x: torch.tensor(x, device=self.device, dtype=torch.float32) + self.task = MotionTrackingTask( + kin, + tensor(self._resting), + tensor(self._scales), + torch.stack((mid - half, mid + half), -1), + tensor(self._reference_weights), + _TRACKED_LINKS, + c.task, + ) + self._task_features = self.task.features + self._task_analytics = self._compute_transition + if c.compile_mode is not None: + # Partial resets have variable row counts. Fuse their features without + # capturing a separate CUDA graph for every reset size. + self._task_features = torch.compile( + self.task.features, + fullgraph=True, + dynamic=True, + options={"triton.cudagraphs": False}, + ) + self._task_analytics = torch.compile( + self._task_analytics, mode=c.compile_mode, fullgraph=True + ) + self.start_sampler = MotionStartSampler(self.motion, c.sampler) + self._episode_start = torch.zeros( + self.num_envs, device=self.device, dtype=torch.long + ) + self._episode_steps = torch.zeros_like(self._episode_start) + self._episode_discounted_return = torch.zeros(self.num_envs, device=self.device) + self.motion_frame = torch.zeros( + self.num_envs, device=self.device, dtype=torch.long + ) + self._history = torch.zeros( + self.num_envs, + c.task.history_steps, + self.task.observation_dim, + device=self.device, + ) + self._goal = torch.zeros(self.num_envs, self.task.goal_dim, device=self.device) + self._features = torch.zeros(self.num_envs, 245, device=self.device) + self._reward = torch.zeros(self.num_envs, device=self.device) + self._failed = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + self._continuation = torch.ones(self.num_envs, device=self.device) + self._reward_terms = {} + self._friction = torch.ones(self.num_envs, 1, device=self.device) + self._com_offset = torch.zeros(self.num_envs, 3, device=self.device) + self.policy_joint_ids = torch.tensor( + [self.robot.joint_names.index(n) for n in self.motion_joint_names], + device=self.device, + ) + self._root_com = self.robot.get_com_pose(["pelvis"])[:, 0, :3].clone() + self._default_torso_com = self.robot.get_com_pose(["torso_link"]).clone() + super()._init_sim_state(**kwargs) + self.position_action = self.action_manager.get_term("joint_position") + self.add_detached_uids_for_reset([self.robot.uid, "floor"]) + # The per-world plane supplies independently configurable ground friction. + for handle in self.sim.spawn_result.handles.values(): + if handle.get_name() == "default_plane": + handle.enable_collision(False) + self.sim.prepare() + self._refresh( + torch.arange(self.num_envs, device=self.device), reset_history=True + ) + + def read_state(self) -> dict[str, torch.Tensor]: + """Return owned root-origin world state in the motion joint order.""" + data = self.robot.body_data.fetch_state() + pose = data["root_pose"] + rotation = matrix_from_quat(pose[:, 3:7]) + angular = data["root_ang_vel"] + offset = (rotation @ self._root_com.unsqueeze(-1)).squeeze(-1) + return { + "pos": pose[:, :3].clone(), + "rot": rotation, + "quat": pose[:, 3:7].clone(), + "q": data["qpos"].index_select(1, self.policy_joint_ids), + "v": data["root_lin_vel"] - torch.cross(angular, offset, dim=-1), + "w": angular.clone(), + "qd": data["qvel"].index_select(1, self.policy_joint_ids), + } + + def capture_transition_start(self) -> dict[str, Any]: + """Capture owned pre-action state, reference progress and physical context. + + Pair this snapshot with ``info['transition']`` after stepping. The latter + holds the successor before auto-reset, even on a terminal transition. + """ + return { + "state": self.read_state(), + "frame": self.motion_frame.clone(), + "context": torch.cat( + (self._friction, self.position_action.position_bias, self._com_offset), + -1, + ), + "previous_action": self.position_action.raw_actions.clone(), + } + + def write_state( + self, env_ids: torch.Tensor, state: dict[str, torch.Tensor] + ) -> None: + """Reset selected physical rows from world root-origin state.""" + from embodichain.utils.math import quat_from_matrix + + if env_ids.numel() == 0: + return + rotation = state["rot"] + angular = state["w"] + offset = (rotation @ self._root_com[env_ids].unsqueeze(-1)).squeeze(-1) + velocity = state["v"] + torch.cross(angular, offset, dim=-1) + self.robot.set_state( + env_ids=env_ids, + joint_ids=self.policy_joint_ids.tolist(), + root_pose=torch.cat((state["pos"], quat_from_matrix(rotation)), -1), + qpos=state["q"], + target_qpos=state["q"], + qvel=state["qd"], + root_velocity=torch.cat((velocity, angular), -1), + clear_dynamics=True, + ) + + def _initialize_episode( + self, env_ids: torch.Tensor | None = None, **kwargs: Any + ) -> None: + super()._initialize_episode(env_ids, **kwargs) + ids = self.episode_context._ids(env_ids) + frames = kwargs.get("motion_frames") + if frames is None and self.motion_cfg.fixed_clip_assignment: + frames = self.motion.starts[ids] + if ( + frames is None + and self.motion_cfg.adaptive_sampling + and self.motion_cfg.random_start + ): + frames = self.start_sampler.sample( + len(ids), self.get_generator("motion_reset") + ) + if frames is None: + generator = self.get_generator("motion_reset") + clips = torch.randint( + self.motion.num_clips, + (len(ids),), + device=self.device, + generator=generator, + ) + frames = self.motion.starts[clips].clone() + if self.motion_cfg.random_start: + frames += ( + torch.rand(len(ids), device=self.device, generator=generator) + * (self.motion.lengths[clips] - 1) + ).long() + frames = torch.as_tensor(frames, device=self.device, dtype=torch.long) + if frames.shape != ids.shape or bool( + ((frames < 0) | (frames >= self.motion.num_frames)).any() + ): + raise ValueError("motion_frames must provide one valid frame per reset row") + self.motion_frame[ids] = frames + self._episode_start[ids] = frames + self._episode_steps[ids] = 0 + self._episode_discounted_return[ids] = 0 + state = self.motion.frame(frames) + if self.motion_cfg.reset_noise: + from embodichain.utils.math import matrix_from_euler + + generator = self.get_generator("motion_reset_noise") + draw = ( + lambda width: torch.rand( + (len(ids), width), device=self.device, generator=generator + ) + * 2 + - 1 + ) + pose = draw(6) * state["pos"].new_tensor([0.05, 0.05, 0.01, 0.1, 0.1, 0.2]) + velocity = draw(6) * state["pos"].new_tensor( + [0.5, 0.5, 0.2, 0.52, 0.52, 0.78] + ) + state["pos"] = state["pos"] + pose[:, :3] + state["rot"] = matrix_from_euler(pose[:, 3:].flip(-1), "ZYX") @ state["rot"] + state["q"] = (state["q"] + draw(self.task.dof) * 0.1).clamp( + self.task.soft_limits[:, 0], self.task.soft_limits[:, 1] + ) + state["v"] = state["v"] + velocity[:, :3] + state["w"] = state["w"] + velocity[:, 3:] + self.write_state(ids, state) + self._refresh(ids, reset_history=True) + self._reward[ids] = 0 + self._failed[ids] = False + self._continuation[ids] = 1 + + def _refresh( + self, + ids: torch.Tensor | None, + *, + reset_history: bool, + state: dict[str, torch.Tensor] | None = None, + analytics: dict[str, Any] | None = None, + ) -> None: + rows = slice(None) if ids is None else ids + if analytics is None: + state = { + k: v[rows] + for k, v in (self.read_state() if state is None else state).items() + } + obs = ( + self.task.observation( + state, + self.position_action.raw_actions[rows], + self.position_action.position_bias[rows], + ) + if analytics is None + else analytics["observation"][rows] + ) + if self.motion_cfg.observation_noise: + obs = ( + obs + + ( + torch.rand( + obs.shape, + device=self.device, + generator=self.get_generator("motion_observation"), + ) + * 2 + - 1 + ) + * self.task.noise_scale + ) + if reset_history: + self._history[rows] = obs.unsqueeze(1) + else: + self._history[rows] = torch.cat( + (self._history[rows, 1:], obs.unsqueeze(1)), 1 + ) + self._goal[rows] = ( + self.task.goal( + state, + self.motion.future(self.motion_frame[rows], self.task.future_offsets), + ) + if analytics is None + else analytics["goal"][rows] + ) + self._features[rows] = ( + self._task_features(state) + if analytics is None + else analytics["features"][rows] + ) + + def _compute_transition( + self, + state: dict[str, torch.Tensor], + reference: dict[str, torch.Tensor], + future: dict[str, torch.Tensor], + action: torch.Tensor, + previous_action: torch.Tensor, + bias: torch.Tensor, + ) -> dict[str, Any]: + """Evaluate finite state rows and task outputs in one compiled function.""" + valid = torch.stack( + [torch.isfinite(value).flatten(1).all(1) for value in state.values()] + ).all(0) + # Invalid rows use finite reference values for task arithmetic and are + # excluded from dynamics supervision by the returned validity mask. + state = { + key: torch.where( + valid.reshape((-1,) + (1,) * (value.ndim - 1)), value, reference[key] + ) + for key, value in state.items() + } + result = self.task.analytics( + state, reference, future, action, previous_action, bias + ) + result["state"], result["valid"] = state, valid + result["failed"] = result["failed"] | ~valid + result["reward"] = torch.where(valid, result["reward"], 0) + result["reward_terms"] = { + name: torch.where(valid, value, 0) + for name, value in result["reward_terms"].items() + } + result["continuation"] = torch.where(valid, result["continuation"], 0) + return result + + def _update_sim_state(self, **kwargs: Any) -> None: + super()._update_sim_state(**kwargs) + self.motion_frame = torch.minimum( + self.motion_frame + 1, + self.motion.ends[self.motion.clip_ids(self.motion_frame)] - 1, + ) + state = self.read_state() + reference = self.motion.frame(self.motion_frame) + analytics = self._task_analytics( + state, + reference, + self.motion.future(self.motion_frame, self.task.future_offsets), + self.position_action.raw_actions, + self.position_action.previous_raw_actions, + self.position_action.position_bias, + ) + self._transition_valid = analytics["valid"] + self._transition_state = analytics["state"] + # Reset invokes another compiled function. Keep mutable episode fields + # in owned buffers rather than retaining CUDA graph output storage. + self._reward.copy_(analytics["reward"]) + self._reward_terms = analytics["reward_terms"] + self._episode_discounted_return += ( + self.motion_cfg.gamma**self._episode_steps * self._reward + ) + self._episode_steps += 1 + self._failed.copy_(analytics["failed"]) + self._continuation.copy_(analytics["continuation"]) + self._refresh( + None, + reset_history=False, + analytics=analytics, + ) + + def get_obs(self, **kwargs: Any) -> TensorDict: + remaining = ( + self.motion.remaining(self.motion_frame) + .float() + .clamp(max=100) + .unsqueeze(-1) + / 100.0 + ) + parameters = torch.cat( + (self._friction, self.position_action.position_bias, self._com_offset), -1 + ) + return TensorDict( + { + "policy": torch.cat((self._history.flatten(1), self._goal), -1), + "critic": torch.cat( + (self._features, self._goal, parameters, remaining), -1 + ), + }, + batch_size=[self.num_envs], + device=self.device, + ) + + def compute_task_state( + self, **kwargs: Any + ) -> tuple[torch.Tensor, torch.Tensor, dict[str, torch.Tensor]]: + """Expose tracking failure through the simulator RL termination contract.""" + return ( + torch.zeros_like(self._failed), + self._failed.clone(), + {"tracking_reward": self._reward.clone()}, + ) + + def check_truncated(self, obs: TensorDict, info: dict[str, Any]) -> torch.Tensor: + return self.motion.remaining(self.motion_frame) == 0 + + def get_reward( + self, obs: TensorDict, action: torch.Tensor, info: dict[str, Any] + ) -> torch.Tensor: + return self._reward.clone() + + def _hook_after_sim_step( + self, + obs: TensorDict, + action: torch.Tensor, + rewards: torch.Tensor, + dones: torch.Tensor, + info: dict[str, Any], + **kwargs: Any, + ) -> None: + kwargs["truncateds"].logical_and_(~kwargs["terminateds"]) + if self.motion_cfg.adaptive_sampling: + ids = torch.nonzero(dones, as_tuple=False).flatten() + if len(ids): + horizon = self.motion.remaining(self._episode_start[ids]).clamp_max( + self.max_episode_steps + ) + maximum = ( + self.task.max_step_reward + * (1 - self.motion_cfg.gamma**horizon) + / (1 - self.motion_cfg.gamma) + ) + ratio = self._episode_discounted_return[ids] / maximum + self.start_sampler.update(self._episode_start[ids], ratio) + info["completed_normalized_return"] = ratio.clone() + info["final_observation"] = obs.clone() + info["_final_observation"] = dones.clone() + info["transition"] = { + "state": {k: v.clone() for k, v in self._transition_state.items()}, + "usable": self._transition_valid.clone(), + "frame": self.motion_frame.clone(), + "history": self._history.clone(), + "goal": self._goal.clone(), + "action": self.position_action.raw_actions.clone(), + "setpoint_action": self.position_action.setpoint_actions.clone(), + "target_qpos": self.position_action.processed_actions.clone(), + "continuation": self._continuation.clone(), + } + info["reward_terms"] = {k: v.clone() for k, v in self._reward_terms.items()} + super()._hook_after_sim_step(obs, action, rewards, dones, info, **kwargs) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/motion_model.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/motion_model.py new file mode 100644 index 0000000000..58f2f673a8 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/tracking/motion_model.py @@ -0,0 +1,474 @@ +# ---------------------------------------------------------------------------- +# 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 reference-motion rollouts from measured replay starts.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping +from dataclasses import dataclass +from typing import Any + +import gymnasium as gym +import torch +from tensordict import TensorDict + +from embodichain.compute.kinematics import LinkKinematics +from embodichain.data_pipeline.datasets.motion import MotionLibrary +from embodichain.learning.rl.env import DifferentiableRolloutSpec, register_learning_env +from embodichain.utils import configclass +from embodichain_models.dynamics.rwm import KinematicWorldModel, TransitionReplay +from embodichain_models.dynamics.rwm.robot_state import unpack_robot_state +from ._task import MotionTrackingTask + +__all__ = [ + "MotionModelCfg", + "MotionModelStart", + "MotionModelEnv", + "replay_motion_starts", +] + + +@dataclass(frozen=True) +class MotionModelStart: + """Owned, detached measured state and clean oldest-first observation history.""" + + state: torch.Tensor + context: torch.Tensor + frame: torch.Tensor + history: torch.Tensor + previous_action: torch.Tensor + valid: torch.Tensor + + +@torch.no_grad() +def replay_motion_starts( + replay: TransitionReplay, batch_size: int, task: MotionTrackingTask +) -> MotionModelStart: + """Sample usable starts with episode-padded history and preserved encoder bias.""" + for _ in range(4): + data = replay.sample_starts(batch_size * 4, task.cfg.history_steps) + mask = ( + data["history_complete"] + & data["start"]["usable"] + & data["history"]["usable"].all(-1) + ) + if int(mask.sum()) >= batch_size: + start = {k: v[mask][:batch_size] for k, v in data["start"].items()} + history = {k: v[mask][:batch_size] for k, v in data["history"].items()} + observations = task.observation( + unpack_robot_state(history["state"]), + history["previous_action"], + history["context"][..., 1 : 1 + task.dof], + ) + return MotionModelStart( + start["state"], + start["context"], + start["frame"], + observations, + start["previous_action"], + start["usable"], + ) + raise RuntimeError("Replay has too few usable starts with complete history.") + + +@configclass +class MotionModelCfg: + """Short-horizon prediction and training-time perturbations.""" + + horizon: int = 32 + observation_noise: bool = True + goal_position_noise: float = 0.0 + action_noise: float = 0.0 + clean_previous_action: bool = False + compile_mode: str | None = None + + +class MotionModelEnv: + """Run a fixed-horizon model trajectory using the physical task's tensor rules. + + A workflow injects measured starts or a provider that samples replay at reset. + Motion-clip suffixes are marked invalid; soft continuation remains differentiable. + Finite-horizon bootstrap is handled by APG. Each complete rollout is reset by + the trainer. WM parameters/statistics are frozen by the surrounding workflow + until all dependent backward work finishes. + """ + + def __init__( + self, + num_envs: int, + device: torch.device | str, + *, + model: KinematicWorldModel, + motion: MotionLibrary, + task: MotionTrackingTask, + cfg: MotionModelCfg | None = None, + start_provider: Callable[[], MotionModelStart] | None = None, + ) -> None: + self.num_envs = num_envs + self.device = torch.empty(0, device=device).device + self.model, self.motion, self.task = model, motion, task + self.cfg = cfg or MotionModelCfg() + if ( + num_envs < 1 + or self.cfg.horizon < 1 + or min(self.cfg.action_noise, self.cfg.goal_position_noise) < 0 + ): + raise ValueError("Invalid motion-model batch, horizon or noise.") + if tuple(task.kinematics.joint_names) != tuple(model.cfg.joint_names) or tuple( + motion.joint_names + ) != tuple(model.cfg.joint_names): + raise ValueError("Model, motion and task joint order must match.") + if ( + motion.device != self.device + or task.kinematics.device != self.device + or not abs(motion.dt - model.cfg.step_dt) < 1e-8 + ): + raise ValueError( + "Model environment device/control period differs from motion data." + ) + if ( + not torch.equal(task.resting_position, model.resting_position) + or task.kinematics.urdf != model.urdf + ): + raise ValueError( + "Task FK features must use the model geometry and resting pose." + ) + self._transition_fn = ( + self._transition + if self.cfg.compile_mode is None + else torch.compile( + self._transition, mode=self.cfg.compile_mode, fullgraph=True + ) + ) + self._reference_links_fn = ( + self.task.links + if self.cfg.compile_mode is None + else torch.compile( + self.task.links, mode=self.cfg.compile_mode, fullgraph=True + ) + ) + self.start_provider = start_provider + self._start: MotionModelStart | None = None + self._steps = 0 + self._generator = torch.Generator(device=self.device) + self._generator.manual_seed(torch.initial_seed()) + policy_dim = task.observation_dim * task.cfg.history_steps + task.goal_dim + critic_dim = model.feature_dim + task.goal_dim + model.cfg.context_dim + 1 + self.single_observation_space = gym.spaces.Dict( + { + "policy": gym.spaces.Box(-float("inf"), float("inf"), (policy_dim,)), + "critic": gym.spaces.Box(-float("inf"), float("inf"), (critic_dim,)), + } + ) + self.single_action_space = gym.spaces.Box( + -float("inf"), float("inf"), (task.dof,) + ) + + def set_starts(self, starts: MotionModelStart) -> None: + """Copy a batch so replay replacement cannot change an outstanding rollout.""" + expected = { + "state": (self.num_envs, self.model.state_dim), + "context": (self.num_envs, self.model.cfg.context_dim), + "frame": (self.num_envs,), + "history": ( + self.num_envs, + self.task.cfg.history_steps, + self.task.observation_dim, + ), + "previous_action": (self.num_envs, self.task.dof), + "valid": (self.num_envs,), + } + values = {} + for name, shape in expected.items(): + value = getattr(starts, name) + if value.shape != shape or value.device != self.device: + raise ValueError( + f"Invalid imagination start field {name}; expected {shape} on {self.device}." + ) + values[name] = value.detach().clone() + self._start = MotionModelStart(**values) + + def prepare_differentiable_rollout( + self, rollout_index: int + ) -> DifferentiableRolloutSpec: + """Request one independent, complete imagination horizon.""" + return DifferentiableRolloutSpec(self.cfg.horizon) + + def reset( + self, *, seed: int | None = None, options: Mapping[str, Any] | None = None + ) -> tuple[TensorDict, dict[str, Any]]: + """Start from a replay sample, adding fresh noise to its clean history.""" + if seed is not None: + self._generator.manual_seed(seed) + if options and "starts" in options: + self.set_starts(options["starts"]) + elif self.start_provider is not None: + self.set_starts(self.start_provider()) + if self._start is None: + raise RuntimeError( + "Inject measured starts before resetting the model environment." + ) + s = self._start + self.state = s.state.clone() + self.context, self.frame = s.context.clone(), s.frame.clone() + self.previous_action = s.previous_action.clone() + self.history = s.history.clone() + self._observation_noise(s.history.shape) + self._steps = 0 + with torch.no_grad(): + offsets = torch.arange( + self.cfg.horizon + int(self.task.future_offsets[-1]) + 1, + device=self.device, + ) + self._reference = self.motion.future(self.frame, offsets) + raw = self.frame[:, None] + offsets[None] + self._valid = ( + raw < self.motion.ends[self.motion.clip_ids(self.frame), None] + ) & s.valid[:, None] + self._remaining = ( + self.motion.ends[self.motion.clip_ids(self.frame), None] - raw - 1 + ).clamp(0, 100).float() / 100 + ref = {k: v[:, : self.cfg.horizon + 1] for k, v in self._reference.items()} + self._reference_links = self._reference_links_fn(ref) + return self._refresh(), {} + + def _observation_noise(self, shape: torch.Size | tuple[int, ...]) -> torch.Tensor: + if not self.cfg.observation_noise: + return torch.zeros(shape, device=self.device) + return ( + torch.rand(shape, device=self.device, generator=self._generator) * 2 - 1 + ) * self.task.noise_scale + + def _refresh(self, *, noise_goal: bool = True) -> TensorDict: + state = unpack_robot_state(self.state) + links = self.task.links(state) + self._features = self.task.features(state, links) + future = { + k: v[:, self._steps + self.task.future_offsets] + for k, v in self._reference.items() + } + clean_goal = self.task.goal(state, future) + goal = clean_goal + if noise_goal and self.cfg.goal_position_noise: + offsets = len(self.task.cfg.future_offsets) + split = goal.reshape(*goal.shape[:-1], offsets, goal.shape[-1] // offsets) + noise = ( + torch.rand( + split[..., :3].shape, device=self.device, generator=self._generator + ) + * 2 + - 1 + ) * self.cfg.goal_position_noise + goal = torch.cat((split[..., :3] + noise, split[..., 3:]), -1).flatten(-2) + self._observation = TensorDict( + { + "policy": torch.cat((self.history.flatten(1), goal), -1), + "critic": torch.cat( + ( + self._features, + clean_goal, + self.context, + self._remaining[:, self._steps, None], + ), + -1, + ), + }, + batch_size=[self.num_envs], + device=self.device, + ) + return self._observation + + def step( + self, action: torch.Tensor + ) -> tuple[TensorDict, torch.Tensor, torch.Tensor, torch.Tensor, dict[str, Any]]: + """Advance state/history and expose graph-connected reward and continuation.""" + if self._steps >= self.cfg.horizon: + raise RuntimeError( + "Reset before collecting another complete imagination rollout." + ) + index = self._steps + executed = action + if self.cfg.action_noise: + executed = ( + action + + torch.randn( + action.shape, device=self.device, generator=self._generator + ) + * self.cfg.action_noise + ) + reference = {k: v[:, index + 1] for k, v in self._reference.items()} + future = { + k: v[:, index + 1 + self.task.future_offsets] + for k, v in self._reference.items() + } + ref_links = LinkKinematics( + **{ + name: getattr(self._reference_links, name)[:, index + 1] + for name in ( + "position", + "rotation", + "linear_velocity", + "angular_velocity", + ) + } + ) + previous_action = action if self.cfg.clean_previous_action else executed + noise = self._observation_noise((self.num_envs, self.task.observation_dim)) + goal_noise = None + if self.cfg.goal_position_noise: + goal_noise = ( + torch.rand( + (self.num_envs, len(self.task.cfg.future_offsets), 3), + device=self.device, + generator=self._generator, + ) + * 2 + - 1 + ) * self.cfg.goal_position_noise + ( + self.state, + self.history, + self._features, + policy_observation, + critic_observation, + reward, + terms, + hard, + continuation, + ) = self._transition_fn( + self.state, + self.history, + self._features, + self.context, + executed, + previous_action, + self.previous_action, + reference, + future, + ref_links, + self._remaining[:, index + 1, None], + noise, + goal_noise, + ) + self.previous_action = previous_action + self._steps += 1 + self._observation = output = TensorDict( + {"policy": policy_observation, "critic": critic_observation}, + batch_size=[self.num_envs], + device=self.device, + ) + done = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + return ( + output, + reward, + done, + done.clone(), + { + "continuation": continuation, + "valid": self._valid[:, index], + "next_valid": self._valid[:, index + 1], + "hard_failure": hard.detach(), + "reward_terms": {k: v.detach() for k, v in terms.items()}, + }, + ) + + def _transition( + self, + state: torch.Tensor, + history: torch.Tensor, + features: torch.Tensor, + context: torch.Tensor, + executed: torch.Tensor, + action: torch.Tensor, + previous: torch.Tensor, + reference: Mapping[str, torch.Tensor], + future: Mapping[str, torch.Tensor], + ref_links: LinkKinematics, + remaining: torch.Tensor, + noise: torch.Tensor, + goal_noise: torch.Tensor | None, + ) -> tuple: + """Evaluate one tensor transition; RNG and episode bookkeeping stay outside.""" + bias = context[:, 1 : 1 + self.task.dof] + setpoint, _ = self.task.control(executed, reference["q"], bias) + predicted = self.model(state, setpoint, context, features=features) + state_dict = unpack_robot_state(predicted) + observation = self.task.observation(state_dict, action, bias) + noise + history = torch.cat((history[:, 1:], observation[:, None]), 1) + links = self.task.links(state_dict) + features = self.task.features(state_dict, links) + goal = self.task.goal(state_dict, future) + policy_goal = goal + if goal_noise is not None: + offsets = len(self.task.cfg.future_offsets) + split = goal.reshape(*goal.shape[:-1], offsets, goal.shape[-1] // offsets) + policy_goal = torch.cat( + (split[..., :3] + goal_noise, split[..., 3:]), -1 + ).flatten(-2) + policy_observation = torch.cat((history.flatten(1), policy_goal), -1) + critic_observation = torch.cat((features, goal, context, remaining), -1) + reward, terms = self.task.reward( + state_dict, reference, action, previous, links, ref_links + ) + hard, continuation = self.task.termination(state_dict, reference) + return ( + predicted, + history, + features, + policy_observation, + critic_observation, + reward, + {name: value.detach() for name, value in terms.items()}, + hard, + continuation, + ) + + def detach_state(self) -> TensorDict: + """Cut recurrent gradients while preserving state, reference progress and noise.""" + self.state, self.history, self.previous_action = ( + self.state.detach(), + self.history.detach(), + self.previous_action.detach(), + ) + self._features = self._features.detach() + self._observation = self._observation.detach() + return self._observation + + def close(self) -> None: + """Release no external resources; this environment owns tensor state only.""" + + +@register_learning_env("MotionTrackingModelRL") +def _build_motion_model_environment( + num_envs: int, + device: torch.device, + *, + model: KinematicWorldModel, + motion: MotionLibrary, + task: MotionTrackingTask, + start_provider: Callable[[], MotionModelStart], + **cfg: Any, +) -> MotionModelEnv: + """Combine scalar configuration with workflow-owned model and replay resources.""" + return MotionModelEnv( + num_envs, + device, + model=model, + motion=motion, + task=task, + cfg=MotionModelCfg(**cfg), + start_provider=start_provider, + ) diff --git a/scripts/benchmark/rl/locomotion_ppo.py b/scripts/benchmark/rl/locomotion_ppo.py index 2e98c14f98..e7987c0588 100644 --- a/scripts/benchmark/rl/locomotion_ppo.py +++ b/scripts/benchmark/rl/locomotion_ppo.py @@ -31,7 +31,7 @@ BACKENDS = { "default": "Dexsim Default CUDA", - "newton": "Dexsim Newton/MJWarp CUDA", + "newton": "Dexsim Newton CUDA", } TASK_DIR = ( Path(__file__).resolve().parents[3] @@ -90,11 +90,13 @@ def _check_backend(env: Any, name: str) -> Any: from dexsim.engine.newton_physics.backend_registry import get_newton_backend backend = get_newton_backend(sim._world) + solver = backend.mujoco_solver if ( - backend.solver.use_mujoco_cpu - or not backend.solver.mjw_data.qpos.device.is_cuda + solver is None + or solver.use_mujoco_cpu + or not solver.mjw_data.qpos.device.is_cuda ): - raise RuntimeError("Newton/MJWarp is not using CUDA") + raise RuntimeError("Newton MuJoCo solver is not using CUDA") return backend if not (sim._world_config.enable_gpu_sim and sim._world_config.direct_gpu_api): raise RuntimeError("Default backend is not using direct GPU simulation") @@ -250,9 +252,13 @@ def _run(args: argparse.Namespace, result: dict) -> None: env_config = yaml.safe_load(Path(config["trainer"]["gym_config"]).read_text()) env_config["num_envs"] = args.envs if args.backend == "newton": - env_config["physics_config"]["solver_cfg"][ - "iterations" - ] = args.solver_iterations + solver_cfg = env_config["physics_config"]["solver_cfg"] + solver_options = ( + solver_cfg.setdefault("mujoco_options", {}) + if solver_cfg.get("solver_type") == "dexuni" + else solver_cfg + ) + solver_options["iterations"] = args.solver_iterations env_path = args.output / "env.yaml" env_path.write_text(yaml.safe_dump(env_config, sort_keys=False)) config["trainer"]["gym_config"] = str(env_path.resolve()) @@ -313,7 +319,7 @@ def _run(args: argparse.Namespace, result: dict) -> None: if not all(math.isfinite(float(value)) for value in losses.values()): raise RuntimeError("Nonfinite PPO loss") if backend is not None and bool( - backend.solver.mjw_data.overflow.numpy().any() + backend.mujoco_solver.mjw_data.overflow.numpy().any() ): raise RuntimeError("MJWarp capacity or solver iteration limit reached") result["updates"].append( diff --git a/tests/benchmark/test_locomotion_ppo.py b/tests/benchmark/test_locomotion_ppo.py index 0d9d038825..eaa51be380 100644 --- a/tests/benchmark/test_locomotion_ppo.py +++ b/tests/benchmark/test_locomotion_ppo.py @@ -120,6 +120,52 @@ def test_config_rejects_incomplete_minibatches(envs: int, steps: int) -> None: _load_config(TASK_DIR, "default", envs, steps) +@pytest.mark.no_sim +@pytest.mark.parametrize("solver_type", ["mujoco_warp", "dexuni"]) +@pytest.mark.parametrize( + "use_cpu,device_is_cuda", [(False, True), (True, False), (False, False)] +) +def test_newton_backend_checks_active_mujoco_solver( + monkeypatch: pytest.MonkeyPatch, + solver_type: str, + use_cpu: bool, + device_is_cuda: bool, +) -> None: + """Both solver routes must use GPU integration and GPU state buffers.""" + from dexsim.engine.newton_physics import backend_registry + + mujoco = SimpleNamespace( + use_mujoco_cpu=use_cpu, + mjw_data=SimpleNamespace( + qpos=SimpleNamespace(device=SimpleNamespace(is_cuda=device_is_cuda)) + ), + ) + backend = SimpleNamespace( + solver_type=solver_type, + solver=( + mujoco + if solver_type == "mujoco_warp" + else SimpleNamespace(mujoco_solver=mujoco) + ), + mujoco_solver=mujoco, + ) + monkeypatch.setattr(backend_registry, "get_newton_backend", lambda world: backend) + env = SimpleNamespace( + unwrapped=SimpleNamespace( + sim=SimpleNamespace( + physics=SimpleNamespace(name="newton"), + device="cuda:0", + _world=object(), + ) + ) + ) + if not use_cpu and device_is_cuda: + assert benchmark._check_backend(env, "newton") is backend + else: + with pytest.raises(RuntimeError, match="not using CUDA"): + benchmark._check_backend(env, "newton") + + @pytest.mark.parametrize("status", ["passed", "failed"]) def test_report_does_not_rank_training_as_task_success(tmp_path, status: str) -> None: result = _result(status) @@ -220,9 +266,20 @@ def read_text(path: Path, *args, **kwargs): @pytest.mark.no_sim -@pytest.mark.parametrize("renderer", [None, "auto", "no-render", "hybrid"]) +@pytest.mark.parametrize( + "renderer,solver_type", + [ + (None, "mujoco_warp"), + ("auto", "dexuni"), + ("no-render", "mujoco_warp"), + ("hybrid", "dexuni"), + ], +) def test_creation_failure_retains_resource_samples( - tmp_path: Path, monkeypatch: pytest.MonkeyPatch, renderer: str | None + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + renderer: str | None, + solver_type: str, ) -> None: """Write failure reports with samples taken before runtime construction.""" from embodichain.lab.gym.utils import registration @@ -230,7 +287,9 @@ def test_creation_failure_retains_resource_samples( from scripts.benchmark.rl import runtime as benchmark_runtime env_config = tmp_path / "input.yaml" - env_config.write_text("physics_config:\n solver_cfg: {}\n") + env_config.write_text( + f"physics_config:\n solver_cfg:\n solver_type: {solver_type}\n" + ) output = tmp_path / "result" monkeypatch.setattr( sys, @@ -243,6 +302,8 @@ def test_creation_failure_retains_resource_samples( str(output), "--envs", "32", + "--solver-iterations", + "37", ], ) trainer_cfg = {"gym_config": str(env_config)} @@ -280,7 +341,19 @@ def sample(resources: ProcessResources, label: str) -> dict: failure = RuntimeError("runtime construction failed") def fail_creation(*args, **kwargs): + import yaml + assert kwargs["renderer"] == renderer + saved = yaml.safe_load((output / "env.yaml").read_text()) + expected_options = ( + {"mujoco_options": {"iterations": 37}} + if solver_type == "dexuni" + else {"iterations": 37} + ) + assert saved["physics_config"]["solver_cfg"] == { + "solver_type": solver_type, + **expected_options, + } raise failure monkeypatch.setattr(policy_runtime, "build_gym_policy_runtime", fail_creation) @@ -292,9 +365,7 @@ def fail_creation(*args, **kwargs): assert result["resources"]["samples"] == [ {"label": "before_create", "cpu_rss_mib": 50.0} ] - assert ( - "| Dexsim Newton/MJWarp CUDA | failed |" in (output / "report.md").read_text() - ) + assert "| Dexsim Newton CUDA | failed |" in (output / "report.md").read_text() @pytest.mark.no_sim diff --git a/tests/compute/kinematics/test_matrix.py b/tests/compute/kinematics/test_matrix.py new file mode 100644 index 0000000000..31faeccca0 --- /dev/null +++ b/tests/compute/kinematics/test_matrix.py @@ -0,0 +1,92 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Compiled matrix geometry agrees with Torch products, including gradients.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.compute.kinematics.matrix import matrix_product, matrix_vector_product + + +@pytest.mark.parametrize("compiled, size", [(False, 3), (True, 3), (True, 4)]) +def test_broadcast_noncontiguous_products_and_gradients(compiled, size): + torch.manual_seed(71) + left = ( + torch.randn(2, 1, size, size, dtype=torch.float64) + .transpose(-1, -2) + .requires_grad_() + ) + right = ( + torch.randn(1, 4, size, size, dtype=torch.float64) + .transpose(-1, -2) + .requires_grad_() + ) + vector = torch.randn(1, 4, size, dtype=torch.float64, requires_grad=True) + + def evaluate(left, right, vector): + return matrix_product(left, right), matrix_vector_product(left, vector) + + fn = ( + torch.compile(evaluate, backend="aot_eager", fullgraph=True) + if compiled + else evaluate + ) + actual = fn(left, right, vector) + expected = (left @ right, (left @ vector[..., None]).squeeze(-1)) + for value, reference in zip(actual, expected): + torch.testing.assert_close(value, reference, rtol=1e-12, atol=1e-12) + gradients = torch.autograd.grad( + sum(value.square().sum() for value in actual), (left, right, vector) + ) + reference_gradients = torch.autograd.grad( + sum(value.square().sum() for value in expected), (left, right, vector) + ) + for value, reference in zip(gradients, reference_gradients): + torch.testing.assert_close(value, reference, rtol=1e-12, atol=1e-12) + + +def test_compiled_autocast_keeps_torch_product_precision(): + matrix = torch.randn(2, 3, 3) + vector = torch.randn(2, 3) + + def evaluate(matrix, vector): + return matrix_product(matrix, matrix), matrix_vector_product(matrix, vector) + + fn = torch.compile(evaluate, backend="aot_eager", fullgraph=True) + with torch.autocast("cpu", dtype=torch.bfloat16): + actual = fn(matrix, vector) + expected = (matrix @ matrix, (matrix @ vector[..., None]).squeeze(-1)) + for value, reference in zip(actual, expected): + assert value.dtype == torch.bfloat16 + torch.testing.assert_close(value, reference) + + +def test_compiled_integer_products_preserve_dtype(): + matrix = torch.arange(18, dtype=torch.int32).reshape(2, 3, 3) + vector = torch.arange(6, dtype=torch.int32).reshape(2, 3) + + def evaluate(matrix, vector): + return matrix_product(matrix, matrix), matrix_vector_product(matrix, vector) + + actual = torch.compile(evaluate, backend="aot_eager", fullgraph=True)( + matrix, vector + ) + expected = (matrix @ matrix, (matrix @ vector[..., None]).squeeze(-1)) + for value, reference in zip(actual, expected): + torch.testing.assert_close(value, reference, rtol=0, atol=0) diff --git a/tests/compute/kinematics/test_tree.py b/tests/compute/kinematics/test_tree.py new file mode 100644 index 0000000000..9e8d385e5d --- /dev/null +++ b/tests/compute/kinematics/test_tree.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. +# ---------------------------------------------------------------------------- + +"""Tree kinematics contracts on an analytically solvable branched model.""" + +from __future__ import annotations + +import math +import pytest +import torch + +from embodichain.compute.kinematics.tree import TreeKinematics + +URDF = """ + + + + + +""" + + +def test_branched_pose_and_link_origin_velocity(): + with pytest.raises(ValueError, match="every movable"): + TreeKinematics(URDF, ["hinge"]) + kin = TreeKinematics(URDF, ["slide", "hinge"], dtype=torch.float64) + q = torch.tensor([[0.7, math.pi / 2]], dtype=torch.float64) + root = torch.zeros(1, 3, dtype=q.dtype) + linear = torch.tensor([[0.2, -0.1, 0.3]], dtype=q.dtype) + angular = torch.tensor([[0.0, 0.0, 2.0]], dtype=q.dtype) + state = kin.evaluate( + root, + torch.eye(3, dtype=q.dtype)[None], + q, + linear, + angular, + torch.tensor([[0.4, 3.0]], dtype=q.dtype), + ) + tip, slide = kin.link_indices(["tip", "slider"]) + torch.testing.assert_close( + state.position[:, tip], torch.tensor([[1.0, 1.0, 0.0]], dtype=q.dtype) + ) + torch.testing.assert_close( + state.linear_velocity[:, tip], torch.tensor([[-4.8, 1.9, 0.3]], dtype=q.dtype) + ) + torch.testing.assert_close( + state.angular_velocity[:, tip], torch.tensor([[0.0, 0.0, 5.0]], dtype=q.dtype) + ) + torch.testing.assert_close( + state.position[:, slide], torch.tensor([[0.7, 1.0, 0.0]], dtype=q.dtype) + ) + torch.testing.assert_close( + state.linear_velocity[:, slide], torch.tensor([[-1.4, 1.3, 0.3]], dtype=q.dtype) + ) + + +def test_tree_gradients_and_arbitrary_batch_axes(): + kin = TreeKinematics(URDF, ["slide", "hinge"], dtype=torch.float64) + q = torch.tensor( + [[[0.3, 0.4], [0.8, -0.2]]], dtype=torch.float64, requires_grad=True + ) + dq = torch.tensor( + [[[0.1, 0.2], [0.2, 0.1]]], dtype=torch.float64, requires_grad=True + ) + root = torch.zeros(1, 2, 3, dtype=q.dtype) + rot = torch.eye(3, dtype=q.dtype).expand(1, 2, 3, 3) + + def evaluate(x, dx): + k = kin.evaluate(root, rot, x, root, root, dx) + return k.position, k.rotation, k.linear_velocity, k.angular_velocity + + assert torch.autograd.gradcheck(evaluate, (q, dq)) + first = evaluate(q, dq)[0] + original = first.detach().clone() + evaluate(q + 1, dq) + torch.testing.assert_close(first, original) diff --git a/tests/data_pipeline/test_motion.py b/tests/data_pipeline/test_motion.py new file mode 100644 index 0000000000..c9a83b95c5 --- /dev/null +++ b/tests/data_pipeline/test_motion.py @@ -0,0 +1,185 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Reference motion boundaries, ownership and archive mapping.""" + +from __future__ import annotations + +import numpy as np +import pytest +import torch + +from embodichain.data_pipeline.datasets.motion import MotionLibrary + + +def data(): + qpos = np.zeros((5, 8), np.float32) + qpos[:, 6] = 1 + qpos[:, 0] = np.arange(5) + return qpos, np.zeros((5, 7), np.float32) + + +def test_future_hold_cannot_cross_either_clip_boundary(): + library = MotionLibrary(*data(), [3, 2], ["first", "second"], ["hinge"], 50) + future = library.future(torch.tensor([2, 3]), torch.tensor([-10, 0, 1, 20])) + torch.testing.assert_close( + future["pos"][..., 0], + torch.tensor([[0.0, 2.0, 2.0, 2.0], [3.0, 3.0, 4.0, 4.0]]), + ) + assert library.remaining(torch.tensor([0, 2, 3, 4])).tolist() == [2, 0, 1, 0] + + +def test_scalar_and_batch_queries_own_storage(): + qpos, qvel = data() + library = MotionLibrary(qpos, qvel, [3, 2], ["a", "b"], ["hinge"], 50) + qpos[:] = 99 + library.frame(torch.tensor(0))["pos"].fill_(33) + library.frame(torch.tensor([0, 1]))["q"].fill_(20) + assert library.frame(torch.tensor([0]))["pos"][0, 0] == 0 + assert library.frame(torch.tensor([0]))["q"][0, 0] == 0 + + +def test_archive_selection_and_explicit_quaternion_order(tmp_path): + qpos, qvel = data() + qpos[:, 3:7] = [1, 0, 0, 0] + path = tmp_path / "motions.npz" + np.savez( + path, + qpos=qpos, + qvel=qvel, + lengths=[3, 2], + starts=[0, 3], + names=["a", "b"], + fps=50, + joint_names=["hinge"], + ) + library = MotionLibrary.from_npz( + path, ["hinge"], quaternion_order="wxyz", clips=["b"] + ) + state = library.frame(torch.tensor([0])) + assert library.names == ("b",) and library.num_frames == 2 + torch.testing.assert_close(state["quat"], torch.tensor([[0.0, 0.0, 0.0, 1.0]])) + assert state["pos"][0, 0] == 3 + with pytest.raises(ValueError, match="joint order"): + MotionLibrary.from_npz(path, ["other"], quaternion_order="wxyz") + + +@pytest.mark.parametrize("source_frames, target_frames", [(70, 116), (71, 117)]) +def test_csv_resampling_constant_translation_and_rotation( + tmp_path, source_frames, target_frames +): + times = np.arange(source_frames, dtype=np.float32) / 30.0 + qpos = np.zeros((source_frames, 8), dtype=np.float32) + qpos[:, 0] = times + qpos[:, 5] = np.sin(0.3 * times / 2) + qpos[:, 6] = np.cos(0.3 * times / 2) + qpos[::2, 3:7] *= 2 + qpos[1::2, 3:7] *= -0.5 + qpos[:, 7] = 2 * times + source = tmp_path / "turn.csv" + np.savetxt(source, qpos, delimiter=",") + library = MotionLibrary.from_csv([source], ["joint"]) + assert library.num_frames == target_frames + state = library.frame(torch.arange(target_frames)) + assert state["pos"][-1, 0] == pytest.approx((target_frames - 1) / 50) + torch.testing.assert_close( + state["v"], + torch.tensor([[1.0, 0.0, 0.0]]).expand(target_frames, 3), + atol=1e-5, + rtol=1e-5, + ) + torch.testing.assert_close( + state["w"], + torch.tensor([[0.0, 0.0, 0.3]]).expand(target_frames, 3), + atol=1e-5, + rtol=1e-5, + ) + torch.testing.assert_close( + state["qd"], torch.full((target_frames, 1), 2.0), atol=2e-5, rtol=1e-5 + ) + archive = tmp_path / "portable.npz" + library.save_npz(archive) + loaded = MotionLibrary.from_npz(archive, ["joint"], quaternion_order="xyzw") + torch.testing.assert_close( + loaded.frame(torch.arange(target_frames))["rot"], state["rot"] + ) + with pytest.raises(ValueError, match="quaternion convention"): + MotionLibrary.from_npz(archive, ["joint"], quaternion_order="wxyz") + + +def test_motion_start_sampling_merges_duplicate_returns_and_restores_distribution(): + from embodichain.data_pipeline.datasets.motion_sampling import ( + MotionStartSampler, + MotionStartSamplerCfg, + ) + + library = MotionLibrary(*data(), [3, 2], ["a", "b"], ["hinge"], 50) + sampler = MotionStartSampler(library, MotionStartSamplerCfg(probability_cap=1.0)) + sampler.update(torch.tensor([0, 1, 3]), torch.tensor([0.2, 0.6, 0.9])) + torch.testing.assert_close(sampler.scores, torch.tensor([0.4, 0.9])) + torch.testing.assert_close(sampler.visits, torch.tensor([2.0, 1.0])) + torch.testing.assert_close(sampler.probabilities(), torch.tensor([0.5, 0.5])) + sampler.update(torch.tensor([0]), torch.tensor([1.0])) + assert sampler.scores[0] == pytest.approx(0.46) + restored = MotionStartSampler(library, sampler.cfg) + restored.load_state_dict(sampler.state_dict()) + first = sampler.sample(100, torch.Generator().manual_seed(7)) + second = restored.sample(100, torch.Generator().manual_seed(7)) + torch.testing.assert_close(first, second, rtol=0, atol=0) + assert set(first.tolist()) <= {0, 1, 3} + + +@pytest.mark.parametrize("change", ["qpos", "qvel", "missing_identity"]) +def test_sampler_rejects_changed_motion_before_mutating_state(tmp_path, change): + from copy import deepcopy + from embodichain.data_pipeline.datasets.motion_sampling import ( + MotionStartSampler, + MotionStartSamplerCfg, + ) + + path = tmp_path / "motions.npz" + qpos, qvel = data() + + def load(): + np.savez(path, qpos=qpos, qvel=qvel, lengths=[3, 2], names=["a", "b"], fps=50) + return MotionLibrary.from_npz( + path, ["hinge"], quaternion_order="xyzw", clips=["b"] + ) + + original = load() + source = MotionStartSampler(original, MotionStartSamplerCfg()) + source.update(torch.tensor([0]), torch.tensor([0.7])) + saved = source.state_dict() + compatible = MotionStartSampler(load(), source.cfg) + compatible.load_state_dict(saved) + torch.testing.assert_close(compatible.scores, source.scores, rtol=0, atol=0) + # Unselected content has no effect on the selected library identity. + qpos[0, 7] += 0.5 + assert load().content_digest == original.content_digest + if change == "qpos": + qpos[3, 7] += 0.5 + elif change == "qvel": + qvel[3, 6] += 0.5 + else: + saved.pop("content_digest") + restored = MotionStartSampler(load(), source.cfg) + before = deepcopy(restored.state_dict()) + with pytest.raises(ValueError, match="content differs"): + restored.load_state_dict(saved) + for name in ("scores", "visits", "past"): + torch.testing.assert_close( + getattr(restored, name), before[name], rtol=0, atol=0 + ) diff --git a/tests/examples/test_motion_tracking_configuration.py b/tests/examples/test_motion_tracking_configuration.py new file mode 100644 index 0000000000..581143d289 --- /dev/null +++ b/tests/examples/test_motion_tracking_configuration.py @@ -0,0 +1,110 @@ +# ---------------------------------------------------------------------------- +# 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 copy import deepcopy + +import pytest + +from examples.learning.motion_tracking._configuration import ( + normalize_recipe, + recipe_identity, + validate_recipe, +) + + +@pytest.mark.parametrize( + ("owner", "name"), + [ + ("algorithm", "APG_ACTOR_CRITIC"), + ("policy", "MOTION_TRACKING"), + ("learning_env", "motiontrackingmodelrl"), + ], +) +def test_case_insensitive_recipe_validation_preserves_checkpoint_identity(owner, name): + config = { + "trainer": {"learning_env": {"name": "MotionTrackingModelRL", "cfg": {}}}, + "policy": {"name": "motion_tracking", "cfg": {}}, + "algorithm": {"name": "apg_actor_critic", "cfg": {}}, + "workflow": {"iterations": 10}, + } + block = config["trainer"][owner] if owner == "learning_env" else config[owner] + block["name"] = name + original = deepcopy(config) + saved_identity = recipe_identity(config) + resumed = normalize_recipe(config) + resumed["workflow"]["iterations"] = 20 + validate_recipe(resumed) + assert config == original + original["workflow"]["iterations"] = 20 + assert resumed == original + assert recipe_identity(resumed) == saved_identity + assert saved_identity["algorithm"]["name"] == config["algorithm"]["name"] + + +@pytest.mark.parametrize("name", ["ppo", None, 1]) +def test_recipe_validation_rejects_unsupported_algorithm_names(name): + config = { + "trainer": {"learning_env": {"name": "MotionTrackingModelRL"}}, + "policy": {"name": "motion_tracking"}, + "algorithm": {"name": name}, + "workflow": {}, + } + with pytest.raises(ValueError, match="requires algorithm.name='apg_actor_critic'"): + validate_recipe(config) + + +@pytest.mark.parametrize("saved_digest", ["motion-v1:old", None]) +def test_motion_checkpoint_mismatch_precedes_learning_state_restore( + tmp_path, monkeypatch, saved_digest +): + from pathlib import Path + from types import SimpleNamespace + from unittest.mock import Mock + import torch + + example = Path(__file__).resolve().parents[2] / "examples/learning/motion_tracking" + monkeypatch.syspath_prepend(str(example)) + from examples.learning.motion_tracking.train import MotionTraining + + config = { + "trainer": {"learning_env": {"name": "MotionTrackingModelRL", "cfg": {}}}, + "policy": {"name": "motion_tracking", "cfg": {}}, + "algorithm": {"name": "apg_actor_critic", "cfg": {}}, + "workflow": {}, + } + workflow = object.__new__(MotionTraining) + workflow.env = SimpleNamespace(device=torch.device("cpu"), start_sampler=Mock()) + workflow.config = config + workflow.identity = {"motion": {"content_digest": "motion-v1:current"}} + workflow.schedule = Mock() + workflow.policy, workflow.learner, workflow.algorithm = Mock(), Mock(), Mock() + saved_motion = {} if saved_digest is None else {"content_digest": saved_digest} + path = tmp_path / "checkpoint.pt" + torch.save( + {"schema": 1, "config": config, "identity": {"motion": saved_motion}}, path + ) + with pytest.raises(ValueError, match="task identity differs"): + workflow.restore(path) + for owner in ( + workflow.policy, + workflow.learner, + workflow.algorithm, + workflow.schedule, + workflow.env.start_sampler, + ): + assert owner.mock_calls == [] diff --git a/tests/gym/envs/tasks/test_motion_model.py b/tests/gym/envs/tasks/test_motion_model.py new file mode 100644 index 0000000000..341fc39b35 --- /dev/null +++ b/tests/gym/envs/tasks/test_motion_model.py @@ -0,0 +1,421 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Replay-seeded imagination, gradient boundaries and clip validity.""" + +from __future__ import annotations + +import numpy as np +import torch +import pytest +from tensordict import TensorDict +from embodichain.compute.kinematics import TreeKinematics +from embodichain.data_pipeline.datasets.motion import MotionLibrary +from embodichain_models.dynamics.rwm import KinematicWorldModel, KinematicWorldModelCfg +from embodichain_models.dynamics.rwm.model import frozen_world_model +from embodichain_models.dynamics.rwm.robot_state import pack_robot_state +from embodichain_models.policies.motion_tracking import ( + MotionTrackingPolicy, + MotionTrackingPolicyCfg, +) +from embodichain_tasks.locomotion.tracking._task import ( + MotionTrackingTask, + MotionTrackingCfg, +) +from embodichain_tasks.locomotion.tracking.motion_model import ( + MotionModelEnv, + MotionModelCfg, + MotionModelStart, +) + +URDF = """ + +""" + + +def make_env(noise=False, compile_mode=None): + qpos = np.zeros((8, 8), dtype=np.float32) + qpos[:, 2] = 1 + qpos[:, 6] = 1 + motion = MotionLibrary( + qpos, np.zeros((8, 7), dtype=np.float32), [8], ["stand"], ["joint"], 50 + ) + kin = TreeKinematics(URDF, ["joint"]) + task = MotionTrackingTask( + kin, + torch.zeros(1), + torch.ones(1) * 0.5, + torch.tensor([[-1.0, 1.0]]), + torch.ones(1), + ["base", "tip"], + MotionTrackingCfg(history_steps=2, future_offsets=(1, 2)), + ) + wm = KinematicWorldModel( + URDF, + KinematicWorldModelCfg( + joint_names=("joint",), + resting_position=(0.0,), + action_dim=1, + context_dim=5, + hidden_dims=(16, 16), + ), + ) + env = MotionModelEnv( + 2, + "cpu", + model=wm, + motion=motion, + task=task, + cfg=MotionModelCfg( + horizon=3, + observation_noise=noise, + goal_position_noise=0.05 if noise else 0.0, + action_noise=0.0, + compile_mode=compile_mode, + ), + ) + frame = torch.tensor([0, 6]) + state = motion.frame(frame) + prev = torch.zeros(2, 1) + starts = MotionModelStart( + pack_robot_state(state), + torch.zeros(2, 5), + frame, + task.observation(state, prev, torch.zeros_like(prev))[:, None].expand( + -1, 2, -1 + ), + prev, + torch.ones(2, dtype=torch.bool), + ) + env.set_starts(starts) + return env, wm + + +def test_model_rollout_preserves_early_action_gradient_and_clip_end_validity(): + torch.manual_seed(14) + env, wm = make_env() + env.reset() + actions = torch.full((3, 2, 1), 0.02, requires_grad=True) + with frozen_world_model(wm): + rewards = [] + valid = [] + for action in actions: + _, reward, terminated, truncated, info = env.step(action) + rewards.append(reward) + valid.append(info["next_valid"]) + assert not terminated.any() and not truncated.any() + torch.testing.assert_close( + torch.stack(valid), + torch.tensor([[True, True], [True, False], [True, False]]), + ) + torch.stack(rewards)[-1].sum().backward() + assert torch.isfinite(actions.grad).all() and actions.grad[0].abs().sum() > 0 + assert all(p.grad is None for p in wm.parameters()) + with pytest.raises(RuntimeError, match="Reset"): + env.step(actions[0]) + + +def test_detach_preserves_noisy_observation_and_reference_progress(): + env, wm = make_env(noise=True) + first, _ = env.reset(seed=43) + before, _, _, _, _ = env.step(torch.ones(2, 1, requires_grad=True) * 0.01) + before = before.clone() + state = env.state.detach().clone() + after = env.detach_state() + for key in ("policy", "critic"): + torch.testing.assert_close(after[key], before[key], rtol=0, atol=0) + assert not after[key].requires_grad + torch.testing.assert_close(env.state, state, rtol=0, atol=0) + _, _, _, _, info = env.step(torch.zeros(2, 1)) + assert info["next_valid"].tolist() == [True, False] + + +def test_policy_actor_and_critic_have_independent_inputs_and_statistics(): + cfg = MotionTrackingPolicyCfg( + history_dim=18, + goal_dim=34, + critic_dim=57, + action_dim=1, + latent_dim=4, + encoder_hidden=(8,), + decoder_hidden=(8,), + critic_hidden=(8,), + ) + p = MotionTrackingPolicy(cfg) + x = torch.randn(3, 52, requires_grad=True) + p.update_actor_normalization(x.detach()) + count = p.critic_obs_normalizer.count.clone() + p.get_differentiable_action(TensorDict({"obs": x}, [3]))["action"].sum().backward() + assert x.grad[:, :18].abs().sum() > 0 and x.grad[:, 18:].abs().sum() > 0 + assert all(v.grad is None for v in p.critic.parameters()) + assert p.critic_obs_normalizer.count == count + p.update_critic_normalization(torch.randn(3, 57)) + assert p.critic_obs_normalizer.count == 3 + clone = MotionTrackingPolicy(cfg) + clone.load_state_dict(p.state_dict()) + torch.testing.assert_close(clone.actor(x), p.actor(x), rtol=0, atol=0) + + +def test_compiled_transition_matches_eager_values_noise_and_multistep_gradients( + monkeypatch, +): + compile_fn = torch.compile + monkeypatch.setattr( + torch, + "compile", + lambda fn, **kwargs: compile_fn(fn, backend="aot_eager", fullgraph=True), + ) + results = [] + for mode in (None, "default"): + torch.manual_seed(14) + env, wm = make_env(noise=True, compile_mode=mode) + env.cfg.action_noise = 0.1 + env.reset(seed=42) + actions = torch.full((3, 2, 1), 0.02, requires_grad=True) + with frozen_world_model(wm), torch.device(actions.device): + rewards = [] + for action in actions: + observation, reward, _, _, info = env.step(action) + rewards.append(reward) + objective = torch.stack(rewards).sum() + 0.01 * observation["policy"].sum() + gradient = torch.autograd.grad(objective, actions)[0] + results.append( + ( + env.state.detach(), + env.history.detach(), + observation["policy"].detach(), + info["continuation"].detach(), + objective.detach(), + gradient, + ) + ) + for eager, compiled in zip(*results): + torch.testing.assert_close(compiled, eager, rtol=1e-5, atol=1e-6) + + +@pytest.mark.parametrize("compiled", [False, True]) +def test_task_analytics_preserves_values_and_state_gradients(compiled): + """The physical task fusion retains the independently callable objectives.""" + env, _ = make_env() + task = env.task + frame = torch.tensor([1, 3]) + reference = env.motion.frame(frame) + future = env.motion.future(frame, task.future_offsets) + state = {k: v.clone() for k, v in reference.items()} + state["q"] = (state["q"] + 0.17).requires_grad_() + state["v"] = (state["v"] + 0.12).requires_grad_() + action = torch.full((2, 1), 0.08, requires_grad=True) + previous = torch.full_like(action, -0.03) + bias = torch.full_like(action, 0.01) + expected_reward, expected_terms = task.reward(state, reference, action, previous) + expected_failed, expected_continuation = task.termination(state, reference) + expected = { + "reward": expected_reward, + "reward_terms": expected_terms, + "failed": expected_failed, + "continuation": expected_continuation, + "observation": task.observation(state, action, bias), + "goal": task.goal(state, future), + "features": task.features(state), + } + evaluate = task.analytics + if compiled: + evaluate = torch.compile(evaluate, backend="aot_eager", fullgraph=True) + actual = evaluate(state, reference, future, action, previous, bias) + torch.testing.assert_close(actual, expected) + inputs = (state["q"], state["v"], action) + expected_gradient = torch.autograd.grad( + expected_reward.sum(), inputs, retain_graph=True + ) + actual_gradient = torch.autograd.grad(actual["reward"].sum(), inputs) + torch.testing.assert_close(actual_gradient, expected_gradient) + + +@pytest.mark.parametrize("compiled", [False, True]) +def test_physical_transition_excludes_nonfinite_rows(compiled): + """Divergent state rows terminate with finite features and zero return.""" + from types import SimpleNamespace + from embodichain_tasks.locomotion.tracking.g1_motion import UnitreeG1MotionEnv + + env, _ = make_env() + frame = torch.tensor([1, 3]) + reference = env.motion.frame(frame) + state = {key: value.clone() for key, value in reference.items()} + state["q"][0] += 0.17 + state["q"][1] = float("nan") + state["q"].requires_grad_() + future = env.motion.future(frame, env.task.future_offsets) + action = torch.full((2, 1), 0.08) + zero = torch.zeros_like(action) + owner = SimpleNamespace(task=env.task) + evaluate = lambda *args: UnitreeG1MotionEnv._compute_transition(owner, *args) + if compiled: + evaluate = torch.compile(evaluate, backend="aot_eager", fullgraph=True) + result = evaluate(state, reference, future, action, zero, zero) + assert result["valid"].tolist() == [True, False] + assert result["failed"][1] + assert result["reward"][1] == 0 + assert result["continuation"][1] == 0 + for key in ("observation", "goal", "features"): + assert torch.isfinite(result[key]).all() + for key, value in reference.items(): + torch.testing.assert_close(result["state"][key][1], value[1]) + expected, _ = env.task.reward(result["state"], reference, action, zero) + torch.testing.assert_close(result["reward"][0], expected[0]) + gradient = torch.autograd.grad(result["reward"].sum(), state["q"])[0] + assert torch.isfinite(gradient).all() + torch.testing.assert_close(gradient[1], torch.zeros_like(gradient[1])) + + +def test_physical_episode_metrics_own_storage_across_task_invocations(monkeypatch): + """Reset metrics remain writable when a compiled producer reuses its output.""" + from types import SimpleNamespace + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain_tasks.locomotion.tracking.g1_motion import UnitreeG1MotionEnv + + model_env, _ = make_env() + env = object.__new__(UnitreeG1MotionEnv) + env.motion, env.task = model_env.motion, model_env.task + env.motion_cfg = SimpleNamespace(gamma=0.99) + env.motion_frame = torch.tensor([1, 3]) + state = env.motion.frame(env.motion_frame) + env.read_state = lambda: state + zero = torch.zeros(2, 1) + env.position_action = SimpleNamespace( + raw_actions=zero, previous_raw_actions=zero, position_bias=zero + ) + env._episode_steps = torch.zeros(2, dtype=torch.long) + env._episode_discounted_return = torch.zeros(2) + env._reward = torch.zeros(2) + env._failed = torch.zeros(2, dtype=torch.bool) + env._continuation = torch.ones(2) + env._refresh = lambda *args, **kwargs: None + outputs = {} + + def evaluate(*args): + outputs.update(env._compute_transition(*args)) + return outputs + + env._task_analytics = evaluate + monkeypatch.setattr(EmbodiedEnv, "_update_sim_state", lambda *args, **kw: None) + env._update_sim_state() + expected_reward = env._reward.clone() + outputs["reward"].zero_() + outputs["failed"].fill_(True) + outputs["continuation"].zero_() + torch.testing.assert_close(env._reward, expected_reward) + assert not env._failed.any() + assert (env._continuation > 0).all() + env._reward.zero_() + env._continuation.fill_(1) + assert not outputs["continuation"].any() + + +def test_configured_model_policy_update_shares_workflow_resources(): + from embodichain.learning.rl.policies import build_policy + from embodichain.learning.rl.algo import build_algo + from embodichain.learning.rl.differentiable_trainer import ( + DifferentiableTrainer, + DifferentiableTrainerCfg, + ) + from embodichain.learning.rl.runtime import build_learning_policy_runtime + + source, model = make_env() + with pytest.raises(ValueError, match="dimensions differ"): + build_policy( + {"name": "motion_tracking", "cfg": {"action_dim": 29}}, + source.single_observation_space, + source.single_action_space, + torch.device("cpu"), + ) + task = source.task + config = { + "trainer": { + "learning_env": { + "name": "MotionTrackingModelRL", + "cfg": {"horizon": 3, "observation_noise": False}, + }, + }, + "policy": { + "name": "motion_tracking", + "cfg": { + "history_dim": task.observation_dim * task.cfg.history_steps, + "goal_dim": task.goal_dim, + "critic_dim": source.single_observation_space["critic"].shape[0], + "action_dim": 1, + "latent_dim": 4, + "encoder_hidden": [8], + "decoder_hidden": [8], + "critic_hidden": [8], + }, + }, + "algorithm": { + "name": "apg_actor_critic", + "cfg": { + "optimizer": {"learning_rate": 0.002}, + "critic_optimizer": {"learning_rate": 0.003}, + "critic_epochs": 1, + "critic_minibatches": 1, + }, + }, + } + starts = source._start + provider = lambda: starts + resources = { + "model": model, + "motion": source.motion, + "task": task, + "start_provider": provider, + } + + runtime = build_learning_policy_runtime( + config, device=torch.device("cpu"), num_envs=2, resources=resources + ) + algorithm = build_algo( + config["algorithm"]["name"], + config["algorithm"]["cfg"], + runtime.policy, + runtime.device, + ) + trainer = DifferentiableTrainer( + DifferentiableTrainerCfg(rollout_mode="complete", deterministic_actions=True), + runtime.env, + runtime.policy, + algorithm, + ) + assert runtime.env is not source + assert runtime.env.model is model + assert runtime.env.motion is source.motion + assert runtime.env.task is task + assert runtime.env.start_provider is provider + assert runtime.env.cfg.horizon == 3 + assert trainer.algorithm.critic_optimizer.param_groups[0]["lr"] == 0.003 + original_actor = [ + parameter.detach().clone() for parameter in runtime.policy.actor.parameters() + ] + original_model = [parameter.detach().clone() for parameter in model.parameters()] + with frozen_world_model(model): + trainer.update_once() + assert trainer.num_updates == 1 + assert trainer.global_step == 6 + assert any( + not torch.equal(old, new) + for old, new in zip(original_actor, runtime.policy.actor.parameters()) + ) + for old, new in zip(original_model, model.parameters()): + torch.testing.assert_close(old, new, rtol=0, atol=0) + assert new.grad is None + runtime.close() diff --git a/tests/gym/envs/tasks/test_motion_tracking.py b/tests/gym/envs/tasks/test_motion_tracking.py new file mode 100644 index 0000000000..98d266c866 --- /dev/null +++ b/tests/gym/envs/tasks/test_motion_tracking.py @@ -0,0 +1,355 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Tracking objectives, action mapping and native G1 reset behavior.""" + +from __future__ import annotations +import math +import os +from pathlib import Path +import subprocess +import sys +from types import SimpleNamespace +from unittest.mock import Mock +import pytest +import torch +from embodichain.compute.kinematics import TreeKinematics +from embodichain.data_pipeline.datasets.motion import MotionLibrary +from embodichain.lab.gym.envs.managers.cfg import ActionTermCfg +from embodichain_tasks.locomotion.managers.actions import ReferenceJointPositionAction +from embodichain_tasks.locomotion.tracking._task import MotionTrackingTask + + +@pytest.mark.subprocess_sim +@pytest.mark.gpu +@pytest.mark.requires_sim +@pytest.mark.requires_tasks +@pytest.mark.parametrize("update_data_interval", [0, 1]) +def test_g1_state_injection_and_selected_reset(update_data_interval: int) -> None: + """Written starts survive native stepping and preserve other arenas/materials.""" + data_root = os.environ.get("EMBODICHAIN_DATA_ROOT") + if not data_root or not (Path(data_root) / "LAFAN1G1/motion.npz").is_file(): + pytest.skip("Prepare LAFAN1G1 under EMBODICHAIN_DATA_ROOT to run G1 physics") + result = subprocess.run( + [sys.executable, str(Path(__file__).resolve()), str(update_data_interval)], + capture_output=True, + text=True, + timeout=120, + cwd=Path(__file__).resolve().parents[4], + env=os.environ.copy(), + ) + assert result.returncode == 0, result.stdout + result.stderr + + +def _run_g1_state_injection_and_selected_reset(update_data_interval: int) -> None: + """Create the NoRender world in a process with no preinitialized renderer.""" + import numpy as np + import yaml + + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.sim import SimulationManager + from embodichain_tasks.locomotion.tracking.g1_motion import UnitreeG1MotionEnv + + data_root = os.environ.get("EMBODICHAIN_DATA_ROOT") + archive = Path(data_root or "") / "LAFAN1G1/motion.npz" + if not data_root or not archive.is_file(): + pytest.skip("Prepare LAFAN1G1 under EMBODICHAIN_DATA_ROOT to run G1 physics") + root = Path(__file__).resolve().parents[4] + config_path = ( + root + / "extensions/embodichain_tasks/configs/tasks/locomotion/tracking/g1_motion/env.newton.yaml" + ) + gym = yaml.safe_load(config_path.read_text()) + gym["num_envs"] = 2 + gym["max_episode_steps"] = 2 + gym["physics_config"]["solver_cfg"]["update_data_interval"] = update_data_interval + options = gym["env"]["extensions"]["motion_tracking"] + with np.load(archive) as data: + options["clips"] = data["names"].tolist()[:2] + options.update( + observation_noise=False, random_start=False, fixed_clip_assignment=True + ) + cfg = config_to_cfg(gym) + cfg.init_rollout_buffer = False + env = UnitreeG1MotionEnv(cfg) + try: + env.reset(seed=10000) + env.sim.update(step=1, render_final_step=False) + assert env.sim.physics.cuda_graph_status == "captured" + ids = torch.tensor([1], device=env.device) + floor = env.sim.get_rigid_object("floor") + floor.set_friction(torch.tensor([0.73], device=env.device), env_ids=ids) + com = env.robot.get_com_pose(["torso_link"])[ids].clone() + com[:, :, 0] += 0.005 + env.robot.set_com_pose(com, link_names=["torso_link"], env_ids=ids) + env.sim.update(step=1, render_final_step=False) + friction_before = floor.get_friction().clone() + com_before = env.robot.get_com_pose(["torso_link"]).clone() + before = env.read_state() + injected = {key: value[ids].clone() for key, value in before.items()} + injected["pos"][:, 0] += 1.5 + injected["pos"][:, 2] += 0.4 + injected["v"].zero_() + injected["w"].zero_() + injected["qd"].zero_() + env.write_state(ids, injected) + after = env.read_state() + for name in before: + torch.testing.assert_close( + after[name][[0]], before[name][[0]], rtol=0, atol=0 + ) + torch.testing.assert_close( + floor.get_friction(), friction_before, rtol=0, atol=0 + ) + torch.testing.assert_close( + env.robot.get_com_pose(["torso_link"]), com_before, rtol=0, atol=0 + ) + assert env.sim.physics.cuda_graph_status == "captured" + env.sim.update(step=1, render_final_step=False) + # A stale native start returns to the old root location, 1.5 m away. + torch.testing.assert_close( + env.read_state()["pos"][ids], injected["pos"], rtol=0, atol=0.05 + ) + assert env.sim.physics.cuda_graph_status == "captured" + before_reset = env.read_state() + env.reset(options={"reset_ids": ids}) + after_reset = env.read_state() + torch.testing.assert_close( + after_reset["pos"][ids], + env.motion.frame(env.motion.starts[ids])["pos"], + rtol=0, + atol=2e-6, + ) + for name in before_reset: + torch.testing.assert_close( + after_reset[name][[0]], before_reset[name][[0]], rtol=0, atol=0 + ) + for _ in range(2): + _, _, terminated, truncated, _ = env.step( + torch.zeros(2, env.task.dof, device=env.device) + ) + assert bool((terminated | truncated).all()) + torch.testing.assert_close(env.motion_frame, env.motion.starts, rtol=0, atol=0) + torch.testing.assert_close( + env.read_state()["pos"], + env.motion.frame(env.motion.starts)["pos"], + rtol=0, + atol=2e-6, + ) + torch.testing.assert_close( + floor.get_friction(), friction_before, rtol=0, atol=0 + ) + torch.testing.assert_close( + env.robot.get_com_pose(["torso_link"]), com_before, rtol=0, atol=0 + ) + finally: + floor = None + env.close(exit_process=False) + del env + SimulationManager.flush_cleanup_queue() + + +def task_and_state(): + urdf = """ + + """ + kin = TreeKinematics(urdf, ["joint"], dtype=torch.float64) + scalar = torch.zeros(1, dtype=torch.float64) + task = MotionTrackingTask( + kin, + scalar, + torch.ones_like(scalar) * 0.5, + torch.tensor([[-1.0, 1.0]], dtype=scalar.dtype), + torch.ones_like(scalar), + ["root", "tip"], + ) + state = { + "pos": torch.tensor([[0.0, 0.0, 1.0]], dtype=scalar.dtype), + "rot": torch.eye(3, dtype=scalar.dtype)[None], + "q": scalar.reshape(1, 1), + "qd": scalar.reshape(1, 1), + "v": torch.zeros(1, 3, dtype=scalar.dtype), + "w": torch.zeros(1, 3, dtype=scalar.dtype), + } + return task, state + + +def test_reference_reward_and_gradient_point_toward_target(): + task, ref = task_and_state() + action = torch.zeros(1, 1, dtype=torch.float64) + reward, _ = task.reward(ref, ref, action, action) + torch.testing.assert_close(reward, torch.tensor([0.1], dtype=reward.dtype)) + state = {k: v.clone() for k, v in ref.items()} + state["pos"][:, 2] += 0.1 + state["pos"].requires_grad_() + state["q"] += 0.2 + state["q"].requires_grad_() + reward, _ = task.reward(state, ref, action, action) + gradient = torch.autograd.grad(reward.sum(), (state["pos"], state["q"])) + assert gradient[0][0, 2] < 0 and gradient[1][0, 0] < 0 + assert all(torch.isfinite(g).all() for g in gradient) + state["pos"] = state["pos"].detach() + state["pos"][:, 2] = 1.4 + hard, continuation = task.termination(state, ref) + assert hard.item() and continuation.item() < 0.001 + + +def test_reference_residual_bias_and_robot_relative_goal(): + task, ref = task_and_state() + action = torch.tensor([[0.4]], dtype=torch.float64) + normalized, target = task.control( + action, + torch.tensor([[0.3]], dtype=torch.float64), + torch.tensor([[0.05]], dtype=torch.float64), + ) + torch.testing.assert_close(normalized, torch.ones_like(action)) + torch.testing.assert_close(target, torch.full_like(action, 0.45)) + future = { + k: v.unsqueeze(1).expand(-1, 10, *v.shape[1:]).clone() for k, v in ref.items() + } + future["pos"][..., 0] += 2.0 + goal = task.goal(ref, future).unflatten(-1, (10, -1)) + torch.testing.assert_close( + goal[..., :3], + torch.tensor([[[2.0, 0.0, 0.0]]], dtype=action.dtype).expand(1, 10, 3), + ) + + +def test_tracking_diagnostics_distinguish_alignment_and_failure_conditions(): + task, ref = task_and_state() + state = {k: v.clone() for k, v in ref.items()} + state["pos"] += torch.tensor([[2.0, 0.0, 0.3]], dtype=state["pos"].dtype) + state["q"] += math.pi / 2 + errors = task.tracking_errors(state, ref) + # Horizontal translation is removed only from the body-relative pose error. + torch.testing.assert_close( + errors["body_pos"], torch.tensor([0.09], dtype=torch.float64) + ) + torch.testing.assert_close( + errors["root_pos"], torch.tensor([4.09], dtype=torch.float64) + ) + torch.testing.assert_close( + errors["body_ori"], torch.tensor([math.pi**2 / 8], dtype=torch.float64) + ) + reasons = task.failure_reasons(state, ref) + assert reasons["height"].item() and not reasons["tilt"].item() + state["pos"] = ref["pos"].clone() + state["rot"] = torch.tensor( + [[[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]], dtype=torch.float64 + ) + reasons = task.failure_reasons(state, ref) + assert not reasons["height"].item() and reasons["tilt"].item() + failed, _ = task.termination(state, ref) + assert torch.equal(failed, reasons["height"] | reasons["tilt"]) + + # Equal headings with different root tilts must not introduce a yaw offset. + from embodichain_models.dynamics.rwm.robot_state import rotation_exp + + task, ref = task_and_state() + state = {k: v.clone() for k, v in ref.items()} + state["rot"] = rotation_exp( + torch.tensor([[math.pi / 4, 0.0, 0.0]], dtype=torch.float64) + ) + ref["rot"] = rotation_exp( + torch.tensor([[0.0, math.pi / 4, 0.0]], dtype=torch.float64) + ) + state["pos"].requires_grad_() + error = task.tracking_errors(state, ref)["body_pos"] + torch.testing.assert_close( + error, torch.tensor([1 - math.cos(math.pi / 4)], dtype=torch.float64) + ) + gradient = torch.autograd.grad(error.sum(), state["pos"])[0] + torch.testing.assert_close( + gradient, torch.tensor([[0.0, 0.0, math.sin(math.pi / 4)]], dtype=torch.float64) + ) + + +@pytest.mark.parametrize("compiled", [False, True]) +def test_rotation_error_has_correct_values_and_gradients_near_zero_and_pi( + compiled: bool, +) -> None: + """Check rotation values and gradients with a default device active.""" + from embodichain_models.dynamics.rwm.robot_state import rotation_exp + + task, original = task_and_state() + angles = torch.tensor( + [0.0, 1e-9, -1e-6, 0.3, math.pi - 1e-4, -math.pi + 1e-4], + dtype=torch.float64, + requires_grad=True, + ) + reference = { + name: value.expand(len(angles), *value.shape[1:]) + for name, value in original.items() + } + axis = torch.tensor([1.0, -2.0, 3.0], dtype=torch.float64) + axis = axis / axis.norm() + rotation = rotation_exp(angles[:, None] * axis) + + def error(rotation): + state = {**reference, "rot": rotation} + return task.tracking_errors(state, reference)["root_ori"] + + fn = ( + torch.compile(error, backend="aot_eager", fullgraph=True) if compiled else error + ) + with torch.device(rotation.device): + values = fn(rotation) + torch.testing.assert_close(values, angles.square(), atol=1e-12, rtol=1e-10) + (gradient,) = torch.autograd.grad(values.sum(), (angles,)) + torch.testing.assert_close(gradient, 2 * angles.detach(), atol=1e-10, rtol=1e-10) + half_turn = ( + 2 * axis[:, None] * axis[None, :] - torch.eye(3, dtype=axis.dtype) + ).expand(len(angles), -1, -1) + with torch.device(rotation.device): + half_turn_values = fn(half_turn) + torch.testing.assert_close( + half_turn_values, torch.full_like(angles, math.pi**2), atol=1e-12, rtol=1e-12 + ) + + +@pytest.mark.parametrize("future_offsets", [(1, 2), (5, 10)]) +def test_control_uses_next_frame_independent_of_goal_offsets(future_offsets): + task, _ = task_and_state() + task.future_offsets = torch.tensor(future_offsets) + qpos = torch.zeros(8, 8) + qpos[:, 6] = 1 + qpos[:, 7] = torch.arange(8) + motion = MotionLibrary(qpos, torch.zeros(8, 7), [8], ["move"], ["joint"], 50) + env = SimpleNamespace( + task=task, + motion=motion, + motion_frame=torch.tensor([0, 7]), + num_envs=2, + device=torch.device("cpu"), + robot=SimpleNamespace( + joint_names=["joint"], cfg=SimpleNamespace(init_qpos=[0.0]), set_qpos=Mock() + ), + ) + action = ReferenceJointPositionAction( + ActionTermCfg(params={"joint_names": ["joint"]}), env + ) + action.process_actions(torch.zeros(2, 1)) + torch.testing.assert_close(action.processed_actions, torch.tensor([[1.0], [7.0]])) + torch.testing.assert_close(action.setpoint_actions, torch.tensor([[2.0], [14.0]])) + action.apply_actions() + call = env.robot.set_qpos.call_args + torch.testing.assert_close(call.args[0], torch.tensor([[1.0], [7.0]])) + assert call.kwargs["joint_ids"] == [0] + assert call.kwargs["clamp_target_qpos"] is False + + +if __name__ == "__main__": + _run_g1_state_injection_and_selected_reset(int(sys.argv[1])) diff --git a/tests/learning/test_apg_actor_critic.py b/tests/learning/test_apg_actor_critic.py new file mode 100644 index 0000000000..0d352467ea --- /dev/null +++ b/tests/learning/test_apg_actor_critic.py @@ -0,0 +1,473 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Pathwise derivatives, bootstrap boundaries and actor/critic isolation.""" + +from __future__ import annotations + +from dataclasses import replace + +import pytest +import torch +from torch import nn +from tensordict import TensorDict +from gymnasium.spaces import Box + +from embodichain.learning.rl.algo import ( + APGActorCritic, + APGActorCriticCfg, + build_algo, + differentiable_lambda_returns, +) +from embodichain.learning.rl.collector import DifferentiableCollector +from embodichain.learning.rl.differentiable_trainer import ( + DifferentiableTrainer, + DifferentiableTrainerCfg, +) +from embodichain.learning.rl.policies import ActorCritic +from embodichain.learning.rl.utils import OptimizerCfg + + +class LinearEnv: + num_envs = 1 + device = torch.device("cpu") + single_observation_space = Box(-1000, 1000, (1,)) + single_action_space = Box(-1000, 1000, (1,)) + + def __init__( + self, + boundary: str = "none", + grouped: bool = False, + soft: bool = True, + terminal_snapshot: bool = True, + ): + self.boundary, self.grouped, self.soft = boundary, grouped, soft + self.terminal_snapshot = terminal_snapshot + self.reset() + + def obs(self): + return ( + TensorDict({"actor": self.state, "critic": self.state * 2}, [1]) + if self.grouped + else self.state + ) + + def reset(self, *, seed=None): + self.state = torch.ones(1, 1, dtype=torch.float64) + self.steps = 0 + return self.obs(), {} + + def step(self, action): + self.state = self.state + action + self.steps += 1 + reward = -self.state[:, 0].square() + terminated = torch.tensor([self.boundary == "terminated" and self.steps == 1]) + truncated = torch.tensor([self.boundary == "truncated" and self.steps == 1]) + info = { + "terminal_observation": self.obs(), + "continuation": ( + self.state[:, 0].sigmoid() + if self.soft + else torch.ones(1, dtype=torch.float64) + ), + } + if not self.terminal_snapshot: + info.pop("terminal_observation") + if bool((terminated | truncated).any()): + self.state = torch.full_like(self.state, 100) + return self.obs(), reward, terminated, truncated, info + + def detach_state(self): + self.state = self.state.detach() + return self.obs() + + +def policy(grouped=False): + actor, critic = nn.Linear(1, 1, bias=False), nn.Linear(1, 1, bias=False) + nn.init.constant_(actor.weight, 0.2) + nn.init.constant_(critic.weight, 2) + return ActorCritic( + 1, + 1, + torch.device("cpu"), + actor, + critic, + critic_obs_dim=1 if grouped else None, + actor_obs_groups=("actor",) if grouped else None, + critic_obs_groups=("critic",) if grouped else None, + ).double() + + +def algorithm(p): + return APGActorCritic( + APGActorCriticCfg( + gamma=0.9, + gae_lambda=0.7, + normalize_return=False, + optimizer=OptimizerCfg(name="sgd", learning_rate=0.001), + critic_optimizer=OptimizerCfg(name="adam", learning_rate=0.001), + critic_epochs=1, + critic_minibatches=1, + max_grad_norm=1e6, + critic_max_grad_norm=1e6, + ), + p, + ) + + +def objective(weight): + # Independent three-step scalar dynamics, reward and terminal-value formula. + s1, s2, s3 = (1 + weight), (1 + weight) ** 2, (1 + weight) ** 3 + c1, c2, c3 = ( + torch.tensor(s, dtype=torch.float64).sigmoid().item() for s in (s1, s2, s3) + ) + return ( + -(s1**2) + - 0.9 * c1 * s2**2 + - 0.9**2 * c1 * c2 * s3**2 + + 0.9**3 * c1 * c2 * c3 * 2 * s3 + ) + + +@pytest.mark.parametrize("terminal_snapshot", [False, True]) +def test_pathwise_gradient_includes_soft_continuation_and_terminal_value( + terminal_snapshot, +): + p = policy() + algo = algorithm(p) + rollout = DifferentiableCollector( + LinearEnv(terminal_snapshot=terminal_snapshot), p, torch.device("cpu") + ).collect(3, deterministic=True) + algo.begin_update() + algo.accumulate_complete_rollout(rollout) + eps = 1e-5 + expected = -(objective(0.2 + eps) - objective(0.2 - eps)) / (2 * eps) + assert p.actor.weight.grad.item() == pytest.approx(expected, rel=1e-7) + assert p.critic.weight.grad is None + before = p.actor.weight.detach().clone() + critic_before = p.critic.weight.detach().clone() + metrics = algo.finish_update() + assert metrics["objective"] == pytest.approx(objective(0.2), rel=1e-7) + torch.testing.assert_close(p.actor.weight, before - 0.001 * expected) + assert not torch.equal(p.critic.weight, critic_before) + assert p.actor.weight.grad.item() == pytest.approx(expected, rel=1e-7) + + +@pytest.mark.parametrize( + "boundary,expected", [("terminated", -1.44), ("truncated", 0.72)] +) +def test_complete_rollout_stops_at_done_and_truncation_uses_pre_reset_value( + boundary, expected +): + p = policy() + algo = algorithm(p) + env = LinearEnv(boundary=boundary, soft=False) + rollout = DifferentiableCollector(env, p, env.device).collect(3, deterministic=True) + assert rollout.transitions[0].terminal_critic_observation.item() == pytest.approx( + 1.2 + ) + assert rollout.transitions[0].next_observation.item() == 100 + algo.begin_update() + algo.accumulate_complete_rollout(rollout) + metrics = algo.finish_update() + assert metrics["objective"] == pytest.approx(expected, abs=1e-7) + assert metrics["critic_weight_mean"] == pytest.approx(1 / 3) + + +def test_lambda_targets_distinguish_termination_truncation_and_rollout_end(): + rewards = torch.tensor([[1.0], [2.0], [3.0]]) + values = torch.tensor([[10.0], [20.0], [30.0]]) + done = torch.tensor([[False], [True], [False]]) + continuation = torch.ones_like(values) + result = differentiable_lambda_returns( + rewards, values, continuation, done, gamma=0.5, gae_lambda=0.5 + ) + torch.testing.assert_close(result, torch.tensor([[6.5], [12.0], [18.0]])) + continuation[1] = 0 + result = differentiable_lambda_returns( + rewards, values, continuation, done, gamma=0.5, gae_lambda=0.5 + ) + torch.testing.assert_close(result, torch.tensor([[4.0], [2.0], [18.0]])) + + +def test_grouped_observations_and_detach_preserve_state_values(): + env, p = LinearEnv(grouped=True), policy(grouped=True) + collector = DifferentiableCollector(env, p, env.device) + rollout = collector.collect(2, deterministic=True) + assert rollout.transitions[0].observation.item() == 1 + assert rollout.transitions[0].critic_observation.item() == 2 + assert rollout.transitions[-1].next_critic_observation.item() == pytest.approx(2.88) + before = env.state.clone() + collector.detach_state() + torch.testing.assert_close(env.state, before) + assert env.state.grad_fn is None + assert env.steps == 2 + algorithm(p).update(rollout) + + +def test_validity_masks_stop_reward_bootstrap_and_critic_weights(): + p = policy() + algo = algorithm(p) + rollout = DifferentiableCollector( + LinearEnv(soft=False), p, torch.device("cpu") + ).collect(3, deterministic=True) + steps = list(rollout.transitions) + steps[0] = replace(steps[0], next_valid=torch.zeros(1)) + steps[1] = replace(steps[1], valid=torch.zeros(1), next_valid=torch.zeros(1)) + steps[2] = replace(steps[2], valid=torch.zeros(1), next_valid=torch.zeros(1)) + algo.begin_update() + algo.accumulate_complete_rollout(replace(rollout, transitions=tuple(steps))) + metrics = algo.finish_update() + assert metrics["objective"] == 0 + assert metrics["critic_target_mean"] == 0 + assert metrics["critic_weight_mean"] == pytest.approx(1 / 3) + assert p.actor.weight.grad.item() == 0 + + +def test_truncation_without_terminal_observation_is_rejected(): + p = policy() + algo = algorithm(p) + rollout = DifferentiableCollector( + LinearEnv(boundary="truncated"), p, torch.device("cpu") + ).collect(1, deterministic=True) + rollout = replace( + rollout, + transitions=( + replace(rollout.transitions[0], terminal_critic_observation=None), + ), + ) + with pytest.raises(ValueError, match="pre-reset"): + algo.update(rollout) + + +def test_registered_trainer_checkpoint_restores_both_adam_updates(tmp_path): + def build(): + p = policy() + algo = build_algo( + "apg_actor_critic", + { + "optimizer": {"name": "adam", "learning_rate": 0.001}, + "critic_optimizer": {"name": "adam", "learning_rate": 0.001}, + "critic_epochs": 1, + "critic_minibatches": 1, + }, + p, + torch.device("cpu"), + ) + return DifferentiableTrainer( + DifferentiableTrainerCfg(segment_length=3, deterministic_actions=True), + LinearEnv(), + p, + algo, + ) + + trainer = build() + assert trainer.update_once()["critic_updates"] == 1 + assert trainer.num_updates == len(trainer.train_history) == 1 + assert trainer.global_step == 3 + checkpoint = trainer.save_checkpoint(tmp_path / "actor_critic.pt") + restored = build() + restored.load_checkpoint(checkpoint) + assert restored.num_updates == 1 and restored.global_step == 3 + for name in ("optimizer", "critic_optimizer"): + expected = getattr(trainer.algorithm, name).state_dict() + actual = getattr(restored.algorithm, name).state_dict() + assert actual["state"] + torch.testing.assert_close(actual, expected, rtol=0, atol=0) + for current in (trainer, restored): + # Checkpoints resume learning from a fresh environment rollout. + current.collector.reset() + torch.manual_seed(4) + metrics = current.update_once() + assert current.num_updates == 2 and current.global_step == 6 + assert metrics["critic_updates"] == 1 and metrics["skipped_update"] == 0 + for x, y in zip(trainer.policy.parameters(), restored.policy.parameters()): + torch.testing.assert_close(x, y, rtol=0, atol=0) + + +def test_rollout_context_freezes_critic_statistics_and_restores_training_mode(): + class CountingCritic(nn.Linear): + def __init__(self): + super().__init__(1, 1, bias=False) + self.register_buffer("calls_in_training", torch.zeros(())) + + def forward(self, value): + if self.training: + self.calls_in_training.add_(1) + return super().forward(value) + + p = policy() + p.critic = CountingCritic().double() + algo = algorithm(p) + collector = DifferentiableCollector(LinearEnv(), p, torch.device("cpu")) + with algo.rollout_context(): + rollout = collector.collect(3, deterministic=True) + assert p.critic.training + assert p.critic.calls_in_training == 0 + algo.begin_update() + algo.accumulate_complete_rollout(rollout) + assert p.critic.calls_in_training == 0 + assert p.critic.weight.grad is None + algo.finish_update() + assert p.critic.calls_in_training == 1 + + +def test_critic_normalization_refreshes_targets_after_actor_backward(): + p = policy() + # A known value transform makes old/new-stat targets distinguishable. + p.register_buffer("value_scale", torch.ones((), dtype=torch.float64)) + original_value = p.get_value + observed = [] + + def get_value(fields): + output = original_value(fields) + output["value"] = output["value"] * p.value_scale + return output + + def update_normalization(inputs): + assert not inputs.requires_grad + assert p.actor.weight.grad is not None + assert p.critic.weight.grad is None + observed.append(inputs.clone()) + p.value_scale.fill_(2) + + p.get_value = get_value + p.update_critic_normalization = update_normalization + algo = algorithm(p) + algo.cfg.update_critic_normalization = True + rollout = DifferentiableCollector(LinearEnv(soft=False), p, p.device).collect( + 3, deterministic=True + ) + algo.begin_update() + algo.accumulate_complete_rollout(rollout) + actor_grad = p.actor.weight.grad.clone() + states = torch.tensor([1.2, 1.44, 1.728], dtype=torch.float64) + target2 = -states[2] ** 2 + 0.9 * 4 * states[2] + target1 = -states[1] ** 2 + 0.9 * (0.3 * 4 * states[1] + 0.7 * target2) + target0 = -states[0] ** 2 + 0.9 * (0.3 * 4 * states[0] + 0.7 * target1) + metrics = algo.finish_update() + assert metrics["critic_target_mean"] == pytest.approx( + float((target0 + target1 + target2) / 3) + ) + torch.testing.assert_close( + observed[0].flatten(), torch.cat((torch.ones(1), states)) + ) + torch.testing.assert_close(p.actor.weight.grad, actor_grad) + + +@pytest.mark.parametrize( + "boundary,complete,expected", + [ + ("terminated", True, [1.0]), + ("truncated", True, [1.0, 1.2]), + ("reference_end", True, [1.0]), + ("truncated", False, [1.0, 100.0, 120.0, 1.2, 144.0]), + ], +) +def test_critic_statistics_exclude_invalid_states_and_include_bootstrap( + boundary, complete, expected +): + p = policy() + samples = [] + p.update_critic_normalization = lambda inputs: samples.append(inputs.clone()) + algo = algorithm(p) + algo.cfg.update_critic_normalization = True + rollout = DifferentiableCollector( + LinearEnv(boundary=boundary, soft=False), p, p.device + ).collect(3, deterministic=True) + if boundary == "reference_end": + steps = tuple( + replace( + step, valid=torch.tensor([i == 0]), next_valid=torch.tensor([False]) + ) + for i, step in enumerate(rollout.transitions) + ) + rollout = replace(rollout, transitions=steps) + algo.begin_update() + if complete: + algo.accumulate_complete_rollout(rollout) + else: + algo.accumulate_segment(rollout) + algo.finish_update() + torch.testing.assert_close( + samples[0].flatten(), torch.tensor(expected, dtype=torch.float64) + ) + + +@pytest.mark.parametrize("boundary", ["terminated", "invalid"]) +def test_critic_minibatches_exclude_zero_weight_rows(boundary): + # The same single valid transition must produce the same Adam state even + # when a complete rollout includes two more unusable states. + results = [] + for horizon in (1, 3): + p = policy() + algo = algorithm(p) + algo.cfg.critic_minibatches = 3 + rollout = DifferentiableCollector( + LinearEnv(boundary="terminated", soft=False), p, p.device + ).collect(horizon, deterministic=True) + if boundary == "invalid": + rollout = replace( + rollout, + transitions=tuple( + replace( + step, + valid=torch.tensor([i == 0]), + next_valid=torch.tensor([False]), + ) + for i, step in enumerate(rollout.transitions) + ), + ) + algo.begin_update() + algo.accumulate_complete_rollout(rollout) + metrics = algo.finish_update() + assert metrics["critic_updates"] == 1 + results.append((p.critic.state_dict(), algo.critic_optimizer.state_dict())) + torch.testing.assert_close(results[0], results[1], rtol=0, atol=0) + + +def test_all_zero_critic_weights_preserve_existing_adam_momentum(): + from copy import deepcopy + + p = policy() + algo = algorithm(p) + algo.cfg.critic_minibatches = 3 + for invalid in (False, True): + rollout = DifferentiableCollector(LinearEnv(), p, p.device).collect( + 3, deterministic=True + ) + if invalid: + rollout = replace( + rollout, + transitions=tuple( + replace( + step, + valid=torch.tensor([False]), + next_valid=torch.tensor([False]), + ) + for step in rollout.transitions + ), + ) + algo.begin_update() + algo.accumulate_complete_rollout(rollout) + metrics = algo.finish_update() + current = (p.critic.state_dict(), algo.critic_optimizer.state_dict()) + if invalid: + assert metrics["critic_updates"] == 0 + torch.testing.assert_close(current, previous, rtol=0, atol=0) + else: + assert metrics["critic_updates"] == 3 + previous = deepcopy(current) diff --git a/tests/learning/test_differentiable_env.py b/tests/learning/test_differentiable_env.py index 51628725cb..0465377d2d 100644 --- a/tests/learning/test_differentiable_env.py +++ b/tests/learning/test_differentiable_env.py @@ -21,10 +21,20 @@ from differentiable_env_support import LinearDifferentiableEnv -from embodichain.learning.rl import DifferentiableVecEnv +from embodichain.learning import rl +from embodichain.learning.rl import env as env_module def test_structural_protocol_accepts_differentiable_environment() -> None: env = LinearDifferentiableEnv() - assert isinstance(env, DifferentiableVecEnv) + assert isinstance(env, rl.LearningEnvProtocol) + assert isinstance(env, rl.DifferentiableEnvProtocol) + for old, new in ( + ("LearningVecEnv", "LearningEnvProtocol"), + ("DifferentiableVecEnv", "DifferentiableEnvProtocol"), + ("ScheduledDifferentiableVecEnv", "ScheduledDifferentiableEnvProtocol"), + ): + assert getattr(rl, old) is getattr(rl, new) + assert getattr(env_module, old) is getattr(rl, new) + assert getattr(env_module, new) is getattr(rl, new) diff --git a/tests/learning/test_differentiable_trainer.py b/tests/learning/test_differentiable_trainer.py index 7e769f72ca..3e2f1e52b4 100644 --- a/tests/learning/test_differentiable_trainer.py +++ b/tests/learning/test_differentiable_trainer.py @@ -702,3 +702,44 @@ def test_variable_horizon_requires_update_budget_for_implicit_lr_curve() -> None ): trainer.train(total_timesteps=8) assert trainer.num_updates == 0 + + +@pytest.mark.parametrize("rollout_mode", ["segmented", "complete"]) +def test_consumed_rollouts_are_released_before_further_collection_or_fitting( + monkeypatch, rollout_mode +): + import weakref + + env = _QuadraticActionEnv() + policy = ActorOnly(1, 1, env.device, actor=nn.Linear(1, 1)) + algorithm = APG(APGCfg(device="cpu"), policy) + trainer = DifferentiableTrainer( + DifferentiableTrainerCfg( + rollout_mode=rollout_mode, + segment_length=2, + update_horizon=4, + gradient_accumulation_steps=2, + deterministic_actions=True, + ), + env, + policy, + algorithm, + ) + completed = [] + collect = trainer.collector.collect + finish = algorithm.finish_update + + def collect_after_release(*args, **kwargs): + assert all(reference() is None for reference in completed) + rollout = collect(*args, **kwargs) + completed.append(weakref.ref(rollout)) + return rollout + + def finish_after_release(): + assert all(reference() is None for reference in completed) + return finish() + + monkeypatch.setattr(trainer.collector, "collect", collect_after_release) + monkeypatch.setattr(algorithm, "finish_update", finish_after_release) + trainer.update_once() + assert len(completed) == 2 diff --git a/tests/learning/test_newton_planar_reach.py b/tests/learning/test_newton_planar_reach.py index b6c210b34d..53de692124 100644 --- a/tests/learning/test_newton_planar_reach.py +++ b/tests/learning/test_newton_planar_reach.py @@ -27,7 +27,7 @@ from embodichain.learning.rl.algo import APG, APGCfg from embodichain.learning.rl.algo import segmented_discounted_return from embodichain.learning.rl.collector import DifferentiableCollector -from embodichain.learning.rl.env import DifferentiableVecEnv +from embodichain.learning.rl.env import DifferentiableEnvProtocol from embodichain.learning.rl.experimental.newton import ( NewtonPlanarReachEnv, NewtonPlanarReachEnvCfg, @@ -132,7 +132,7 @@ def test_newton_env_satisfies_contract_and_fk_matches_analytical_pose() -> None: dim=-1, ) - assert isinstance(env, DifferentiableVecEnv) + assert isinstance(env, DifferentiableEnvProtocol) assert observation.shape == (2, 8) assert torch.allclose(observation[:, 2:4], expected_xy, atol=1e-5) diff --git a/tests/learning/test_online_schedule.py b/tests/learning/test_online_schedule.py new file mode 100644 index 0000000000..1c8b2d9694 --- /dev/null +++ b/tests/learning/test_online_schedule.py @@ -0,0 +1,80 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Fractional update ratios, model warmup and schedule restoration.""" + +from __future__ import annotations + +import pytest +from embodichain.learning.dynamics import ( + OnlineModelBasedScheduleCfg, + OnlineModelBasedSchedule, +) + + +def test_prefill_warmup_and_resumed_fractional_update_credit(): + schedule = OnlineModelBasedSchedule( + OnlineModelBasedScheduleCfg( + warmup_updates=2, updates_per_collect=0.4, model_updates_per_cycle=3 + ) + ) + events = [] + + def collect(): + events.append("collect") + return 8 + + def model(): + events.append("model") + return {"loss": 2.0} + + def policy(): + events.append("policy") + return {"loss": 3.0} + + schedule.collect(collect, prefill=True) + with pytest.raises(RuntimeError, match="warmup"): + schedule.step(collect, model, policy) + schedule.warmup(model) + schedule.warmup(model) + schedule.step(collect, model, policy) + schedule.step(collect, model, policy) + assert events == ["collect", "model", "model", "collect", "collect"] + restored = OnlineModelBasedSchedule(schedule.cfg) + restored.load_state_dict(schedule.state_dict()) + events.clear() + for _ in range(3): + restored.step(collect, model, policy) + assert events == [ + "collect", + "model", + "model", + "model", + "policy", + "collect", + "collect", + "model", + "model", + "model", + "policy", + ] + assert ( + restored.iterations == 5 + and restored.policy_updates == 2 + and restored.model_updates == 8 + ) + assert restored.prefill_steps == 1 and restored.physical_transitions == 48 + assert restored.credit == pytest.approx(0.0) diff --git a/tests/learning/test_point_mass.py b/tests/learning/test_point_mass.py index 58cc973b4a..ba11258142 100644 --- a/tests/learning/test_point_mass.py +++ b/tests/learning/test_point_mass.py @@ -27,7 +27,7 @@ from embodichain.learning.rl.buffer import RolloutBuffer from embodichain.learning.rl.collector import SyncCollector from embodichain.learning.rl.env import ( - DifferentiableVecEnv, + DifferentiableEnvProtocol, build_learning_env, ) from embodichain.learning.rl.policies import ActorCritic @@ -42,7 +42,7 @@ def test_point_mass_is_registered_differentiable_environment() -> None: env = build_learning_env("PointMassRL", num_envs=4, device="cpu") assert isinstance(env, PointMassEnv) - assert isinstance(env, DifferentiableVecEnv) + assert isinstance(env, DifferentiableEnvProtocol) assert env.single_observation_space.shape == (14,) assert env.single_action_space.shape == (2,) diff --git a/tests/learning/test_runtime.py b/tests/learning/test_runtime.py index 77a005f160..8184196d34 100644 --- a/tests/learning/test_runtime.py +++ b/tests/learning/test_runtime.py @@ -241,3 +241,15 @@ def build(_id, *, base_env_cfg): ) ], ) + + +def test_learning_environment_rejects_config_resource_collision(): + from embodichain.learning.rl.runtime import _build_learning_environment + + with pytest.raises(ValueError, match="config and resources overlap"): + _build_learning_environment( + {"trainer": {"learning_env": {"name": "Unused", "cfg": {"model": "file"}}}}, + device=torch.device("cpu"), + num_envs=2, + resources={"model": object()}, + ) diff --git a/tests/models/dynamics/rwm/test_kinematic.py b/tests/models/dynamics/rwm/test_kinematic.py new file mode 100644 index 0000000000..0251d4a159 --- /dev/null +++ b/tests/models/dynamics/rwm/test_kinematic.py @@ -0,0 +1,182 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Heading increments, weighted autoregression and frozen-model gradients.""" + +from __future__ import annotations + +import math + +import pytest +import torch + +from embodichain_models.dynamics.rwm.kinematic import ( + KinematicWorldModel, + KinematicWorldModelCfg, + heading_state_increment, + integrate_heading_state, +) +from embodichain_models.dynamics.rwm.model import frozen_world_model +from embodichain_models.dynamics.rwm.robot_state import rotation_exp +from embodichain_models.dynamics.rwm.supervised import ( + RobotWorldModelObjective, + replay_supervised_batch, +) +from embodichain.learning.models.normalizer import EmpiricalNormalizer +from embodichain.learning.sl import SupervisedBatch, SupervisedLearner + +URDF = """ + +""" + + +def model_and_state(): + model = KinematicWorldModel( + URDF, + KinematicWorldModelCfg( + joint_names=("joint",), + resting_position=(0.0,), + action_dim=1, + context_dim=0, + hidden_dims=(16, 16), + ), + ) + state = torch.zeros(2, 20) + state[:, 2] = 1 + state[:, 3:12] = torch.eye(3).flatten() + return model, state + + +def test_heading_increment_moves_horizontal_and_transports_body_angular_velocity(): + _, state = model_and_state() + # A pitched robot still translates along the ground heading axes. + rotation = rotation_exp(torch.tensor([[0.0, math.pi / 3, 0.0]])) + state[:, 3:12] = rotation.flatten() + state[:, 15:18] = torch.tensor([0.0, 0.0, 1.0]) + delta = torch.zeros(2, 14) + delta[:, 0] = 0.4 + delta[:, 5] = 0.2 + predicted = integrate_heading_state(state, delta) + torch.testing.assert_close( + predicted[:, :3], state[:, :3] + torch.tensor([0.4, 0.0, 0.0]) + ) + expected_rotation = rotation @ rotation_exp(delta[:, 3:6]) + expected_angular = ( + expected_rotation @ rotation.transpose(-1, -2) @ state[:, 15:18, None] + ).squeeze(-1) + torch.testing.assert_close(predicted[:, 15:18], expected_angular) + torch.testing.assert_close( + heading_state_increment(state, predicted), delta, atol=1e-6, rtol=1e-5 + ) + + +def test_variance_epsilon_normalizer_uses_measured_moments_and_clips(): + normalizer = EmpiricalNormalizer(2, epsilon=0.5, variance_epsilon=True, clip=2) + normalizer.update(torch.tensor([[1.0, 3.0], [3.0, 3.0]])) + torch.testing.assert_close(normalizer.scale, torch.tensor([[math.sqrt(1.25), 0.5]])) + torch.testing.assert_close( + normalizer(torch.tensor([[2.0, 5.0]])), torch.tensor([[0.0, 2.0]]) + ) + + +@pytest.mark.parametrize("compiled", [False, True]) +def test_weighted_time_sum_matches_explicit_objective_and_backward_keeps_first_action( + compiled, monkeypatch +): + if compiled: + compile_fn = torch.compile + monkeypatch.setattr( + torch, + "compile", + lambda fn, **kwargs: compile_fn(fn, backend="aot_eager", fullgraph=True), + ) + torch.manual_seed(4) + model, state = model_and_state() + actions = torch.tensor( + [[[0.2], [0.3], [0.1]], [[-0.1], [0.1], [0.4]]], requires_grad=True + ) + target = state[:, None].expand(-1, 3, -1).clone() + target[..., 18] = 0.1 + batch = SupervisedBatch( + {"initial_state": state, "actions": actions}, + target, + {"valid": torch.ones(2, 3, dtype=torch.bool)}, + ) + weights = (1.0,) * 12 + (2.0, 3.0) + objective = RobotWorldModelObjective( + None, + temporal_reduction="sum", + error_weights=weights, + compile_mode="default" if compiled else None, + ) + with torch.device(actions.device): + result = objective.compute_loss(model, batch) + current, expected = state, state.new_zeros(()) + for step in range(3): + current = model(current, actions[:, step]) + error = ( + model.state_error(current, target[:, step]) / model.output_normalizer.scale + ) + expected = expected + (error.square() * error.new_tensor(weights)).mean() + torch.testing.assert_close(result.loss, expected) + optimizer = torch.optim.Adam(model.parameters(), lr=1e-3) + learner = SupervisedLearner(model, objective, optimizer, max_grad_norm=1) + before = [p.detach().clone() for p in model.parameters()] + learner.update(batch) + assert any( + not torch.equal(old, new) for old, new in zip(before, model.parameters()) + ) + optimizer.zero_grad(set_to_none=True) + with frozen_world_model(model): + current = state + for step in range(3): + current = model(current, actions[:, step]) + gradient = torch.autograd.grad(current[:, 18:].sum(), actions)[0] + assert torch.isfinite(gradient).all() and gradient[:, 0].abs().sum() > 0 + with pytest.raises(RuntimeError, match="frozen"): + model.update_normalization(state, actions[:, 0].detach(), state) + assert all(p.grad is None for p in model.parameters()) + + +def test_complete_window_adapter_rejects_unusable_physical_transitions(): + _, state = model_and_state() + windows = { + "state": state[:, None].repeat(1, 2, 1), + "next_state": state[:, None].repeat(1, 2, 1), + "action": torch.zeros(2, 2, 1), + "context": torch.zeros(2, 2, 0), + "valid": torch.ones(2, 2, dtype=torch.bool), + "usable": torch.tensor([[True, False], [True, True]]), + } + batch = replay_supervised_batch(windows, complete_windows=True) + assert batch.targets.shape[0] == 1 + windows["usable"][:] = False + with pytest.raises(ValueError, match="No complete"): + replay_supervised_batch(windows, complete_windows=True) + + +def test_checkpoint_rejects_different_kinematic_geometry(tmp_path): + model, _ = model_and_state() + path = tmp_path / "world-model.pt" + torch.save({"learner": {"model": model.state_dict()}}, path) + state = torch.load(path, weights_only=True)["learner"]["model"] + restored, _ = model_and_state() + restored.load_state_dict(state) + for left, right in zip(model.parameters(), restored.parameters()): + torch.testing.assert_close(left, right, rtol=0, atol=0) + other = KinematicWorldModel(URDF.replace('xyz="1 0 0"', 'xyz="2 0 0"'), model.cfg) + with pytest.raises(ValueError, match="contract"): + other.load_state_dict(state) diff --git a/tests/models/dynamics/rwm/test_transition_replay.py b/tests/models/dynamics/rwm/test_transition_replay.py index 98944a781c..10437c4625 100644 --- a/tests/models/dynamics/rwm/test_transition_replay.py +++ b/tests/models/dynamics/rwm/test_transition_replay.py @@ -91,6 +91,11 @@ def test_history_padding_respects_episode_and_oldest_retained_boundary(): torch.testing.assert_close( starts["history"]["state"][:, -1], starts["start"]["state"] ) + # Only episodes starting at step 4 (env 0) or step 5 (env 1) can supply + # complete reset-padded histories; the previous episodes lost their starts. + torch.testing.assert_close( + starts["history_complete"], starts["start"]["episode_id"] == 1 + ) def test_snapshot_restores_content_boundaries_and_rng_before_mutation(): @@ -125,3 +130,31 @@ def test_memory_budget_and_invalid_sampling_fail_without_waits(): replay.sample_windows(1, 1) with pytest.raises(ValueError, match="nonempty"): replay.sample_starts(1, 2) + + +def test_auxiliary_fields_are_aligned_owned_budgeted_and_restorable(): + fields = {"frame": (None, torch.int64), "policy_action": (1, torch.float32)} + replay = TransitionReplay(4, 1, 3, 1, extra_fields=fields) + base = TransitionReplay(4, 1, 3, 1) + assert replay.allocated_bytes - base.allocated_bytes == 4 * (8 + 4) + for step in range(6): + action = torch.tensor([[float(step)]], requires_grad=True) + replay.append( + torch.full((1, 3), float(step)), + action + 100, + torch.full((1, 3), float(step + 1)), + extras={"frame": torch.tensor([step + 1000]), "policy_action": action}, + ) + before = replay.state_dict() + with pytest.raises(ValueError, match="declared"): + replay.append(torch.zeros(1, 3), torch.zeros(1, 1), torch.zeros(1, 3)) + assert replay.cursor == before["cursor"] + restored = TransitionReplay(4, 1, 3, 1, extra_fields=fields) + restored.load_state_dict(before) + for replay in (replay, restored): + sample = replay.sample_windows(16, 2) + torch.testing.assert_close( + sample["frame"].float(), sample["state"][..., 0] + 1000 + ) + torch.testing.assert_close(sample["policy_action"] + 100, sample["action"]) + assert not sample["policy_action"].requires_grad diff --git a/tests/sim/motion/test_explicit_interpolation.py b/tests/sim/motion/test_explicit_interpolation.py index bc1ee559bf..807b26271a 100644 --- a/tests/sim/motion/test_explicit_interpolation.py +++ b/tests/sim/motion/test_explicit_interpolation.py @@ -82,6 +82,102 @@ def solve(*, pose, joint_seed, name): return generator, seeds +def _selected_generator(tmp_path, env_ids): + generator, _ = _generator(tmp_path, batch=4, speed=10.0) + generator._env_ids = env_ids + origins = torch.arange(4, dtype=torch.float32)[:, None] * 2 + + def fk(*, qpos, name, to_matrix, env_ids=None): + selected = origins if env_ids is None else origins[list(env_ids)] + poses = torch.eye(4).repeat(len(selected), 1, 1) + poses[:, :1, 3] = qpos + selected + return poses + + def ik(*, pose, joint_seed, name, env_ids=None): + selected = origins if env_ids is None else origins[list(env_ids)] + qpos = pose[:, :1, 3] - selected + return torch.ones(len(selected), dtype=torch.bool), qpos + + generator.robot.compute_fk = fk + generator.robot.compute_ik = ik + return generator, origins[list(env_ids)] + + +@pytest.mark.parametrize("env_ids", [(1,), (3, 1, 0, 2)]) +def test_fixed_grid_uses_selected_velocity_limits_in_requested_order(tmp_path, env_ids): + generator, _ = _selected_generator(tmp_path, env_ids) + generator.robot.get_qvel_limits = lambda **kwargs: torch.tensor( + [[0.5], [2.0], [0.75], [3.0]] + ) + options = MotionGenOptions( + strategy="ik_interp", + sample_count=3, + interpolation_dt=0.1, + control_part="arm", + start_qpos=torch.zeros(len(env_ids), 1), + interpolation_mode="fixed_grid", + ) + # Moving 0.2 in two 0.1-second intervals requires a speed of 1.0. + result = generator.generate( + [PlanState.from_qpos(torch.full((len(env_ids), 1), 0.2))], options + ) + expected = torch.tensor([False, True, False, True])[list(env_ids)] + assert torch.equal(result.success, expected) + torch.testing.assert_close(result.positions[:, -1, 0], expected.float() * 0.2) + + +@pytest.mark.parametrize("env_ids", [(1,), (3, 1, 0, 2)]) +@pytest.mark.parametrize("linear_keyframes", [False, True]) +def test_sequential_cartesian_planning_uses_selected_environment_frames( + tmp_path, env_ids, linear_keyframes +): + generator, origins = _selected_generator(tmp_path, env_ids) + pose = torch.eye(4).repeat(len(env_ids), 1, 1) + pose[:, :1, 3] = origins + 0.2 + options = MotionGenOptions( + strategy="ik_interp", + sample_count=3 if linear_keyframes else 2, + interpolation_dt=0.1, + control_part="arm", + start_qpos=torch.zeros(len(env_ids), 1), + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + is_linear=linear_keyframes, + preserve_cartesian_samples=not linear_keyframes, + ) + result = generator.generate([PlanState.from_xpos(pose)], options) + assert result.is_all_success() + expected = torch.tensor([0.0, 0.1, 0.2] if linear_keyframes else [0.0, 0.2]) + torch.testing.assert_close( + result.positions, expected[None, :, None].expand(len(env_ids), -1, -1) + ) + + +def test_continuous_recovery_uses_selected_joint_bounds_and_velocity_limits(tmp_path): + generator, origins = _selected_generator(tmp_path, (1,)) + generator.robot.get_qpos_limits = lambda **kwargs: torch.tensor( + [[[-3.0, -2.0]], [[-1.0, 1.0]], [[2.0, 3.0]], [[4.0, 5.0]]] + ) + generator.robot.get_qvel_limits = lambda **kwargs: torch.tensor( + [[0.01], [2.0], [0.01], [0.01]] + ) + pose = torch.eye(4)[None] + pose[:, :1, 3] = origins + 0.1 + options = MotionGenOptions( + strategy="ik_interp", + sample_count=2, + interpolation_dt=0.1, + control_part="arm", + start_qpos=torch.zeros(1, 1), + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + ) + result = resolve_continuous_ik_path(generator, [PlanState.from_xpos(pose)], options) + assert result is not None and result.is_all_success() + torch.testing.assert_close(result.positions, torch.tensor([[[0.0], [0.1]]])) + torch.testing.assert_close(result.dt, torch.tensor([[0.0, 0.1]])) + + @pytest.mark.parametrize("cap_owner", ["runtime", "urdf"]) def test_fixed_grid_holds_failed_commands_and_preserves_candidate_for_retry( tmp_path, cap_owner diff --git a/tests/utils/test_math.py b/tests/utils/test_math.py index a70d907ae6..0e88413f32 100644 --- a/tests/utils/test_math.py +++ b/tests/utils/test_math.py @@ -25,6 +25,8 @@ from embodichain.utils import math as math_utils from embodichain.utils.math import ( + axis_angle_from_quat, + axis_angle_to_rotation_matrix, convert_quat, default_orientation, inv_transform, @@ -57,6 +59,19 @@ def test_math_wildcard_import_exposes_public_math_functions() -> None: assert {name for name in namespace if not name.startswith("_")} == expected +def test_rotation_conversions_have_finite_gradients_at_identity() -> None: + vectors = torch.tensor( + [[0.0, 0.0, 0.0], [1e-7, -2e-7, 3e-7], [0.2, -0.3, 0.1]], + dtype=torch.float64, + requires_grad=True, + ) + assert torch.autograd.gradcheck(axis_angle_to_rotation_matrix, (vectors,)) + identity = torch.tensor( + [[0.0, 0.0, 0.0, 1.0]], dtype=torch.float64, requires_grad=True + ) + assert torch.autograd.gradcheck(axis_angle_from_quat, (identity,)) + + @pytest.mark.parametrize( "device", ["cpu", pytest.param("cuda:0", marks=pytest.mark.gpu)] ) @@ -534,3 +549,13 @@ def test_quat_slerp_accepts_leaf_endpoints_with_gradients() -> None: assert torch.autograd.gradcheck( lambda first, second: quat_slerp(first, second, 0.25), (start, end) ) + + +def test_matrix_quaternion_gradient_at_identity_and_half_turn(): + """Unselected quaternion candidates keep finite derivatives.""" + matrices = ( + torch.stack((torch.eye(3), torch.diag(torch.tensor([1.0, -1.0, -1.0])))) + .double() + .requires_grad_() + ) + assert torch.autograd.gradcheck(quat_from_matrix, (matrices,))