diff --git a/src/simulation/README.md b/src/simulation/README.md index 56555944..63e622ca 100644 --- a/src/simulation/README.md +++ b/src/simulation/README.md @@ -10,7 +10,7 @@ src/simulation/ │ └── humanoid_rl/scripts/ ├── humanoid_rl_tasks/ # RL tasks, flat — one folder per task │ └── humanoid_rl_tasks/ -│ ├── inhand/ locomotion/ badminton/ +│ ├── inhand/ locomotion/ │ ├── pick_place/ # ManagerBasedRLEnv, driven by Isaac Lab Mimic (not PPO) │ └── push_block/ # PPO + vision distillation; also a teleop scene ├── humanoid_scenes/ # teleop data-collection scenes — @scene-discovered diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/__init__.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/__init__.py deleted file mode 100644 index 94123eb2..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/__init__.py +++ /dev/null @@ -1 +0,0 @@ -"""Badminton intercept RL task (phases 1 + 2).""" diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/badminton.md b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/badminton.md deleted file mode 100644 index e2214269..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/badminton.md +++ /dev/null @@ -1,134 +0,0 @@ -# Badminton: timed intercept (Wato arm) - -Timed EE intercept for the Wato hand-arm (`UsdModelAssets/right_arm/right_arm_assembly/right_arm_assembly.usd`) in Isaac Lab. The policy receives **privileged** swing targets (no vision): EE position, orientation, linear velocity at impact, seconds until arrival, and a one-step contact pulse. **Play/train with `debug_vis=True`** shows shrinking concentric rings (visual only); rings use the commanded orientation and flash at min size on contact, then hide until the next resample. - -**No shuttlecock in scene yet** — contact is proxied by racket link pose + velocity vs the commanded swing state at the intercept instant. - -**Environments** - -| Task ID | Scene | Mode | -| :--- | :--- | :--- | -| `Isaac-Badminton-Intercept-Humanoid-Arm-v0` | Ground + arm | Train | -| `Isaac-Badminton-Intercept-Humanoid-Arm-Play-v0` | Ground + arm | Play | - -## Train & play - -Run inside the **`simulation_isaac`** container (Isaac Lab 2.3.2 / Sim 5.1). Host setup: [`docker/simulation/isaac_lab/QUICKSTART.md`](../../../../../../../../docker/simulation/isaac_lab/QUICKSTART.md) §0–2. - -```bash -# Host: start container -cd ~/Desktop/humanoid && ./watod up -d && ./watod -t simulation_isaac_dev - -# Inside container — run from $RL_ROOT (src/simulation/humanoid_rl/) -cd $RL_ROOT - -# Train -$ISAACLAB/isaaclab.sh -p $RL_RUNNERS/train.py \ - --task=Isaac-Badminton-Intercept-Humanoid-Arm-v0 --headless - -# Play — omit --headless to see intercept rings -$ISAACLAB/isaaclab.sh -p $RL_RUNNERS/play.py \ - --task=Isaac-Badminton-Intercept-Humanoid-Arm-Play-v0 --num_envs=1 - -# Play — specific checkpoint -$ISAACLAB/isaaclab.sh -p $RL_RUNNERS/play.py \ - --task=Isaac-Badminton-Intercept-Humanoid-Arm-Play-v0 --num_envs=1 \ - --checkpoint logs/rsl_rl/badminton_intercept_humanoid_arm//model_.pt -``` - -Shorthand aliases (after image rebuild): `rl-train --task=...` / `rl-play --task=... --num_envs=1`. - -Checkpoints: `logs/rsl_rl/badminton_intercept_humanoid_arm/`. PPO defaults: `max_iterations=500`, `experiment_name=badminton_intercept_humanoid_arm` (`config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py`). - -## Scene & command - -| Item | Value | -| :--- | :--- | -| Robot | `ARM_CFG` — 6-DOF arm actuated; fingers locked in racket-grip pose on reset | -| Racket proxy (rewards) | `forearm_v8_.*`, `DIP_INDEX_v1_.*` until a racket link exists in `right_arm_assembly.usd` | -| Intercept resample | After each cycle (**lead_time + hit window**); skipped if episode time is too short | -| Intercept position (base frame) | `x ∈ [-0.55, -0.15]`, `y ∈ [-0.45, 0.45]`, `z ∈ [0.15, 0.75]` m | -| Lead time (shuttle arrival) | Uniform **1.5–3.5 s** after each resample | -| Hit moment (pulse in command) | **`hit_moment_duration_s=0.13`** (~2 env steps) when lead time reaches 0 | -| Privileged command (12-D) | `[pos_xyz, quat_wxyz, vel_xyz, hit_pulse, time_to_hit]` — layout in `mdp/intercept_layout.py` | -| Impact orientation (base) | `roll ∈ [-0.15, 0.15]`, `pitch ∈ [0.45, 0.65]`, `yaw ∈ [-0.35, 0.35]` rad | -| Impact speed | Uniform **0.4–1.5 m/s** along base → intercept (arm-reachable) | - -### Debug visualization (rings) - -| Phase | Ring behavior | -| :--- | :--- | -| Countdown | Scale **1.0 → 0.35** linearly with remaining lead time | -| Contact | One step at **min scale** | -| After contact | Hidden until next resample (`post_hit_ring_hidden=True`) | - -Ring colors (center → outer): red, yellow, green, blue + white center dot. Config: `mdp/ring_marker_utils.py`. - -## EE end-state tracking (reward design) - -Paper-style target: commanded **EE position + orientation + full 3D velocity** at impact time. - -- **Command** (`UniformInterceptCommand`): intercept pose, quat, `vel_xyz` + `time_to_hit` + hit pulse. -- **Reward** (`ee_state_tracking_timed_exp`): - - $r = \exp(-\|e_{pos}\|^2)\,\exp(-\|e_{vel}\|^2)\,\exp(-e_{ori}^2)\,\bigl(e^{-t_{hit}/\tau} + \text{hit\_bonus}\,\mathbb{1}_{hit}\bigr)$ - - Full **velocity vector** must match (not just speed along one axis). Orientation in the main product. - -**Intended behavior:** full swing from ready; timing from `time_to_hit` in obs. - -| Term | Weight | Description | -| :--- | :--- | :--- | -| `ee_state_tracking_timed_exp` | 12.0 | `pos_std=0.10`, `vel_std=0.6`, `ori_std=0.8`, `timing_std=0.45`, `hit_bonus=2`. | -| `early_at_target_penalty` | −0.3 | Penalty for waiting at intercept early. | -| Action rate / joint vel | −0.05 / −0.01 | Smoothness (curriculum ramps penalties). | - -Hit window: `hit_moment_duration_s=0.20` s. - -### Curriculum - -| Term | Sim steps | -| :--- | :--- | -| `action_rate` → −0.08 | 25000 | -| `joint_vel` → −0.02 | 25000 | - -### Logged metrics (`UniformInterceptCommand._update_metrics`) - -| Metric | Meaning | -| :--- | :--- | -| `Metrics/intercept/position_error` | Closest proxy link ↔ commanded intercept [m] | -| `Metrics/intercept/orientation_error` | Quaternion error [rad] | -| `Metrics/intercept/velocity_error` | $\|v - v_{cmd}\|$ [m/s] | -| `Metrics/intercept/hit_in_moment` | In **13 cm** zone on hit pulse | - -## Terminations - -| Termination | Condition | -| :--- | :--- | -| Time out | Episode length exceeds **12 s**. | - -No success/failure termination on hit or miss. - -## Sim settings - -| Setting | Value | -| :--- | :--- | -| `decimation` | 4 | -| `sim.dt` | 1/60 s | -| `episode_length_s` | 12.0 | -| Default `num_envs` | 4096 (play: 50) | - -## Training notes - -- Watch **`ee_state_tracking`**, **`velocity_error`**, **`orientation_error`**, and **`hit_in_moment`** together — position alone can look OK while swing vector/ori stay wrong. -- Product reward is strict: all three must be reasonable near impact for high return. -- If learning stalls, try looser `ori_std` (1.0) or fixed `lead_time=(2.0, 2.5)` for early training. -- Lead time is **clamped to remaining episode time** so rings always finish shrinking before reset. -- Obs dim **60** (12-D command + joints + last action). **Retrain** after reward/command changes. - -## Future (phase 3+) - -- Shuttlecock rigid body + trajectory-derived commands -- Perception / estimated intercept in obs (drop privileged pose) -- Contact-force reward -- Replace racket proxy body names in `mdp/rewards.py` diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/badminton_env_cfg.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/badminton_env_cfg.py deleted file mode 100644 index 1becb432..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/badminton_env_cfg.py +++ /dev/null @@ -1,162 +0,0 @@ -import isaaclab.sim as sim_utils -from isaaclab.assets import AssetBaseCfg -from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab.managers import CurriculumTermCfg as CurrTerm -from isaaclab.managers import EventTermCfg as EventTerm -from isaaclab.managers import ObservationGroupCfg as ObsGroup -from isaaclab.managers import ObservationTermCfg as ObsTerm -from isaaclab.managers import RewardTermCfg as RewTerm -from isaaclab.managers import SceneEntityCfg -from isaaclab.managers import TerminationTermCfg as DoneTerm -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.utils import configclass -from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise - -import humanoid_rl_tasks.badminton.mdp as mdp -from pioneer_humanoid.arm_hand import ARM_CFG -from humanoid_rl_tasks.badminton.mdp.events import ARM_JOINT_NAMES -from humanoid_rl_tasks.badminton.mdp.rewards import DEFAULT_RACKET_BODY_NAMES - - -@configclass -class BadmintonSceneCfg(InteractiveSceneCfg): - ground = AssetBaseCfg( - prim_path="/World/ground", - spawn=sim_utils.GroundPlaneCfg(), - init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), - ) - - robot = ARM_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - - light = AssetBaseCfg( - prim_path="/World/light", - spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=2500.0), - ) - - -@configclass -class CommandsCfg: - intercept = mdp.UniformInterceptCommandCfg( - asset_name="robot", - # Ignored by UniformInterceptCommand: resample is cycle-aligned (lead_time + hit window). - resampling_time_range=(5.0, 5.0), - hit_moment_duration_s=0.20, - debug_vis=True, - ranges=mdp.UniformInterceptCommandCfg.Ranges( - pos_x=(-0.55, -0.15), - pos_y=(-0.45, 0.45), - pos_z=(0.15, 0.75), - lead_time=(1.5, 3.5), - speed=(0.4, 1.5), - ), - ) - - -@configclass -class ActionsCfg: - arm_action = mdp.JointPositionActionCfg( - asset_name="robot", - joint_names=ARM_JOINT_NAMES, - scale=0.5, - use_default_offset=True, - ) - - -@configclass -class ObservationsCfg: - @configclass - class PolicyCfg(ObsGroup): - joint_pos = ObsTerm(func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) - joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) - intercept_command = ObsTerm(func=mdp.generated_commands, params={"command_name": "intercept"}) - actions = ObsTerm(func=mdp.last_action) - - def __post_init__(self): - self.enable_corruption = True - self.concatenate_terms = True - - policy: PolicyCfg = PolicyCfg() - - -@configclass -class EventCfg: - reset_robot_joints = EventTerm( - func=mdp.reset_arm_and_racket_grip, - mode="reset", - params={ - "position_range": (0.75, 1.25), - "velocity_range": (0.0, 0.0), - }, - ) - - -@configclass -class RewardsCfg: - # Only penalty that references pre-impact position: don't camp at the intercept. - early_at_target = RewTerm( - func=mdp.early_at_target_penalty, - weight=-0.3, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=DEFAULT_RACKET_BODY_NAMES), - "command_name": "intercept", - "zone_radius": 0.13, - "min_lead_time_remaining": 0.25, - }, - ) - # End-state tracking × urgency(t): strong near impact, ~0 if far or early (not proximity shaping). - ee_state_tracking = RewTerm( - func=mdp.ee_state_tracking_timed_exp, - weight=12.0, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=DEFAULT_RACKET_BODY_NAMES), - "command_name": "intercept", - "pos_std": 0.10, - "vel_std": 0.6, - "ori_std": 0.8, - "timing_std": 0.45, - "hit_bonus": 2.0, - }, - ) - - action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.05) - joint_vel = RewTerm( - func=mdp.joint_vel_l2, - weight=-0.01, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=ARM_JOINT_NAMES)}, - ) - - -@configclass -class TerminationsCfg: - time_out = DoneTerm(func=mdp.time_out, time_out=True) - - -@configclass -class CurriculumCfg: - action_rate = CurrTerm( - func=mdp.modify_reward_weight, - params={"term_name": "action_rate", "weight": -0.08, "num_steps": 25000}, - ) - joint_vel = CurrTerm( - func=mdp.modify_reward_weight, - params={"term_name": "joint_vel", "weight": -0.02, "num_steps": 25000}, - ) - - -@configclass -class BadmintonEnvCfg(ManagerBasedRLEnvCfg): - scene: BadmintonSceneCfg = BadmintonSceneCfg(num_envs=4096, env_spacing=2.5) - observations: ObservationsCfg = ObservationsCfg() - actions: ActionsCfg = ActionsCfg() - commands: CommandsCfg = CommandsCfg() - rewards: RewardsCfg = RewardsCfg() - terminations: TerminationsCfg = TerminationsCfg() - events: EventCfg = EventCfg() - curriculum: CurriculumCfg = CurriculumCfg() - - def __post_init__(self): - self.decimation = 4 - self.sim.render_interval = self.decimation - self.episode_length_s = 12.0 - self.viewer.eye = (2.5, 2.5, 2.0) - self.sim.dt = 1.0 / 60.0 diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/HumanoidRLEnv/__init__.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/HumanoidRLEnv/__init__.py deleted file mode 100644 index 15525b83..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/HumanoidRLEnv/__init__.py +++ /dev/null @@ -1,23 +0,0 @@ -import gymnasium as gym - -from . import agents - -gym.register( - id="Isaac-Badminton-Intercept-Humanoid-Arm-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:HumanoidArmBadmintonEnvCfg", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidArmBadmintonPPORunnerCfg", - }, -) - -gym.register( - id="Isaac-Badminton-Intercept-Humanoid-Arm-Play-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:HumanoidArmBadmintonEnvCfg_PLAY", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidArmBadmintonPPORunnerCfg", - }, -) diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py deleted file mode 100644 index 918fac95..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py +++ /dev/null @@ -1,33 +0,0 @@ -from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg - - -@configclass -class HumanoidArmBadmintonPPORunnerCfg(RslRlOnPolicyRunnerCfg): - num_steps_per_env = 24 - max_iterations = 500 - save_interval = 50 - experiment_name = "badminton_intercept_humanoid_arm" - run_name = "" - resume = False - empirical_normalization = True - policy = RslRlPpoActorCriticCfg( - init_noise_std=0.5, - actor_hidden_dims=[64, 64], - critic_hidden_dims=[64, 64], - activation="elu", - ) - algorithm = RslRlPpoAlgorithmCfg( - value_loss_coef=1.0, - use_clipped_value_loss=True, - clip_param=0.2, - entropy_coef=0.01, - num_learning_epochs=8, - num_mini_batches=4, - learning_rate=1.0e-3, - schedule="adaptive", - gamma=0.99, - lam=0.95, - desired_kl=0.01, - max_grad_norm=1.0, - ) diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/HumanoidRLEnv/joint_pos_env_cfg.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/HumanoidRLEnv/joint_pos_env_cfg.py deleted file mode 100644 index c930a3f6..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/HumanoidRLEnv/joint_pos_env_cfg.py +++ /dev/null @@ -1,22 +0,0 @@ -from isaaclab.utils import configclass - -from humanoid_rl_tasks.badminton.badminton_env_cfg import BadmintonEnvCfg - - -@configclass -class HumanoidArmBadmintonEnvCfg(BadmintonEnvCfg): - pass - - -@configclass -class HumanoidArmBadmintonEnvCfg_PLAY(HumanoidArmBadmintonEnvCfg): - def __post_init__(self): - super().__post_init__() - self.scene.num_envs = 50 - self.scene.env_spacing = 2.5 - self.observations.policy.enable_corruption = False - - -# PYTHONPATH=$(pwd) $ISAACLAB/isaaclab.sh -p $RL_RUNNERS/train.py --task=Isaac-Badminton-Intercept-Humanoid-Arm-v0 --headless -# -# PYTHONPATH=$(pwd) $ISAACLAB/isaaclab.sh -p $RL_RUNNERS/play.py --task=Isaac-Badminton-Intercept-Humanoid-Arm-Play-v0 --num_envs=1 diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/__init__.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/__init__.py deleted file mode 100644 index ecced0b6..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/config/__init__.py +++ /dev/null @@ -1 +0,0 @@ -"""Configurations for the badminton intercept environments.""" diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/__init__.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/__init__.py deleted file mode 100644 index ffcb3cd3..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -from isaaclab.envs.mdp import * # noqa: F401, F403 - -from .commands import * # noqa: F401, F403 -from .curriculum import * # noqa: F401, F403 -from .events import * # noqa: F401, F403 -from .rewards import * # noqa: F401, F403 diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/commands/__init__.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/commands/__init__.py deleted file mode 100644 index 49e6ddf0..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/commands/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -from .commands_cfg import UniformInterceptCommandCfg -from .intercept_command import UniformInterceptCommand diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/commands/commands_cfg.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/commands/commands_cfg.py deleted file mode 100644 index 73a675b7..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/commands/commands_cfg.py +++ /dev/null @@ -1,56 +0,0 @@ -from __future__ import annotations - -from dataclasses import MISSING - -from isaaclab.managers import CommandTermCfg -from isaaclab.utils import configclass - -from humanoid_rl_tasks.badminton.mdp.ring_marker_utils import ( - build_intercept_target_visualizer_cfg, -) - -from .intercept_command import UniformInterceptCommand - - -@configclass -class UniformInterceptCommandCfg(CommandTermCfg): - """Configuration for a timed EE intercept (badminton shuttle arrival) command.""" - - class_type: type = UniformInterceptCommand - - asset_name: str = MISSING - """Robot asset used to transform commands into the world frame.""" - - tracking_body_names: list[str] | str = "DIP_INDEX_v1_.*" - """Body name(s) used for command metrics (closest link if multiple).""" - - hit_moment_duration_s: float = 0.0 - """How long the hit-moment pulse stays active [s]. 0 = one env step (~67 ms).""" - - post_hit_ring_hidden: bool = True - """After the contact flash, hide rings until the next resample (visualization only).""" - - min_ring_scale: float = 0.35 - """Debug-vis ring scale on the one-step shuttle-contact flash.""" - - @configclass - class Ranges: - """Uniform distribution ranges for intercept commands.""" - - pos_x: tuple[float, float] = MISSING - pos_y: tuple[float, float] = MISSING - pos_z: tuple[float, float] = MISSING - lead_time: tuple[float, float] = MISSING - """Seconds after resample until the shuttle arrives at the intercept point.""" - roll: tuple[float, float] = (-0.15, 0.15) - """Desired EE roll at impact [rad] (base frame).""" - pitch: tuple[float, float] = (0.45, 0.65) - """Desired EE pitch at impact [rad] (base frame, ~racket face tilt).""" - yaw: tuple[float, float] = (-0.35, 0.35) - """Desired EE yaw at impact [rad] (base frame).""" - speed: tuple[float, float] = (0.4, 1.5) - """Desired EE linear speed at impact [m/s] along base→intercept axis.""" - - ranges: Ranges = MISSING - - target_visualizer_cfg = build_intercept_target_visualizer_cfg() diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/commands/intercept_command.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/commands/intercept_command.py deleted file mode 100644 index 45846749..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/commands/intercept_command.py +++ /dev/null @@ -1,233 +0,0 @@ -from __future__ import annotations - -import torch -from collections.abc import Sequence -from typing import TYPE_CHECKING - -from isaaclab.assets import Articulation -from isaaclab.managers import CommandTerm -from isaaclab.markers import VisualizationMarkers -from isaaclab.utils.math import combine_frame_transforms, quat_from_euler_xyz, quat_mul, quat_rotate_inverse - -from humanoid_rl_tasks.badminton.mdp.ee_tracking import best_racket_tracking_errors -from humanoid_rl_tasks.badminton.mdp.ring_marker_utils import NUM_INTERCEPT_MARKERS - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedEnv - - from .commands_cfg import UniformInterceptCommandCfg - - -class UniformInterceptCommand(CommandTerm): - """Timed EE intercept: position, orientation, and velocity at shuttle arrival. - - Privileged timing for the policy: full swing target, time-to-hit, and a short - hit-moment pulse when the shuttle would arrive. - - Debug visualization: rings shrink during countdown, flash at min size for one - step at contact, then hide until the next resample. - """ - - cfg: UniformInterceptCommandCfg - - def __init__(self, cfg: UniformInterceptCommandCfg, env: ManagerBasedEnv): - super().__init__(cfg, env) - - self.robot: Articulation = env.scene[cfg.asset_name] - self._body_ids, _ = self.robot.find_bodies(cfg.tracking_body_names) - - self.pos_command_b = torch.zeros(self.num_envs, 3, device=self.device) - self.quat_command_b = torch.zeros(self.num_envs, 4, device=self.device) - self.quat_command_b[:, 0] = 1.0 - self.vel_command_b = torch.zeros(self.num_envs, 3, device=self.device) - self.lead_time_left = torch.zeros(self.num_envs, device=self.device) - self.lead_time_total = torch.ones(self.num_envs, device=self.device) - self.hit_moment_time_left = torch.zeros(self.num_envs, device=self.device) - self.hit_moment_active = torch.zeros(self.num_envs, device=self.device) - - self.pos_command_w = torch.zeros_like(self.pos_command_b) - self.quat_command_w = torch.zeros(self.num_envs, 4, device=self.device) - - self.metrics["position_error"] = torch.zeros(self.num_envs, device=self.device) - self.metrics["orientation_error"] = torch.zeros(self.num_envs, device=self.device) - self.metrics["velocity_error"] = torch.zeros(self.num_envs, device=self.device) - self.metrics["hit_in_moment"] = torch.zeros(self.num_envs, device=self.device) - - def __str__(self) -> str: - msg = "UniformInterceptCommand:\n" - msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" - msg += f"\tHit moment duration: {self.cfg.hit_moment_duration_s} s (0 = one env step)\n" - msg += f"\tResampling: cycle-aligned (lead_time + hit window)\n" - return msg - - @property - def command(self) -> torch.Tensor: - """Privileged intercept command. Shape is (num_envs, 12). See module-level slices.""" - return torch.cat( - [ - self.pos_command_b, - self.quat_command_b, - self.vel_command_b, - self.hit_moment_active.unsqueeze(-1), - self.lead_time_left.unsqueeze(-1), - ], - dim=-1, - ) - - def _ring_scale(self) -> torch.Tensor: - """Visual-only ring scale: shrink → contact flash → hide.""" - lead_ratio = self.lead_time_left / self.lead_time_total.clamp(min=1.0e-3) - countdown_scale = self.cfg.min_ring_scale + (1.0 - self.cfg.min_ring_scale) * lead_ratio - - in_countdown = self.lead_time_left > 0.0 - at_contact = self.hit_moment_active > 0.5 - after_contact = (~in_countdown) & (~at_contact) - - scale = torch.where(in_countdown, countdown_scale, torch.full_like(countdown_scale, self.cfg.min_ring_scale)) - scale = torch.where(at_contact, torch.full_like(scale, self.cfg.min_ring_scale), scale) - if self.cfg.post_hit_ring_hidden: - scale = torch.where(after_contact, torch.zeros_like(scale), scale) - else: - scale = torch.where(after_contact, torch.ones_like(scale), scale) - return scale - - def _hit_moment_duration(self) -> float: - if self.cfg.hit_moment_duration_s > 0.0: - return self.cfg.hit_moment_duration_s - return self._env.step_dt - - def _episode_time_remaining(self, env_ids: Sequence[int] | torch.Tensor) -> torch.Tensor: - """Seconds until episode timeout for the given env indices.""" - if not isinstance(env_ids, torch.Tensor): - env_ids = torch.tensor(list(env_ids), device=self.device, dtype=torch.long) - steps_left = self._env.max_episode_length - self._env.episode_length_buf[env_ids] - return steps_left.float() * self._env.step_dt - - def _resample(self, env_ids: Sequence[int]): - """Resample after each intercept cycle; skip if the episode cannot fit a full countdown.""" - if len(env_ids) == 0: - return - - env_ids_t = torch.tensor(list(env_ids), device=self.device, dtype=torch.long) - hit_duration = self._hit_moment_duration() - episode_time_left = self._episode_time_remaining(env_ids_t) - min_cycle = hit_duration + self._env.step_dt - - can_resample = episode_time_left >= min_cycle - resample_ids = env_ids_t[can_resample] - skip_ids = env_ids_t[~can_resample] - - if len(resample_ids) > 0: - self.command_counter[resample_ids] += 1 - self._resample_command(resample_ids.tolist()) - self.time_left[resample_ids] = self.lead_time_total[resample_ids] + hit_duration - - if len(skip_ids) > 0: - # Not enough episode time for another countdown — hold idle until reset. - self.time_left[skip_ids] = episode_time_left[skip_ids] - - def _update_metrics(self): - self.pos_command_w, _ = combine_frame_transforms( - self.robot.data.root_pos_w, - self.robot.data.root_quat_w, - self.pos_command_b, - ) - self.quat_command_w = quat_mul(self.robot.data.root_quat_w, self.quat_command_b) - - pos_err, ori_err, vel_err, _ = best_racket_tracking_errors(self.command, self.robot, self._body_ids) - self.metrics["position_error"] = pos_err - self.metrics["orientation_error"] = ori_err - self.metrics["velocity_error"] = vel_err - self.metrics["hit_in_moment"] = ((self.hit_moment_active > 0.5) & (pos_err < 0.13)).float() - - def _resample_command(self, env_ids: Sequence[int]): - r = torch.empty(len(env_ids), device=self.device) - self.pos_command_b[env_ids, 0] = r.uniform_(*self.cfg.ranges.pos_x) - self.pos_command_b[env_ids, 1] = r.uniform_(*self.cfg.ranges.pos_y) - self.pos_command_b[env_ids, 2] = r.uniform_(*self.cfg.ranges.pos_z) - - roll = r.uniform_(*self.cfg.ranges.roll) - pitch = r.uniform_(*self.cfg.ranges.pitch) - yaw = r.uniform_(*self.cfg.ranges.yaw) - self.quat_command_b[env_ids] = quat_from_euler_xyz(roll, pitch, yaw) - - # Strike velocity: world-frame direction base → intercept, magnitude from range. - root_pos_w = self.robot.data.root_pos_w[env_ids] - root_quat_w = self.robot.data.root_quat_w[env_ids] - des_pos_w, _ = combine_frame_transforms( - root_pos_w, - root_quat_w, - self.pos_command_b[env_ids], - ) - strike_dir_w = des_pos_w - root_pos_w - strike_dir_w = strike_dir_w / torch.norm(strike_dir_w, dim=-1, keepdim=True).clamp(min=1.0e-6) - speed = r.uniform_(*self.cfg.ranges.speed) - des_vel_w = strike_dir_w * speed.unsqueeze(-1) - self.vel_command_b[env_ids] = quat_rotate_inverse(root_quat_w, des_vel_w) - - env_ids_t = torch.tensor(list(env_ids), device=self.device, dtype=torch.long) - episode_time_left = self._episode_time_remaining(env_ids_t) - hit_duration = self._hit_moment_duration() - max_lead = (episode_time_left - hit_duration).clamp(min=self._env.step_dt) - - lo, hi = self.cfg.ranges.lead_time - sampled_lead = lo + (hi - lo) * r.uniform_(0.0, 1.0) - sampled_lead = torch.minimum(sampled_lead, max_lead) - self.lead_time_left[env_ids] = sampled_lead - self.lead_time_total[env_ids] = sampled_lead - self.hit_moment_time_left[env_ids] = 0.0 - self.hit_moment_active[env_ids] = 0.0 - - def _update_command(self): - dt = self._env.step_dt - hit_duration = self._hit_moment_duration() - - shuttle_arrives = (self.lead_time_left > 0.0) & (self.lead_time_left - dt <= 0.0) - self.lead_time_left = torch.clamp(self.lead_time_left - dt, min=0.0) - - self.hit_moment_time_left = torch.where( - shuttle_arrives, - torch.full_like(self.hit_moment_time_left, hit_duration), - self.hit_moment_time_left, - ) - self.hit_moment_time_left = torch.where( - self.hit_moment_time_left > 0.0, - torch.clamp(self.hit_moment_time_left - dt, min=0.0), - self.hit_moment_time_left, - ) - self.hit_moment_active = (self.hit_moment_time_left > 0.0).float() - - def _set_debug_vis_impl(self, debug_vis: bool): - if debug_vis: - if not hasattr(self, "target_visualizer"): - self.target_visualizer = VisualizationMarkers(self.cfg.target_visualizer_cfg) - self.target_visualizer.set_visibility(True) - elif hasattr(self, "target_visualizer"): - self.target_visualizer.set_visibility(False) - - def _debug_vis_callback(self, event): - if not self.robot.is_initialized: - return - - self._update_metrics() - - num_markers = self.num_envs * NUM_INTERCEPT_MARKERS - translations = self.pos_command_w.repeat_interleave(NUM_INTERCEPT_MARKERS, dim=0) - orientations = self.quat_command_w.repeat_interleave(NUM_INTERCEPT_MARKERS, dim=0) - - env_ring_scale = self._ring_scale() - ring_scale = env_ring_scale.repeat_interleave(NUM_INTERCEPT_MARKERS) - scales = ring_scale.unsqueeze(-1).expand(-1, 3).clone() - center_mask = ( - torch.arange(num_markers, device=self.device) % NUM_INTERCEPT_MARKERS - ) == (NUM_INTERCEPT_MARKERS - 1) - center_scale = torch.where(env_ring_scale > 0.0, 0.45, 0.0).repeat_interleave(NUM_INTERCEPT_MARKERS) - scales[center_mask] = center_scale[center_mask].unsqueeze(-1).expand(-1, 3) - - marker_indices = torch.arange(NUM_INTERCEPT_MARKERS, device=self.device).repeat(self.num_envs) - self.target_visualizer.visualize( - translations=translations, - orientations=orientations, - scales=scales, - marker_indices=marker_indices, - ) diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/curriculum.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/curriculum.py deleted file mode 100644 index 05898186..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/curriculum.py +++ /dev/null @@ -1,33 +0,0 @@ -"""Badminton-specific curriculum helpers.""" - -from __future__ import annotations - -from collections.abc import Sequence -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - - -def ramp_reward_weight( - env: ManagerBasedRLEnv, - env_ids: Sequence[int], - term_name: str, - start_weight: float, - end_weight: float, - start_step: int, - end_step: int, -) -> None: - """Linearly ramp a reward term weight between ``start_step`` and ``end_step``.""" - step = env.common_step_counter - if step <= start_step: - weight = start_weight - elif step >= end_step: - weight = end_weight - else: - alpha = (step - start_step) / max(end_step - start_step, 1) - weight = start_weight + alpha * (end_weight - start_weight) - - term_cfg = env.reward_manager.get_term_cfg(term_name) - term_cfg.weight = weight - env.reward_manager.set_term_cfg(term_name, term_cfg) diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/ee_tracking.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/ee_tracking.py deleted file mode 100644 index 4998ce91..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/ee_tracking.py +++ /dev/null @@ -1,136 +0,0 @@ -"""Shared EE tracking errors for intercept command metrics and rewards.""" - -from __future__ import annotations - -import torch -from typing import TYPE_CHECKING - -from isaaclab.assets import Articulation -from isaaclab.managers import SceneEntityCfg -from isaaclab.utils.math import combine_frame_transforms, quat_apply, quat_error_magnitude, quat_mul - -from humanoid_rl_tasks.badminton.mdp.intercept_layout import ( - COMMAND_HIT_SLICE, - COMMAND_POS_SLICE, - COMMAND_QUAT_SLICE, - COMMAND_TIME_SLICE, - COMMAND_VEL_SLICE, -) - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - -DEFAULT_RACKET_BODY_NAMES = [ - "forearm_v8_.*", - "DIP_INDEX_v1_.*", -] - - -def parse_intercept_command( - command: torch.Tensor, -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: - """Split privileged intercept command into pose, velocity, hit pulse, and time-to-hit.""" - return ( - command[:, COMMAND_POS_SLICE], - command[:, COMMAND_QUAT_SLICE], - command[:, COMMAND_VEL_SLICE], - command[:, COMMAND_HIT_SLICE], - command[:, COMMAND_TIME_SLICE], - ) - - -def command_targets_w( - command: torch.Tensor, asset: Articulation -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: - """World-frame position, orientation, velocity, hit pulse, and lead time.""" - des_pos_b, des_quat_b, des_vel_b, hit_moment_active, lead_time_left = parse_intercept_command(command) - des_pos_w, _ = combine_frame_transforms( - asset.data.root_state_w[:, :3], asset.data.root_state_w[:, 3:7], des_pos_b - ) - des_quat_w = quat_mul(asset.data.root_state_w[:, 3:7], des_quat_b) - des_vel_w = quat_apply(asset.data.root_state_w[:, 3:7], des_vel_b) - return des_pos_w, des_quat_w, des_vel_w, hit_moment_active, lead_time_left - - -def _body_positions_w(asset: Articulation, body_ids: list[int]) -> torch.Tensor: - return asset.data.body_state_w[:, body_ids, :3] - - -def _body_orientations_w(asset: Articulation, body_ids: list[int]) -> torch.Tensor: - return asset.data.body_state_w[:, body_ids, 3:7] - - -def _body_lin_vel_w(asset: Articulation, body_ids: list[int]) -> torch.Tensor: - return asset.data.body_lin_vel_w[:, body_ids, :] - - -def best_racket_tracking_errors( - command: torch.Tensor, - asset: Articulation, - body_ids: list[int], -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: - """Position, orientation, and velocity errors for the closest racket proxy link.""" - des_pos_w, des_quat_w, des_vel_w, _, _ = command_targets_w(command, asset) - body_pos_w = _body_positions_w(asset, body_ids) - body_quat_w = _body_orientations_w(asset, body_ids) - body_vel_w = _body_lin_vel_w(asset, body_ids) - - dists = torch.norm(body_pos_w - des_pos_w.unsqueeze(1), dim=-1) - body_idx = dists.argmin(dim=1) - env_ids = torch.arange(command.shape[0], device=command.device) - - pos_err = dists[env_ids, body_idx] - ori_err = quat_error_magnitude(body_quat_w[env_ids, body_idx], des_quat_w) - vel_err = torch.norm(body_vel_w[env_ids, body_idx] - des_vel_w, dim=-1) - return pos_err, ori_err, vel_err, body_idx - - -def best_racket_tracking_errors_from_env( - env: ManagerBasedRLEnv, - command_name: str, - asset_cfg: SceneEntityCfg, -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: - """Convenience wrapper using the env command manager and scene entity body ids.""" - asset: Articulation = env.scene[asset_cfg.name] - command = env.command_manager.get_command(command_name) - pos_err, ori_err, vel_err, _ = best_racket_tracking_errors( - command, asset, asset_cfg.body_ids # type: ignore[arg-type] - ) - return pos_err, ori_err, vel_err - - -def best_racket_state_w( - command: torch.Tensor, - asset: Articulation, - body_ids: list[int], -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: - """World-frame position and linear velocity of the closest racket proxy link.""" - pos_err, _, _, body_idx = best_racket_tracking_errors(command, asset, body_ids) - env_ids = torch.arange(command.shape[0], device=command.device) - body_pos_w = _body_positions_w(asset, body_ids) - body_vel_w = _body_lin_vel_w(asset, body_ids) - return body_pos_w[env_ids, body_idx], body_vel_w[env_ids, body_idx], pos_err - - -def best_racket_state_from_env( - env: ManagerBasedRLEnv, - command_name: str, - asset_cfg: SceneEntityCfg, -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: - asset: Articulation = env.scene[asset_cfg.name] - command = env.command_manager.get_command(command_name) - return best_racket_state_w(command, asset, asset_cfg.body_ids) # type: ignore[arg-type] - - -def strike_axis_w(command: torch.Tensor, asset: Articulation) -> tuple[torch.Tensor, torch.Tensor]: - """Unit strike direction and commanded speed magnitude [m/s] in world frame.""" - _, _, des_vel_w, _, _ = command_targets_w(command, asset) - cmd_speed = torch.norm(des_vel_w, dim=-1) - strike_dir = des_vel_w / cmd_speed.unsqueeze(-1).clamp(min=1.0e-6) - return strike_dir, cmd_speed - - -def impact_urgency_weight(lead_time_left: torch.Tensor, urgency_time_constant: float, prep_floor: float) -> torch.Tensor: - """Weight in [prep_floor, 1]; always some signal during countdown, peak at impact.""" - urgency = torch.exp(-lead_time_left / max(urgency_time_constant, 1.0e-3)) - return prep_floor + (1.0 - prep_floor) * urgency diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/events.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/events.py deleted file mode 100644 index 15c4b547..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/events.py +++ /dev/null @@ -1,67 +0,0 @@ -from __future__ import annotations - -import torch -from typing import TYPE_CHECKING - -from isaaclab.assets import Articulation -from isaaclab.envs.mdp.events import reset_joints_by_scale -from isaaclab.managers import SceneEntityCfg - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedEnv - -ARM_JOINT_NAMES = [ - "shoulder_flexion_extension", - "shoulder_abduction_adduction", - "shoulder_rotation", - "elbow_flexion_extension", - "forearm_rotation", - "wrist_extension", -] - -RACKET_GRIP_JOINT_POS = { - "mcp_index": 0.8, - "pip_index": 0.9, - "dip_index": 0.7, - "mcp_middle": 0.8, - "pip_middle": 0.9, - "dip_middle": 0.7, - "mcp_ring": 0.75, - "pip_ring": 0.85, - "dip_ring": 0.65, - "mcp_pinky": 0.7, - "pip_pinky": 0.8, - "dip_pinky": 0.6, - "cmc_thumb": 0.5, - "mcp_thumb": 1.2, - "ip_thumb": 0.6, -} - - -def reset_arm_and_racket_grip( - env: ManagerBasedEnv, - env_ids: torch.Tensor, - position_range: tuple[float, float], - velocity_range: tuple[float, float], - asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), -): - """Randomize arm joints and restore a fixed racket-grip pose on the fingers.""" - asset: Articulation = env.scene[asset_cfg.name] - - reset_joints_by_scale( - env, - env_ids, - position_range=position_range, - velocity_range=velocity_range, - asset_cfg=asset_cfg, - ) - - joint_pos = asset.data.joint_pos[env_ids].clone() - joint_vel = asset.data.joint_vel[env_ids].clone() - for joint_name, grip_pos in RACKET_GRIP_JOINT_POS.items(): - joint_ids, _ = asset.find_joints(joint_name) - for joint_idx in joint_ids: - joint_pos[:, joint_idx] = grip_pos - joint_vel[:, joint_idx] = 0.0 - - asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/intercept_layout.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/intercept_layout.py deleted file mode 100644 index 06cc6849..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/intercept_layout.py +++ /dev/null @@ -1,15 +0,0 @@ -"""Privileged intercept command tensor layout (shared by command + tracking).""" - -# Command shape: (num_envs, 12) -# [0:3] intercept position in robot base frame -# [3:7] desired EE orientation (quaternion, base frame) -# [7:10] desired EE linear velocity at impact (base frame) -# [10] hit-moment pulse (1.0 on shuttle-arrival step) -# [11] seconds until shuttle arrival - -COMMAND_DIM = 12 -COMMAND_POS_SLICE = slice(0, 3) -COMMAND_QUAT_SLICE = slice(3, 7) -COMMAND_VEL_SLICE = slice(7, 10) -COMMAND_HIT_SLICE = 10 -COMMAND_TIME_SLICE = 11 diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/rewards.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/rewards.py deleted file mode 100644 index 56a9ac55..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/rewards.py +++ /dev/null @@ -1,78 +0,0 @@ -from __future__ import annotations - -from typing import TYPE_CHECKING - -import torch - -from isaaclab.managers import SceneEntityCfg - -from humanoid_rl_tasks.badminton.mdp.ee_tracking import ( - DEFAULT_RACKET_BODY_NAMES, - best_racket_tracking_errors_from_env, - parse_intercept_command, -) - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - - -def early_at_target_penalty( - env: ManagerBasedRLEnv, - command_name: str, - asset_cfg: SceneEntityCfg, - zone_radius: float, - min_lead_time_remaining: float, -) -> torch.Tensor: - """Penalize waiting at the intercept before impact (does not reward moving closer).""" - command = env.command_manager.get_command(command_name) - pos_err, _, _ = best_racket_tracking_errors_from_env(env, command_name, asset_cfg) - _, _, _, _, lead_time_left = parse_intercept_command(command) - - in_zone = (pos_err < zone_radius).float() - too_early = (lead_time_left > min_lead_time_remaining).float() - return in_zone * too_early - - -def _ee_tracking_terms( - env: ManagerBasedRLEnv, - command_name: str, - asset_cfg: SceneEntityCfg, - pos_std: float, - vel_std: float, - ori_std: float, -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: - """Exp kernels for position, full 3D velocity, and orientation errors.""" - pos_err, ori_err, vel_err = best_racket_tracking_errors_from_env(env, command_name, asset_cfg) - - pos_term = torch.exp(-torch.square(pos_err / max(pos_std, 1.0e-6))) - vel_term = torch.exp(-torch.square(vel_err / max(vel_std, 1.0e-6))) - ori_term = torch.exp(-torch.square(ori_err / max(ori_std, 1.0e-6))) - return pos_term, vel_term, ori_term, pos_err - - -def ee_state_tracking_timed_exp( - env: ManagerBasedRLEnv, - command_name: str, - asset_cfg: SceneEntityCfg, - pos_std: float, - vel_std: float, - ori_std: float, - timing_std: float = 0.45, - hit_bonus: float = 2.0, -) -> torch.Tensor: - """Paper-style EE tracking: pos + full 3D vel + orientation, strongest near impact. - - Product of exp kernels (all must match for high reward). Not a proximity reward: - far from target → ``pos_term ≈ 0``; wrong velocity vector → ``vel_term ≈ 0``; - on target early → ``exp(-t/τ) ≈ 0``. - """ - command = env.command_manager.get_command(command_name) - pos_term, vel_term, ori_term, _ = _ee_tracking_terms( - env, command_name, asset_cfg, pos_std, vel_std, ori_std - ) - _, _, _, hit_moment_active, lead_time_left = parse_intercept_command(command) - - tracking = pos_term * vel_term * ori_term - urgency = torch.exp(-lead_time_left / max(timing_std, 1.0e-3)) - at_hit = (hit_moment_active > 0.5).float() - return tracking * (urgency + hit_bonus * at_hit) diff --git a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/ring_marker_utils.py b/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/ring_marker_utils.py deleted file mode 100644 index 3a7663f8..00000000 --- a/src/simulation/humanoid_rl_tasks/humanoid_rl_tasks/badminton/mdp/ring_marker_utils.py +++ /dev/null @@ -1,112 +0,0 @@ -"""Flat annulus ring meshes for the badminton intercept target visualizer.""" - -from __future__ import annotations - -from collections.abc import Callable -from dataclasses import MISSING - -import numpy as np -import trimesh -from pxr import Usd - -import isaacsim.core.utils.prims as prim_utils -import isaaclab.sim as sim_utils -from isaaclab.markers import VisualizationMarkersCfg -from isaaclab.sim.spawners.meshes.meshes import _spawn_mesh_geom_from_mesh -from isaaclab.sim.spawners.meshes.meshes_cfg import MeshCfg -from isaaclab.utils import configclass - -INTERCEPT_RING_SPECS: tuple[tuple[str, float, float, tuple[float, float, float]], ...] = ( - ("ring_red", 0.0, 0.035, (1.0, 0.0, 0.0)), - ("ring_yellow", 0.035, 0.070, (1.0, 1.0, 0.0)), - ("ring_green", 0.070, 0.110, (0.0, 1.0, 0.0)), - ("ring_blue", 0.110, 0.160, (0.0, 0.0, 1.0)), -) - - -def make_annulus_trimesh(inner_radius: float, outer_radius: float, segments: int = 64) -> trimesh.Trimesh: - """Flat annulus in the XY plane (normal +Z), suitable for a tilted intercept disk.""" - angles = np.linspace(0.0, 2.0 * np.pi, segments, endpoint=False) - cos_a = np.cos(angles) - sin_a = np.sin(angles) - - outer = np.stack([outer_radius * cos_a, outer_radius * sin_a, np.zeros(segments)], axis=1) - if inner_radius <= 1.0e-6: - center = np.array([[0.0, 0.0, 0.0]]) - vertices = np.vstack([center, outer]) - faces = [] - for i in range(segments): - j = (i + 1) % segments - faces.append([0, i + 1, j + 1]) - return trimesh.Trimesh(vertices=vertices, faces=np.asarray(faces)) - - inner = np.stack([inner_radius * cos_a, inner_radius * sin_a, np.zeros(segments)], axis=1) - vertices = np.vstack([outer, inner]) - faces = [] - for i in range(segments): - j = (i + 1) % segments - o0, o1 = i, j - i0, i1 = segments + i, segments + j - faces.append([o0, o1, i1]) - faces.append([o0, i1, i0]) - return trimesh.Trimesh(vertices=vertices, faces=np.asarray(faces)) - - -@configclass -class AnnulusMeshCfg(MeshCfg): - """Visual-only flat annulus mesh.""" - - func: Callable[..., Usd.Prim] = MISSING - inner_radius: float = 0.0 - outer_radius: float = 0.1 - segments: int = 64 - visual_material: sim_utils.VisualMaterialCfg | None = None - - -def spawn_annulus( - prim_path: str, - cfg: AnnulusMeshCfg, - translation: tuple[float, float, float] | None = None, - orientation: tuple[float, float, float, float] | None = None, -) -> Usd.Prim: - """Spawn a flat annulus mesh prim for debug visualization.""" - mesh = make_annulus_trimesh(cfg.inner_radius, cfg.outer_radius, cfg.segments) - _spawn_mesh_geom_from_mesh(prim_path, cfg, mesh, translation, orientation) - return prim_utils.get_prim_at_path(prim_path) - - -def _ring_material(color: tuple[float, float, float], emissive_scale: float = 0.35) -> sim_utils.PreviewSurfaceCfg: - emissive = tuple(min(1.0, c * emissive_scale + 0.15) for c in color) - return sim_utils.PreviewSurfaceCfg( - diffuse_color=color, - emissive_color=emissive, - opacity=0.92, - ) - - -def build_intercept_target_visualizer_cfg() -> VisualizationMarkersCfg: - """Concentric flat rings + white center dot, like the badminton intercept reference.""" - markers: dict[str, AnnulusMeshCfg | sim_utils.SphereCfg] = {} - for name, inner_r, outer_r, color in INTERCEPT_RING_SPECS: - markers[name] = AnnulusMeshCfg( - func=spawn_annulus, - inner_radius=inner_r, - outer_radius=outer_r, - visual_material=_ring_material(color), - ) - markers["center_dot"] = sim_utils.SphereCfg( - radius=0.015, - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=(1.0, 1.0, 1.0), - emissive_color=(0.85, 0.85, 0.85), - opacity=1.0, - ), - ) - return VisualizationMarkersCfg( - prim_path="/Visuals/Command/intercept_target", - markers=markers, - ) - - -INTERCEPT_MARKER_NAMES: tuple[str, ...] = tuple(name for name, _, _, _ in INTERCEPT_RING_SPECS) + ("center_dot",) -NUM_INTERCEPT_MARKERS = len(INTERCEPT_MARKER_NAMES) diff --git a/src/simulation/humanoid_scenes/README.md b/src/simulation/humanoid_scenes/README.md index 3dbedb4d..cb3d0f7d 100644 --- a/src/simulation/humanoid_scenes/README.md +++ b/src/simulation/humanoid_scenes/README.md @@ -48,8 +48,8 @@ keyboard_teleop --scene # pass an unknown name to list them - **Here:** lightweight teleop/data-collection scenes — mostly a `scene.py`. - **Not here:** full task packages with training/eval/recording infra - (`so101_vial_task`) and the RL tasks (`humanoid_rl_tasks/` — `locomotion`, - `badminton`, `inhand`, `pick_place`, `push_block`). A scene that doubles as an + (`so101_vial_task`) and the RL tasks (`humanoid_rl_tasks/` — `inhand`, + `locomotion`, `pick_place`, `push_block`). A scene that doubles as an RL task keeps its geometry in `humanoid_rl_tasks//scene.py`; the folder here is just a one-liner that registers it for teleop (see `push_block/`).