Import upstream snapshot d424a0c899f6b33cbd3daeb279913134349c0b63
Upstream: https://github.com/pollen-robotics/microduck_rl Upstream-Commit: d424a0c899f6b33cbd3daeb279913134349c0b63 Upstream-Branch: develop
This commit is contained in:
Upstream Snapshot
@@ -0,0 +1,899 @@
|
||||
# Roller Crouch-Glide Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** Ajouter un geste « s'accroupir en glissant puis se relever » déclenché au bouton A, sans modifier le runtime Rust, en entraînant une policy mjlab chargée dans le slot `--ground-pick`.
|
||||
|
||||
**Architecture:** Nouvelle tâche mjlab entraînée sur le robot rollers, pilotée par la commande de phase `GroundPickPhaseCommand` (celle qu'envoie le slot ground-pick du runtime). Une nouvelle reward suit une cible de hauteur du tronc « en trapèze » (haut → bas → palier 1 s → haut) le long de la phase. Le même layout d'obs 61D que la policy roller → interchangeable au runtime. Export ONNX, chargé via `--ground-pick`.
|
||||
|
||||
**Tech Stack:** Python, PyTorch, mjlab 1.3.0, MuJoCo, uv, ONNX. Runtime cible : `apirrone/microduck_runtime` (Rust, binaire — NON modifié).
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- **Aucune modification du runtime Rust.** Le geste réutilise le slot `--ground-pick` existant (bouton A, one-shot).
|
||||
- **Layout d'obs unifié 61D** obligatoire (`--new-cmd-obs`) : `[twist(3), head(4), body(6)]`, head/body zero-paddés. Toute nouvelle policy DOIT conserver ce layout.
|
||||
- **14 joints actifs** (roues passives exclues via `SceneEntityCfg("robot", joint_names=(r"^(?!passive_).*",))`), `action.scale = 1.0`, `kp_fw = 200`.
|
||||
- **Parité entraînement/déploiement (sim2real) :** au déploiement, forcer `--ground-pick-kp-ratio 1.0` (défaut 0.6), `--ground-pick-action-scale` = action_scale runtime, `--ground-pick-period 5.0`.
|
||||
- **Phase encoding (imposé par le runtime) :** `command = [cos(2π·φ), sin(2π·φ), 0]`, période 4 s. Palier de glisse = 1 s → `hold_lo=0.375`, `hold_hi=0.625`.
|
||||
- **Commits simples** (pas de `Co-Authored-By`).
|
||||
- Lancer les tests via `uv run --with pytest pytest` (pas de dépendance pytest ajoutée au projet).
|
||||
- Spec de référence : `docs/superpowers/specs/2026-07-17-roller-crouch-glide-design.md`.
|
||||
|
||||
---
|
||||
|
||||
## File Structure
|
||||
|
||||
| Fichier | Responsabilité |
|
||||
|---|---|
|
||||
| `src/mjlab_microduck/tasks/mdp.py` | **Modifier.** Ajouter 3 fonctions : `crouch_height_target` (pure), `crouch_glide_reward_from_values` (pure), `crouch_glide_height_by_phase` (wrapper env) et `forward_speed_reward`. |
|
||||
| `tests/test_crouch_glide.py` | **Créer.** Tests unitaires des fonctions pures. |
|
||||
| `src/mjlab_microduck/tasks/microduck_roller_crouch_env_cfg.py` | **Créer.** L'env (hybride roller + phase) + `MicroduckRollerCrouchRlCfg`. |
|
||||
| `src/mjlab_microduck/tasks/__init__.py` | **Modifier.** Importer + enregistrer `Mjlab-RollerCrouch-Flat-MicroDuck`. |
|
||||
| `tests/test_roller_crouch_cfg.py` | **Créer.** Smoke test : l'env se construit avec la bonne commande/rewards. |
|
||||
|
||||
---
|
||||
|
||||
## Task 1: Cible de hauteur « en trapèze » (fonction pure)
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/mdp.py` (ajouter la fonction, après `com_height_target` vers la ligne 737)
|
||||
- Test: `tests/test_crouch_glide.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Produces: `crouch_height_target(phase: torch.Tensor, height_low: float, height_high: float, hold_lo: float = 0.375, hold_hi: float = 0.625) -> torch.Tensor` — prend la phase (B,) ∈ [0,1) et retourne la hauteur-cible (B,).
|
||||
|
||||
- [ ] **Step 1: Écrire le test qui échoue**
|
||||
|
||||
Créer `tests/test_crouch_glide.py` :
|
||||
|
||||
```python
|
||||
import math
|
||||
import torch
|
||||
from mjlab_microduck.tasks import mdp
|
||||
|
||||
|
||||
def test_crouch_height_target_endpoints_are_high():
|
||||
# phase 0 (début) et phase ~1 (fin) → hauteur haute (debout)
|
||||
phase = torch.tensor([0.0, 0.999])
|
||||
t = mdp.crouch_height_target(phase, height_low=0.075, height_high=0.11)
|
||||
assert torch.allclose(t, torch.tensor([0.11, 0.11]), atol=2e-3)
|
||||
|
||||
|
||||
def test_crouch_height_target_plateau_is_low():
|
||||
# tout le palier [0.375, 0.625] → hauteur basse constante
|
||||
phase = torch.tensor([0.375, 0.5, 0.624])
|
||||
t = mdp.crouch_height_target(phase, height_low=0.075, height_high=0.11)
|
||||
assert torch.allclose(t, torch.full((3,), 0.075), atol=1e-6)
|
||||
|
||||
|
||||
def test_crouch_height_target_descent_midpoint():
|
||||
# milieu de la descente (phase = hold_lo/2 = 0.1875) → milieu des deux hauteurs
|
||||
phase = torch.tensor([0.1875])
|
||||
t = mdp.crouch_height_target(phase, height_low=0.075, height_high=0.11)
|
||||
assert torch.allclose(t, torch.tensor([(0.11 + 0.075) / 2]), atol=1e-6)
|
||||
|
||||
|
||||
def test_crouch_height_target_rise_midpoint():
|
||||
# milieu de la remontée (phase = 0.8125) → milieu des deux hauteurs
|
||||
phase = torch.tensor([0.8125])
|
||||
t = mdp.crouch_height_target(phase, height_low=0.075, height_high=0.11)
|
||||
assert torch.allclose(t, torch.tensor([(0.11 + 0.075) / 2]), atol=1e-6)
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Lancer le test pour vérifier qu'il échoue**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_crouch_glide.py -v`
|
||||
Expected: FAIL — `AttributeError: module ... has no attribute 'crouch_height_target'`
|
||||
|
||||
- [ ] **Step 3: Implémenter la fonction**
|
||||
|
||||
Dans `src/mjlab_microduck/tasks/mdp.py`, juste après `com_height_target` (après la ligne 737) :
|
||||
|
||||
```python
|
||||
def crouch_height_target(
|
||||
phase: torch.Tensor,
|
||||
height_low: float,
|
||||
height_high: float,
|
||||
hold_lo: float = 0.375,
|
||||
hold_hi: float = 0.625,
|
||||
) -> torch.Tensor:
|
||||
"""Cible de hauteur du tronc « en trapèze » le long de la phase [0,1).
|
||||
|
||||
phase ∈ [0, hold_lo) : descente height_high -> height_low
|
||||
phase ∈ [hold_lo, hold_hi): palier height_low (la glisse accroupie)
|
||||
phase ∈ [hold_hi, 1.0) : remontée height_low -> height_high
|
||||
|
||||
Args:
|
||||
phase: (B,) phase par env, dans [0, 1).
|
||||
height_low: hauteur du tronc accroupi (m).
|
||||
height_high: hauteur du tronc debout (m).
|
||||
hold_lo, hold_hi: bornes du palier bas en fraction de phase.
|
||||
Returns:
|
||||
(B,) hauteur-cible en mètres.
|
||||
"""
|
||||
descend = phase < hold_lo
|
||||
hold = (phase >= hold_lo) & (phase < hold_hi)
|
||||
|
||||
frac_d = phase / hold_lo
|
||||
t_descend = height_high + (height_low - height_high) * frac_d
|
||||
|
||||
t_hold = torch.full_like(phase, height_low)
|
||||
|
||||
frac_r = (phase - hold_hi) / (1.0 - hold_hi)
|
||||
t_rise = height_low + (height_high - height_low) * frac_r
|
||||
|
||||
return torch.where(descend, t_descend, torch.where(hold, t_hold, t_rise))
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Lancer le test pour vérifier qu'il passe**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_crouch_glide.py -v`
|
||||
Expected: PASS (4 tests)
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/mdp.py tests/test_crouch_glide.py
|
||||
git commit -m "roller-crouch: cible de hauteur en trapezoide (fonction pure + tests)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Task 2: Rewards crouch-glide et forward-speed
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/mdp.py`
|
||||
- Test: `tests/test_crouch_glide.py` (ajouts)
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `crouch_height_target` (Task 1).
|
||||
- Produces:
|
||||
- `crouch_glide_reward_from_values(com_height, cmd_cos, cmd_sin, height_low, height_high, hold_lo=0.375, hold_hi=0.625, std=0.02) -> torch.Tensor` (pure).
|
||||
- `crouch_glide_height_by_phase(env, command_name="twist", height_low=0.075, height_high=0.11, hold_lo=0.375, hold_hi=0.625, std=0.02, asset_cfg=_DEFAULT_ASSET_CFG) -> torch.Tensor` (wrapper env).
|
||||
- `forward_speed_reward(env, vel_ref=0.2, asset_cfg=_DEFAULT_ASSET_CFG) -> torch.Tensor` — récompense la vitesse avant (élan), indépendante de la commande.
|
||||
|
||||
- [ ] **Step 1: Écrire les tests qui échouent**
|
||||
|
||||
Ajouter à `tests/test_crouch_glide.py` :
|
||||
|
||||
```python
|
||||
def test_reward_is_one_when_height_matches_target():
|
||||
# phase 0.5 (plein palier) → cible = height_low ; si com_height == height_low → reward 1
|
||||
cmd_cos = torch.tensor([math.cos(2 * math.pi * 0.5)]) # -1
|
||||
cmd_sin = torch.tensor([math.sin(2 * math.pi * 0.5)]) # ~0
|
||||
com_height = torch.tensor([0.075])
|
||||
r = mdp.crouch_glide_reward_from_values(
|
||||
com_height, cmd_cos, cmd_sin, height_low=0.075, height_high=0.11, std=0.02
|
||||
)
|
||||
assert torch.allclose(r, torch.tensor([1.0]), atol=1e-3)
|
||||
|
||||
|
||||
def test_reward_decays_when_off_by_one_std():
|
||||
# à height_low + std de la cible → exp(-1) ≈ 0.368
|
||||
cmd_cos = torch.tensor([math.cos(2 * math.pi * 0.5)])
|
||||
cmd_sin = torch.tensor([math.sin(2 * math.pi * 0.5)])
|
||||
com_height = torch.tensor([0.075 + 0.02])
|
||||
r = mdp.crouch_glide_reward_from_values(
|
||||
com_height, cmd_cos, cmd_sin, height_low=0.075, height_high=0.11, std=0.02
|
||||
)
|
||||
assert torch.allclose(r, torch.tensor([math.exp(-1.0)]), atol=1e-3)
|
||||
|
||||
|
||||
def test_reward_at_phase_zero_expects_high_stance():
|
||||
# phase 0 → cible = height_high ; rester debout est récompensé, être accroupi non
|
||||
cmd_cos = torch.tensor([1.0, 1.0]) # cos(0)
|
||||
cmd_sin = torch.tensor([0.0, 0.0]) # sin(0)
|
||||
com_height = torch.tensor([0.11, 0.075]) # debout vs accroupi
|
||||
r = mdp.crouch_glide_reward_from_values(
|
||||
com_height, cmd_cos, cmd_sin, height_low=0.075, height_high=0.11, std=0.02
|
||||
)
|
||||
assert r[0] > 0.99 # debout à phase 0 → ~1
|
||||
assert r[1] < 0.2 # accroupi à phase 0 → faible
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Vérifier l'échec**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_crouch_glide.py -v`
|
||||
Expected: FAIL — `crouch_glide_reward_from_values` n'existe pas.
|
||||
|
||||
- [ ] **Step 3: Implémenter les trois fonctions**
|
||||
|
||||
Dans `src/mjlab_microduck/tasks/mdp.py`, à la suite de `crouch_height_target` :
|
||||
|
||||
```python
|
||||
def crouch_glide_reward_from_values(
|
||||
com_height: torch.Tensor,
|
||||
cmd_cos: torch.Tensor,
|
||||
cmd_sin: torch.Tensor,
|
||||
height_low: float,
|
||||
height_high: float,
|
||||
hold_lo: float = 0.375,
|
||||
hold_hi: float = 0.625,
|
||||
std: float = 0.02,
|
||||
) -> torch.Tensor:
|
||||
"""Récompense gaussienne du suivi de la cible de hauteur (fonction pure).
|
||||
|
||||
Décode la phase depuis [cos, sin] puis compare la hauteur mesurée à la
|
||||
cible-trapèze. Retourne exp(-((h - cible)/std)^2) ∈ (0, 1].
|
||||
"""
|
||||
phase = (torch.atan2(cmd_sin, cmd_cos) / (2 * torch.pi)) % 1.0
|
||||
target = crouch_height_target(phase, height_low, height_high, hold_lo, hold_hi)
|
||||
return torch.exp(-((com_height - target) / std) ** 2)
|
||||
|
||||
|
||||
def crouch_glide_height_by_phase(
|
||||
env: ManagerBasedRlEnv,
|
||||
command_name: str = "twist",
|
||||
height_low: float = 0.075,
|
||||
height_high: float = 0.11,
|
||||
hold_lo: float = 0.375,
|
||||
hold_hi: float = 0.625,
|
||||
std: float = 0.02,
|
||||
asset_cfg: SceneEntityCfg = _DEFAULT_ASSET_CFG,
|
||||
) -> torch.Tensor:
|
||||
"""Reward principale : suit la cible de hauteur du tronc le long de la phase.
|
||||
|
||||
La hauteur du CoM est calculée comme dans `com_height_target` (world z moins
|
||||
l'origine du terrain, nan->0). La phase provient de la commande GroundPick.
|
||||
"""
|
||||
asset: Entity = env.scene[asset_cfg.name]
|
||||
com_height = torch.nan_to_num(
|
||||
asset.data.root_link_pos_w[:, 2] - env.scene.terrain.env_origins[:, 2], nan=0.0
|
||||
)
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
return crouch_glide_reward_from_values(
|
||||
com_height, cmd[:, 0], cmd[:, 1],
|
||||
height_low, height_high, hold_lo, hold_hi, std,
|
||||
)
|
||||
|
||||
|
||||
def forward_speed_reward(
|
||||
env: ManagerBasedRlEnv,
|
||||
vel_ref: float = 0.2,
|
||||
asset_cfg: SceneEntityCfg = _DEFAULT_ASSET_CFG,
|
||||
) -> torch.Tensor:
|
||||
"""Récompense la vitesse avant du tronc (conserver l'élan / ne pas freiner).
|
||||
|
||||
Indépendante de la commande (la commande porte la phase, pas la vitesse).
|
||||
tanh(clamp(vx, 0)/vel_ref) → sature à ~1, ne récompense jamais reculer.
|
||||
"""
|
||||
asset: Entity = env.scene[asset_cfg.name]
|
||||
vx = asset.data.root_link_lin_vel_b[:, 0]
|
||||
return torch.tanh(torch.clamp(vx, min=0.0) / vel_ref)
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Vérifier le passage**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_crouch_glide.py -v`
|
||||
Expected: PASS (7 tests au total)
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/mdp.py tests/test_crouch_glide.py
|
||||
git commit -m "roller-crouch: rewards crouch-glide-height et forward-speed"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Task 3: L'environnement + enregistrement de la tâche
|
||||
|
||||
**Files:**
|
||||
- Create: `src/mjlab_microduck/tasks/microduck_roller_crouch_env_cfg.py`
|
||||
- Modify: `src/mjlab_microduck/tasks/__init__.py`
|
||||
- Test: `tests/test_roller_crouch_cfg.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `crouch_glide_height_by_phase`, `forward_speed_reward`, `ground_pick_return_pose` (Task 2 + existant), `GroundPickPhaseCommandCfg`, `GroundPickPhaseCommand`, `MICRODUCK_WALK_ROLLERS_ROBOT_CFG`.
|
||||
- Produces: `make_microduck_roller_crouch_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg`, `MicroduckRollerCrouchRlCfg`, tâche `Mjlab-RollerCrouch-Flat-MicroDuck`.
|
||||
|
||||
- [ ] **Step 1: Écrire le smoke test qui échoue**
|
||||
|
||||
Créer `tests/test_roller_crouch_cfg.py` :
|
||||
|
||||
```python
|
||||
from mjlab_microduck.tasks.microduck_roller_crouch_env_cfg import (
|
||||
make_microduck_roller_crouch_env_cfg,
|
||||
)
|
||||
from mjlab_microduck.tasks import mdp as microduck_mdp
|
||||
|
||||
|
||||
def test_cfg_uses_phase_command():
|
||||
cfg = make_microduck_roller_crouch_env_cfg()
|
||||
assert isinstance(
|
||||
cfg.commands["twist"], microduck_mdp.GroundPickPhaseCommandCfg
|
||||
)
|
||||
assert cfg.commands["twist"].period == 4.0
|
||||
|
||||
|
||||
def test_cfg_has_crouch_and_forward_rewards():
|
||||
cfg = make_microduck_roller_crouch_env_cfg()
|
||||
assert "crouch_glide_height" in cfg.rewards
|
||||
assert "forward_speed" in cfg.rewards
|
||||
# rewards de patinage actif retirées (pas de stride pendant le trick)
|
||||
for gone in ("braking", "skating_air_time", "single_support", "glide", "wheel_speed"):
|
||||
assert gone not in cfg.rewards
|
||||
|
||||
|
||||
def test_cfg_has_entry_velocity_event():
|
||||
cfg = make_microduck_roller_crouch_env_cfg()
|
||||
assert "entry_velocity" in cfg.events
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Vérifier l'échec**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_roller_crouch_cfg.py -v`
|
||||
Expected: FAIL — `ModuleNotFoundError: ...microduck_roller_crouch_env_cfg`
|
||||
|
||||
- [ ] **Step 3: Créer le fichier d'environnement**
|
||||
|
||||
Créer `src/mjlab_microduck/tasks/microduck_roller_crouch_env_cfg.py` :
|
||||
|
||||
```python
|
||||
"""Microduck roller crouch-glide task.
|
||||
|
||||
Geste one-shot déclenché au bouton A via le slot --ground-pick du runtime :
|
||||
le robot s'accroupit et glisse sur son élan (palier ~1 s), puis se relève et
|
||||
rend la main à la policy roller.
|
||||
|
||||
Hybride :
|
||||
- physique / robot roller ← microduck_velocity_rollers_env_cfg.py
|
||||
- machinerie phase one-shot ← microduck_ground_pick_env_cfg.py
|
||||
(commande GroundPickPhaseCommand : [cos(2πφ), sin(2πφ), 0], période 4 s)
|
||||
|
||||
Cible de hauteur « en trapèze » (haut→bas→palier 1 s→haut) via
|
||||
crouch_glide_height_by_phase. Obs 61D unifié → interchangeable au runtime.
|
||||
"""
|
||||
|
||||
import math
|
||||
from copy import deepcopy
|
||||
|
||||
ENABLE_SYMMETRY = False
|
||||
|
||||
# DR — repris du roller env
|
||||
ENABLE_COM_RANDOMIZATION = True
|
||||
ENABLE_HEAD_COM_RANDOMIZATION = True
|
||||
ENABLE_MASS_INERTIA_RANDOMIZATION = True
|
||||
ENABLE_JOINT_FRICTION_RANDOMIZATION = True
|
||||
ENABLE_ARMATURE_RANDOMIZATION = True
|
||||
ENABLE_WHEEL_FRICTION_RANDOMIZATION = True
|
||||
ENABLE_VELOCITY_PUSHES = True
|
||||
ENABLE_IMU_ORIENTATION_RANDOMIZATION = True
|
||||
ENABLE_ENCODER_BIAS = True
|
||||
|
||||
COM_RANDOMIZATION_RANGE = 0.003
|
||||
HEAD_COM_RANDOMIZATION_RANGE = 0.003
|
||||
MASS_INERTIA_RANDOMIZATION_RANGE = (0.95, 1.05)
|
||||
JOINT_FRICTION_RANDOMIZATION_RANGE = (0.9, 1.1)
|
||||
ARMATURE_RANDOMIZATION_RANGE = (0.9, 1.1)
|
||||
VELOCITY_PUSH_INTERVAL_S = (3.0, 6.0)
|
||||
VELOCITY_PUSH_RANGE = (-0.2, 0.2)
|
||||
IMU_ORIENTATION_RANDOMIZATION_ANGLE = 6.0
|
||||
ENCODER_BIAS_RANGE = (-0.015, 0.015)
|
||||
|
||||
# Geste : hauteurs cibles (m) et vitesse d'entrée (élan)
|
||||
CROUCH_HEIGHT_HIGH = 0.11 # tronc debout
|
||||
CROUCH_HEIGHT_LOW = 0.075 # tronc accroupi (à affiner en play)
|
||||
CROUCH_STD = 0.02
|
||||
ENTRY_VELOCITY_X = (0.2, 0.5) # m/s : le robot arrive en roulant
|
||||
|
||||
from mjlab.envs import ManagerBasedRlEnvCfg
|
||||
from mjlab.envs.mdp import dr
|
||||
from mjlab.envs.mdp.actions import JointPositionActionCfg
|
||||
from mjlab.managers import (
|
||||
CurriculumTermCfg,
|
||||
EventTermCfg,
|
||||
ObservationTermCfg,
|
||||
RewardTermCfg,
|
||||
TerminationTermCfg,
|
||||
)
|
||||
from mjlab.managers.scene_entity_config import SceneEntityCfg
|
||||
from mjlab.rl import RslRlOnPolicyRunnerCfg, RslRlModelCfg
|
||||
from mjlab.sensor import ContactMatch, ContactSensorCfg
|
||||
from mjlab.tasks.velocity import mdp
|
||||
from mjlab.tasks.velocity.mdp import UniformVelocityCommandCfg
|
||||
from mjlab.tasks.velocity.velocity_env_cfg import make_velocity_env_cfg
|
||||
from mjlab.utils.noise import UniformNoiseCfg as Unoise
|
||||
|
||||
from mjlab_microduck.robot.microduck_constants import MICRODUCK_WALK_ROLLERS_ROBOT_CFG
|
||||
from mjlab_microduck.tasks import mdp as microduck_mdp
|
||||
from mjlab_microduck.tasks.microduck_velocity_env_cfg import HEAD_BODY_NAMES
|
||||
from mjlab_microduck.tasks.symmetry import PpoWithSymmetryCfg, SYMMETRY_CFG
|
||||
|
||||
|
||||
def make_microduck_roller_crouch_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
|
||||
"""Env crouch-glide sur rollers, piloté par la phase du slot ground-pick."""
|
||||
|
||||
feet_ground_cfg = ContactSensorCfg(
|
||||
name="feet_ground_contact",
|
||||
primary=ContactMatch(
|
||||
mode="subtree",
|
||||
pattern=r"^(roller_blade|roller_blade_2)$",
|
||||
entity="robot",
|
||||
),
|
||||
secondary=ContactMatch(mode="body", pattern="terrain"),
|
||||
fields=("found", "force"),
|
||||
reduce="netforce",
|
||||
num_slots=1,
|
||||
track_air_time=True,
|
||||
)
|
||||
self_collision_cfg = ContactSensorCfg(
|
||||
name="self_collision",
|
||||
primary=ContactMatch(mode="subtree", pattern="trunk_base", entity="robot"),
|
||||
secondary=ContactMatch(mode="subtree", pattern="trunk_base", entity="robot"),
|
||||
fields=("found",),
|
||||
reduce="none",
|
||||
num_slots=1,
|
||||
)
|
||||
|
||||
cfg = make_velocity_env_cfg()
|
||||
cfg.scene.entities = {"robot": MICRODUCK_WALK_ROLLERS_ROBOT_CFG}
|
||||
cfg.scene.sensors = (feet_ground_cfg, self_collision_cfg)
|
||||
cfg.viewer.body_name = "trunk_base"
|
||||
|
||||
joint_pos_action = cfg.actions["joint_pos"]
|
||||
assert isinstance(joint_pos_action, JointPositionActionCfg)
|
||||
joint_pos_action.scale = 1.0
|
||||
|
||||
# === REWARDS ===
|
||||
keep = {"upright", "body_ang_vel", "angular_momentum", "action_rate_l2"}
|
||||
for name in list(cfg.rewards.keys()):
|
||||
if name not in keep:
|
||||
del cfg.rewards[name]
|
||||
|
||||
cfg.rewards["upright"].params["asset_cfg"].body_names = ("trunk_base",)
|
||||
cfg.rewards["upright"].weight = 2.0
|
||||
cfg.rewards["body_ang_vel"].params["asset_cfg"].body_names = ("trunk_base",)
|
||||
cfg.rewards["body_ang_vel"].weight = -0.05
|
||||
cfg.rewards["angular_momentum"].weight = -0.02
|
||||
cfg.rewards["action_rate_l2"].weight = -1.0
|
||||
|
||||
# Reward principale : cible de hauteur trapèze le long de la phase
|
||||
cfg.rewards["crouch_glide_height"] = RewardTermCfg(
|
||||
func=microduck_mdp.crouch_glide_height_by_phase,
|
||||
weight=4.0,
|
||||
params={
|
||||
"command_name": "twist",
|
||||
"height_low": CROUCH_HEIGHT_LOW,
|
||||
"height_high": CROUCH_HEIGHT_HIGH,
|
||||
"hold_lo": 0.375,
|
||||
"hold_hi": 0.625,
|
||||
"std": CROUCH_STD,
|
||||
},
|
||||
)
|
||||
# Conserver l'élan (ne pas freiner) — indépendant de la commande
|
||||
cfg.rewards["forward_speed"] = RewardTermCfg(
|
||||
func=microduck_mdp.forward_speed_reward,
|
||||
weight=2.0,
|
||||
params={"vel_ref": 0.2},
|
||||
)
|
||||
# Fin de phase : converger vers la pose roller debout pour rendre la main proprement
|
||||
_LEG_JOINTS = [0, 1, 2, 3, 4, 9, 10, 11, 12, 13]
|
||||
_NECK_JOINTS = [5, 6, 7, 8]
|
||||
cfg.rewards["return_pose_legs"] = RewardTermCfg(
|
||||
func=microduck_mdp.ground_pick_return_pose,
|
||||
weight=3.0,
|
||||
params={"std": 0.3, "command_name": "twist", "joint_indices": _LEG_JOINTS},
|
||||
)
|
||||
cfg.rewards["return_pose_neck"] = RewardTermCfg(
|
||||
func=microduck_mdp.ground_pick_return_pose,
|
||||
weight=3.0,
|
||||
params={"std": 0.15, "command_name": "twist", "joint_indices": _NECK_JOINTS},
|
||||
)
|
||||
# Stabilité de glisse
|
||||
cfg.rewards["feet_flat"] = RewardTermCfg(
|
||||
func=microduck_mdp.feet_flat_penalty,
|
||||
weight=-2.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", site_names=("left_foot", "right_foot")),
|
||||
"sensor_name": "feet_ground_contact",
|
||||
},
|
||||
)
|
||||
cfg.rewards["self_collisions"] = RewardTermCfg(
|
||||
func=mdp.self_collision_cost,
|
||||
weight=-1.0,
|
||||
params={"sensor_name": "self_collision"},
|
||||
)
|
||||
cfg.rewards["neck_action_rate_l2"] = RewardTermCfg(
|
||||
func=microduck_mdp.neck_action_rate_l2, weight=-0.5
|
||||
)
|
||||
cfg.rewards["joint_torques_l2"] = RewardTermCfg(
|
||||
func=microduck_mdp.joint_torques_l2, weight=-1e-3
|
||||
)
|
||||
|
||||
# === TERMINATIONS ===
|
||||
cfg.terminations["nan_state"] = TerminationTermCfg(
|
||||
func=microduck_mdp.robot_state_is_nan, time_out=False,
|
||||
)
|
||||
|
||||
# === EVENTS ===
|
||||
cfg.events["reset_action_history"] = EventTermCfg(
|
||||
func=microduck_mdp.reset_action_history, mode="reset",
|
||||
)
|
||||
del cfg.events["foot_friction"]
|
||||
|
||||
# Vitesse d'entrée : le robot démarre en roulant vers l'avant (élan à conserver)
|
||||
cfg.events["entry_velocity"] = EventTermCfg(
|
||||
func=mdp.push_by_setting_velocity,
|
||||
mode="reset",
|
||||
params={
|
||||
"velocity_range": {"x": ENTRY_VELOCITY_X, "y": (0.0, 0.0)},
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
},
|
||||
)
|
||||
|
||||
if ENABLE_VELOCITY_PUSHES:
|
||||
cfg.events["push_robot"] = EventTermCfg(
|
||||
func=mdp.push_by_setting_velocity,
|
||||
mode="interval",
|
||||
interval_range_s=VELOCITY_PUSH_INTERVAL_S,
|
||||
params={
|
||||
"velocity_range": {"x": VELOCITY_PUSH_RANGE, "y": VELOCITY_PUSH_RANGE},
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
},
|
||||
)
|
||||
|
||||
cfg.events["reset_base"].params["pose_range"]["z"] = (0.1335, 0.1435)
|
||||
|
||||
if ENABLE_WHEEL_FRICTION_RANDOMIZATION:
|
||||
cfg.events["randomize_wheel_friction"] = EventTermCfg(
|
||||
func=dr.dof_frictionloss,
|
||||
mode="reset",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=(r"^passive_.*",)),
|
||||
"operation": "abs",
|
||||
"ranges": (0.000, 0.000),
|
||||
},
|
||||
)
|
||||
if ENABLE_COM_RANDOMIZATION:
|
||||
cfg.events["randomize_com"] = EventTermCfg(
|
||||
func=dr.body_ipos, mode="reset",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=("trunk_base",)),
|
||||
"operation": "add",
|
||||
"ranges": (-COM_RANDOMIZATION_RANGE, COM_RANDOMIZATION_RANGE),
|
||||
},
|
||||
)
|
||||
if ENABLE_HEAD_COM_RANDOMIZATION:
|
||||
cfg.events["randomize_head_com"] = EventTermCfg(
|
||||
func=dr.body_ipos, mode="reset",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=HEAD_BODY_NAMES),
|
||||
"operation": "add",
|
||||
"ranges": (-HEAD_COM_RANDOMIZATION_RANGE, HEAD_COM_RANDOMIZATION_RANGE),
|
||||
},
|
||||
)
|
||||
if ENABLE_MASS_INERTIA_RANDOMIZATION:
|
||||
_mi_lo, _mi_hi = MASS_INERTIA_RANDOMIZATION_RANGE
|
||||
cfg.events["randomize_mass_inertia"] = EventTermCfg(
|
||||
func=dr.pseudo_inertia, mode="startup",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=("trunk_base",)),
|
||||
"alpha_range": (math.log(_mi_lo) / 2.0, math.log(_mi_hi) / 2.0),
|
||||
},
|
||||
)
|
||||
if ENABLE_JOINT_FRICTION_RANDOMIZATION:
|
||||
cfg.events["randomize_joint_friction"] = EventTermCfg(
|
||||
func=microduck_mdp.randomize_bam_friction, mode="reset",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
"scale_range": JOINT_FRICTION_RANDOMIZATION_RANGE,
|
||||
},
|
||||
)
|
||||
if ENABLE_ARMATURE_RANDOMIZATION:
|
||||
cfg.events["randomize_armature"] = EventTermCfg(
|
||||
func=dr.joint_armature, mode="reset",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=(r"^(?!passive_).*",)),
|
||||
"operation": "scale",
|
||||
"ranges": ARMATURE_RANDOMIZATION_RANGE,
|
||||
},
|
||||
)
|
||||
|
||||
# === OBSERVATIONS (unified 61D layout) ===
|
||||
del cfg.observations["actor"].terms["base_lin_vel"]
|
||||
del cfg.observations["critic"].terms["foot_height"]
|
||||
del cfg.observations["actor"].terms["height_scan"]
|
||||
del cfg.observations["critic"].terms["height_scan"]
|
||||
cfg.observations["critic"].terms["base_lin_vel"] = ObservationTermCfg(
|
||||
func=mdp.base_lin_vel, scale=1.0,
|
||||
)
|
||||
|
||||
gravity_term_name = "projected_gravity"
|
||||
cfg.observations["actor"].terms[gravity_term_name] = deepcopy(
|
||||
cfg.observations["actor"].terms[gravity_term_name]
|
||||
)
|
||||
cfg.observations["actor"].terms["base_ang_vel"] = deepcopy(
|
||||
cfg.observations["actor"].terms["base_ang_vel"]
|
||||
)
|
||||
cfg.observations["actor"].terms["base_ang_vel"].delay_min_lag = 0
|
||||
cfg.observations["actor"].terms["base_ang_vel"].delay_max_lag = 1
|
||||
cfg.observations["actor"].terms["base_ang_vel"].delay_update_period = 64
|
||||
cfg.observations["actor"].terms[gravity_term_name].delay_min_lag = 0
|
||||
cfg.observations["actor"].terms[gravity_term_name].delay_max_lag = 1
|
||||
cfg.observations["actor"].terms[gravity_term_name].delay_update_period = 64
|
||||
cfg.observations["actor"].terms["base_ang_vel"].noise = Unoise(n_min=-0.03, n_max=0.03)
|
||||
cfg.observations["actor"].terms[gravity_term_name].noise = Unoise(n_min=-0.01, n_max=0.01)
|
||||
cfg.observations["actor"].terms["joint_pos"].noise = Unoise(n_min=-0.001, n_max=0.001)
|
||||
cfg.observations["actor"].terms["joint_vel"].noise = Unoise(n_min=-0.25, n_max=0.25)
|
||||
|
||||
if ENABLE_IMU_ORIENTATION_RANDOMIZATION:
|
||||
av = cfg.observations["actor"].terms["base_ang_vel"]
|
||||
av.func = microduck_mdp.base_ang_vel_imu_misaligned
|
||||
av.params = {"max_angle_deg": IMU_ORIENTATION_RANDOMIZATION_ANGLE}
|
||||
g = cfg.observations["actor"].terms[gravity_term_name]
|
||||
g.func = microduck_mdp.projected_gravity_imu_misaligned
|
||||
g.params = {"max_angle_deg": IMU_ORIENTATION_RANDOMIZATION_ANGLE}
|
||||
|
||||
cfg.observations["actor"].terms["joint_vel"] = deepcopy(
|
||||
cfg.observations["actor"].terms["joint_vel"]
|
||||
)
|
||||
cfg.observations["actor"].terms["joint_vel"].delay_min_lag = 1
|
||||
cfg.observations["actor"].terms["joint_vel"].delay_max_lag = 1
|
||||
cfg.observations["actor"].terms["joint_vel"].delay_update_period = 0
|
||||
|
||||
passive_excluded = SceneEntityCfg("robot", joint_names=(r"^(?!passive_).*",))
|
||||
for grp in ("actor", "critic"):
|
||||
for term in ("joint_pos", "joint_vel"):
|
||||
cfg.observations[grp].terms[term] = deepcopy(cfg.observations[grp].terms[term])
|
||||
cfg.observations[grp].terms[term].params["asset_cfg"] = deepcopy(passive_excluded)
|
||||
|
||||
if ENABLE_ENCODER_BIAS:
|
||||
cfg.events["encoder_bias"].params["bias_range"] = ENCODER_BIAS_RANGE
|
||||
cfg.observations["actor"].terms["joint_pos"].params["biased"] = True
|
||||
cfg.observations["critic"].terms["joint_pos"].params["biased"] = False
|
||||
else:
|
||||
cfg.events.pop("encoder_bias", None)
|
||||
|
||||
wheel_cfg = SceneEntityCfg("robot", joint_names=(r"^passive_.*",))
|
||||
cfg.observations["critic"].terms["wheel_vel"] = ObservationTermCfg(
|
||||
func=mdp.joint_vel_rel, scale=1.0, params={"asset_cfg": wheel_cfg},
|
||||
)
|
||||
|
||||
for group in ("actor", "critic"):
|
||||
cfg.observations[group].terms["head_command"] = ObservationTermCfg(
|
||||
func=microduck_mdp.zero_command_padding, params={"dim": 4},
|
||||
)
|
||||
cfg.observations[group].terms["body_command"] = ObservationTermCfg(
|
||||
func=microduck_mdp.zero_command_padding, params={"dim": 6},
|
||||
)
|
||||
|
||||
# === COMMAND: phase (comme ground_pick) ===
|
||||
command: UniformVelocityCommandCfg = cfg.commands["twist"]
|
||||
command.rel_standing_envs = 0.0
|
||||
command.rel_heading_envs = 0.0
|
||||
cfg.commands["twist"] = microduck_mdp.GroundPickPhaseCommandCfg(
|
||||
**{**vars(command), "class_type": microduck_mdp.GroundPickPhaseCommand, "period": 4.0}
|
||||
)
|
||||
|
||||
cfg.scene.terrain.terrain_type = "plane"
|
||||
cfg.scene.terrain.terrain_generator = None
|
||||
|
||||
# === CURRICULUM ===
|
||||
del cfg.curriculum["terrain_levels"]
|
||||
del cfg.curriculum["command_vel"]
|
||||
cfg.curriculum["action_rate_weight"] = CurriculumTermCfg(
|
||||
func=microduck_mdp.reward_weight,
|
||||
params={
|
||||
"reward_name": "action_rate_l2",
|
||||
"weight_stages": [
|
||||
{"step": 0, "weight": -0.5},
|
||||
{"step": 250 * 24, "weight": -0.8},
|
||||
{"step": 500 * 24, "weight": -1.0},
|
||||
],
|
||||
},
|
||||
)
|
||||
if ENABLE_COM_RANDOMIZATION:
|
||||
cfg.curriculum["com_range"] = CurriculumTermCfg(
|
||||
func=microduck_mdp.com_range_curriculum,
|
||||
params={
|
||||
"event_name": "randomize_com",
|
||||
"range_stages": [
|
||||
{"step": 0, "range": 0.003},
|
||||
{"step": 500 * 24, "range": 0.005},
|
||||
{"step": 1000 * 24, "range": 0.01},
|
||||
],
|
||||
},
|
||||
)
|
||||
if ENABLE_HEAD_COM_RANDOMIZATION:
|
||||
cfg.curriculum["head_com_range"] = CurriculumTermCfg(
|
||||
func=microduck_mdp.com_range_curriculum,
|
||||
params={
|
||||
"event_name": "randomize_head_com",
|
||||
"range_stages": [
|
||||
{"step": 0, "range": 0.003},
|
||||
{"step": 500 * 24, "range": 0.005},
|
||||
{"step": 1000 * 24, "range": 0.01},
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
return cfg
|
||||
|
||||
|
||||
MicroduckRollerCrouchRlCfg = RslRlOnPolicyRunnerCfg(
|
||||
actor=RslRlModelCfg(
|
||||
hidden_dims=(512, 256, 128),
|
||||
activation="elu",
|
||||
obs_normalization=True,
|
||||
distribution_cfg={
|
||||
"class_name": "GaussianDistribution",
|
||||
"init_std": 1.0,
|
||||
"std_type": "scalar",
|
||||
},
|
||||
),
|
||||
critic=RslRlModelCfg(
|
||||
hidden_dims=(512, 256, 128),
|
||||
activation="elu",
|
||||
obs_normalization=True,
|
||||
),
|
||||
algorithm=PpoWithSymmetryCfg(
|
||||
value_loss_coef=1.0,
|
||||
use_clipped_value_loss=True,
|
||||
clip_param=0.2,
|
||||
entropy_coef=0.01,
|
||||
num_learning_epochs=5,
|
||||
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,
|
||||
symmetry_cfg=SYMMETRY_CFG if ENABLE_SYMMETRY else None,
|
||||
),
|
||||
wandb_project="mjlab_microduck",
|
||||
experiment_name="roller_crouch",
|
||||
run_name="roller_crouch",
|
||||
save_interval=250,
|
||||
num_steps_per_env=24,
|
||||
max_iterations=8_000,
|
||||
)
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Enregistrer la tâche**
|
||||
|
||||
Dans `src/mjlab_microduck/tasks/__init__.py`, ajouter l'import après le bloc rollers (après la ligne 54) :
|
||||
|
||||
```python
|
||||
from .microduck_roller_crouch_env_cfg import (
|
||||
make_microduck_roller_crouch_env_cfg,
|
||||
MicroduckRollerCrouchRlCfg,
|
||||
)
|
||||
```
|
||||
|
||||
et l'enregistrement après le bloc rollers (après la ligne 175) :
|
||||
|
||||
```python
|
||||
register_mjlab_task(
|
||||
task_id="Mjlab-RollerCrouch-Flat-MicroDuck",
|
||||
env_cfg=make_microduck_roller_crouch_env_cfg(),
|
||||
play_env_cfg=make_microduck_roller_crouch_env_cfg(play=True),
|
||||
rl_cfg=MicroduckRollerCrouchRlCfg,
|
||||
runner_cls=MicroduckOnPolicyRunner,
|
||||
)
|
||||
print("✓ RollerCrouch task registered: Mjlab-RollerCrouch-Flat-MicroDuck")
|
||||
```
|
||||
|
||||
- [ ] **Step 5: Vérifier le passage du smoke test**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_roller_crouch_cfg.py -v`
|
||||
Expected: PASS (3 tests). (Ce test construit l'env — il compile le spec MuJoCo, donc il est plus lent ; c'est normal.)
|
||||
|
||||
- [ ] **Step 6: Vérifier que la tâche est bien enregistrée**
|
||||
|
||||
Run: `uv run python -c "import mjlab_microduck.tasks"`
|
||||
Expected: la ligne `✓ RollerCrouch task registered: Mjlab-RollerCrouch-Flat-MicroDuck` s'affiche sans erreur.
|
||||
|
||||
- [ ] **Step 7: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/microduck_roller_crouch_env_cfg.py \
|
||||
src/mjlab_microduck/tasks/__init__.py tests/test_roller_crouch_cfg.py
|
||||
git commit -m "roller-crouch: env crouch-glide + enregistrement de la tache"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Task 4: Smoke run d'entraînement (vérification runtime)
|
||||
|
||||
**Files:** aucun (vérification observationnelle).
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: la tâche `Mjlab-RollerCrouch-Flat-MicroDuck` (Task 3).
|
||||
|
||||
- [ ] **Step 1: Lancer un entraînement très court**
|
||||
|
||||
Run:
|
||||
```bash
|
||||
uv run train Mjlab-RollerCrouch-Flat-MicroDuck \
|
||||
--env.scene.num-envs 64 --agent.max_iterations 5
|
||||
```
|
||||
Expected: l'entraînement démarre, log les rewards (dont `crouch_glide_height`, `forward_speed`), 5 itérations sans crash, un checkpoint est écrit.
|
||||
|
||||
- [ ] **Step 2: Vérifier l'absence d'erreur de forme d'obs**
|
||||
|
||||
Inspecter le log de démarrage : l'obs actor doit être **61D** (comme les autres policies de la famille). Si la dim diffère, le padding head/body ou l'exclusion des roues est mal câblé — corriger avant de continuer.
|
||||
|
||||
- [ ] **Step 3: Commit (si un fichier de conf a dû être ajusté)**
|
||||
|
||||
```bash
|
||||
git add -A && git commit -m "roller-crouch: ajustement post smoke-run"
|
||||
```
|
||||
(S'il n'y a rien à committer, sauter cette étape.)
|
||||
|
||||
---
|
||||
|
||||
## Task 5: Entraînement complet + vérification en play
|
||||
|
||||
**Files:** itérations possibles sur `microduck_roller_crouch_env_cfg.py` (poids de reward, `CROUCH_HEIGHT_LOW`).
|
||||
|
||||
- [ ] **Step 1: Lancer l'entraînement complet**
|
||||
|
||||
Run:
|
||||
```bash
|
||||
uv run train Mjlab-RollerCrouch-Flat-MicroDuck \
|
||||
--env.scene.num-envs 4096 --agent.max_iterations 8000
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Visualiser en play**
|
||||
|
||||
Run: `uv run scripts/play_latest.py` (ou l'entrée play du projet pour cette tâche).
|
||||
Observer le cycle : le robot **descend**, **glisse ~1 s** avec les roues qui continuent de tourner (il ne freine pas), puis **se relève** et la pose finale rejoint la pose roller debout. Il ne doit pas tomber.
|
||||
|
||||
- [ ] **Step 3: Itérer si nécessaire**
|
||||
|
||||
Réglages typiques (dans `microduck_roller_crouch_env_cfg.py`) :
|
||||
- Il ne descend pas assez → baisser `CROUCH_HEIGHT_LOW` (ex. 0.07) et/ou monter le poids de `crouch_glide_height`.
|
||||
- Il freine pendant l'accroupi → monter le poids de `forward_speed`.
|
||||
- Il tombe en position basse → monter `upright`, baisser la vitesse d'entrée `ENTRY_VELOCITY_X`, ou raccourcir le palier (rapprocher `hold_lo`/`hold_hi`).
|
||||
- La remontée est brutale → monter `return_pose_*` et/ou `action_rate_l2`.
|
||||
|
||||
Après chaque changement, relancer un entraînement et re-visualiser. Committer chaque réglage retenu :
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/microduck_roller_crouch_env_cfg.py
|
||||
git commit -m "roller-crouch: reglage <ce qui a change>"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Task 6: Export ONNX + déploiement sur le robot
|
||||
|
||||
**Files:** aucun (manuel / matériel).
|
||||
|
||||
- [ ] **Step 1: Exporter la policy en ONNX**
|
||||
|
||||
Run: `uv run scripts/export_latest.py` (le normaliseur d'obs est baké dans le graphe par `scripts/export.py`).
|
||||
Récupérer le fichier `.onnx`, le renommer `roller_crouch.onnx`, le copier sur le robot (ex. `~/microduck/policies/roller_crouch.onnx`).
|
||||
|
||||
- [ ] **Step 2: Lancer le runtime avec le slot ground-pick**
|
||||
|
||||
Sur le robot :
|
||||
```bash
|
||||
microduck_runtime --variant pre-alpha --new-cmd-obs --roller \
|
||||
--model output.onnx \
|
||||
--new-dxl-imu --kp 200 --action-scale 0.8 \
|
||||
--max-linear-vel 0.6 --max-linear-vel-backward 0.5 --max-angular-vel 0.0 \
|
||||
--ground-pick ~/microduck/policies/roller_crouch.onnx \
|
||||
--ground-pick-period 5.0 \
|
||||
--ground-pick-kp-ratio 1.0 \
|
||||
--ground-pick-action-scale 0.8
|
||||
```
|
||||
|
||||
**Paramètres critiques (parité sim2real) :**
|
||||
- `--ground-pick-kp-ratio 1.0` — le défaut 0.6 baisserait kp à 120 alors qu'on entraîne à 200.
|
||||
- `--ground-pick-action-scale 0.8` — doit matcher l'`action_scale` d'entraînement.
|
||||
- `--ground-pick-period 5.0` — doit matcher la période entraînée.
|
||||
|
||||
- [ ] **Step 3: Tester le geste**
|
||||
|
||||
Lancer le robot à petite vitesse en avant, appuyer sur **A**. Vérifier : il s'accroupit, glisse ~1 s, se relève, et la policy roller reprend la main proprement. Si instable, revenir à la Task 5 (itérer sur les poids / la hauteur / la vitesse d'entrée).
|
||||
|
||||
---
|
||||
|
||||
## Notes de vérification (self-review)
|
||||
|
||||
- **Couverture spec :** cible trapèze 1 s (Task 1) ; rewards crouch + anti-freinage + return-pose (Task 2/3) ; robot rollers + phase + obs 61D + DR (Task 3) ; vitesse d'entrée (Task 3, event `entry_velocity`) ; flags de déploiement dont le piège `kp-ratio` (Task 6). ✅
|
||||
- **Piège phase vs vitesse :** `wheel_speed_reward`/`braking`/`coasting_reward` du roller env utilisent `command[:,0]` comme *vitesse* — invalide ici où `command[:,0]=cos(2πφ)`. Elles sont donc **retirées** et remplacées par `forward_speed_reward` (indépendante de la commande). Testé par `test_cfg_has_crouch_and_forward_rewards`.
|
||||
- **Cohérence des noms :** `crouch_glide_height` (clé reward) vs `crouch_glide_height_by_phase` (fonction) — voulu : la clé est le nom du terme, la fonction est `func=`.
|
||||
```
|
||||
@@ -0,0 +1,671 @@
|
||||
# Mode pente `roller_slope` — Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** Entraîner une politique dédiée où microduck (rollers) démarre sur du plat avec une impulsion, roule sur une rampe descendante, et se laisse glisser jusqu'en bas en restant debout — sans aucun pilotage.
|
||||
|
||||
**Architecture:** Nouvelle tâche isolée clonée de `velocity_rollers` (même robot, même obs 61D → interchangeable au runtime). Terrain custom « plat + rampe » à angle interpolé par difficulté, curriculum de raideur maison, commande neutralisée, récompenses d'équilibre + posture debout nominale. Bouton `Y` de bascule dans `infer_policy.py`.
|
||||
|
||||
**Tech Stack:** Python, mjlab 1.3.x, MuJoCo (MjSpec terrains), rsl_rl (PPO), PyTorch, onnxruntime (déploiement), pytest.
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- **Observation unifiée 61D** : twist (3D) + head_command (4D) + body_command (6D) en zéro-padding. Ne jamais changer ce layout — la politique doit charger via `--new-cmd-obs`.
|
||||
- **Résolution des joints par NOM**, jamais par index (roues passives intercalées).
|
||||
- **Vitesse d'entrée via `reset_root_state_uniform` (velocity_range)**, JAMAIS via `push_by_setting_velocity` en mode reset (accumule sur l'état racine → free-joint diverge → NaN). Leçon `roller_crouch`.
|
||||
- **Angles en radians** dans le code physique ; les constantes de raideur sont exprimées en degrés (`RAMP_DEG_MIN=2.0`, `RAMP_DEG_MAX=20.0`) et converties.
|
||||
- **Commits simples**, style du dépôt (pas de `Co-authored-by`).
|
||||
- Tests dans `tests/`, lancés avec `uv run pytest`.
|
||||
|
||||
---
|
||||
|
||||
## File Structure
|
||||
|
||||
- **Create** `src/mjlab_microduck/tasks/slope_terrain.py` — `ramp_angle_by_difficulty()` + `FlatRampTerrainCfg` (géométrie du terrain plat+rampe). Responsabilité unique : le terrain.
|
||||
- **Modify** `src/mjlab_microduck/tasks/mdp.py` — ajouter `slope_move_masks()` (pur) + `terrain_levels_slope()` (curriculum de raideur).
|
||||
- **Create** `src/mjlab_microduck/tasks/microduck_roller_slope_env_cfg.py` — `make_microduck_roller_slope_env_cfg()` + `MicroduckRollerSlopeRlCfg`.
|
||||
- **Modify** `src/mjlab_microduck/tasks/__init__.py` — enregistrer la tâche.
|
||||
- **Modify** `scripts/infer_policy.py` — flag `--slope` + touche `Y`.
|
||||
- **Create** `tests/test_slope_terrain.py`, `tests/test_slope_curriculum.py`, `tests/test_roller_slope_cfg.py`.
|
||||
|
||||
---
|
||||
|
||||
## Task 1 : angle de rampe par difficulté (fonction pure)
|
||||
|
||||
**Files:**
|
||||
- Create: `src/mjlab_microduck/tasks/slope_terrain.py`
|
||||
- Test: `tests/test_slope_terrain.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Produces: `ramp_angle_by_difficulty(difficulty: float, deg_min: float = 2.0, deg_max: float = 20.0) -> float` (retourne des **radians**). Constantes module `RAMP_DEG_MIN = 2.0`, `RAMP_DEG_MAX = 20.0`.
|
||||
|
||||
- [ ] **Step 1: Écrire le test qui échoue**
|
||||
|
||||
```python
|
||||
# tests/test_slope_terrain.py
|
||||
import math
|
||||
from mjlab_microduck.tasks.slope_terrain import (
|
||||
ramp_angle_by_difficulty,
|
||||
RAMP_DEG_MIN,
|
||||
RAMP_DEG_MAX,
|
||||
)
|
||||
|
||||
|
||||
def test_ramp_angle_endpoints():
|
||||
assert math.isclose(ramp_angle_by_difficulty(0.0), math.radians(RAMP_DEG_MIN), abs_tol=1e-9)
|
||||
assert math.isclose(ramp_angle_by_difficulty(1.0), math.radians(RAMP_DEG_MAX), abs_tol=1e-9)
|
||||
|
||||
|
||||
def test_ramp_angle_midpoint():
|
||||
mid_deg = (RAMP_DEG_MIN + RAMP_DEG_MAX) / 2.0
|
||||
assert math.isclose(ramp_angle_by_difficulty(0.5), math.radians(mid_deg), abs_tol=1e-9)
|
||||
|
||||
|
||||
def test_ramp_angle_clamps_out_of_range():
|
||||
assert math.isclose(ramp_angle_by_difficulty(-1.0), math.radians(RAMP_DEG_MIN), abs_tol=1e-9)
|
||||
assert math.isclose(ramp_angle_by_difficulty(2.0), math.radians(RAMP_DEG_MAX), abs_tol=1e-9)
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Lancer le test — il doit échouer**
|
||||
|
||||
Run: `uv run pytest tests/test_slope_terrain.py -v`
|
||||
Expected: FAIL — `ModuleNotFoundError: No module named 'mjlab_microduck.tasks.slope_terrain'`
|
||||
|
||||
- [ ] **Step 3: Implémentation minimale**
|
||||
|
||||
```python
|
||||
# src/mjlab_microduck/tasks/slope_terrain.py
|
||||
"""Terrain custom « plat + rampe descendante » pour la tâche roller_slope.
|
||||
|
||||
Le robot spawne sur une zone plate, reçoit une impulsion vers +x, roule
|
||||
jusqu'à la rampe et se laisse glisser. L'angle de la rampe est interpolé par
|
||||
la difficulté (curriculum) sur [RAMP_DEG_MIN, RAMP_DEG_MAX] degrés.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
RAMP_DEG_MIN = 2.0
|
||||
RAMP_DEG_MAX = 20.0
|
||||
|
||||
|
||||
def ramp_angle_by_difficulty(
|
||||
difficulty: float, deg_min: float = RAMP_DEG_MIN, deg_max: float = RAMP_DEG_MAX
|
||||
) -> float:
|
||||
"""Angle de rampe (radians) interpolé linéairement par la difficulté [0,1]."""
|
||||
d = float(np.clip(difficulty, 0.0, 1.0))
|
||||
return math.radians(deg_min + d * (deg_max - deg_min))
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Lancer le test — il doit passer**
|
||||
|
||||
Run: `uv run pytest tests/test_slope_terrain.py -v`
|
||||
Expected: PASS (3 tests)
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/slope_terrain.py tests/test_slope_terrain.py
|
||||
git commit -m "roller-slope: angle de rampe par difficulte (fonction pure + tests)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Task 2 : terrain custom `FlatRampTerrainCfg`
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/slope_terrain.py`
|
||||
- Test: `tests/test_slope_terrain.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `ramp_angle_by_difficulty` (Task 1), `SubTerrainCfg`, `TerrainGeometry`, `TerrainOutput` de `mjlab.terrains.terrain_generator`.
|
||||
- Produces: `FlatRampTerrainCfg(SubTerrainCfg)` avec champs `flat_length: float = 2.0`, `ramp_length: float = 5.0`, `deg_min: float = 2.0`, `deg_max: float = 20.0`, `thickness: float = 0.5` ; méthode `function(difficulty, spec, rng) -> TerrainOutput`. L'origine de spawn est sur le plat.
|
||||
|
||||
**Notes géométrie (à retenir) :** la surface du plat est à `z=0` local. La rampe est un box tourné autour de `+y` par un quaternion `[cos(a/2), 0, sin(a/2), 0]` — une rotation `+a` autour de `+y` abaisse le bord `+x` (la rampe descend quand `x` augmente). L'assemblage exact plat/rampe (pas de marche, pas de trou) **doit être vérifié dans le viewer** (Step 6) car le `z` du centre de la rampe est sensible.
|
||||
|
||||
- [ ] **Step 1: Écrire le test qui échoue**
|
||||
|
||||
```python
|
||||
# tests/test_slope_terrain.py (ajouter)
|
||||
import mujoco
|
||||
import numpy as np
|
||||
from mjlab_microduck.tasks.slope_terrain import FlatRampTerrainCfg
|
||||
|
||||
|
||||
def _empty_terrain_spec():
|
||||
spec = mujoco.MjSpec()
|
||||
spec.worldbody.add_body(name="terrain")
|
||||
return spec
|
||||
|
||||
|
||||
def test_flat_ramp_builds_geoms_and_origin_on_flat():
|
||||
cfg = FlatRampTerrainCfg(flat_length=2.0, ramp_length=5.0)
|
||||
cfg.size = (8.0, 4.0) # posé normalement par le générateur
|
||||
spec = _empty_terrain_spec()
|
||||
out = cfg.function(difficulty=0.5, spec=spec, rng=np.random.default_rng(0))
|
||||
# deux géométries : plat + rampe
|
||||
assert len(out.geometries) == 2
|
||||
# origine sur le plat (x dans [0, flat_length], z ~ 0)
|
||||
assert 0.0 <= out.origin[0] <= 2.0
|
||||
assert abs(out.origin[2]) < 1e-6
|
||||
|
||||
|
||||
def test_flat_ramp_steeper_at_higher_difficulty():
|
||||
# à difficulté plus haute, le bout de rampe descend plus bas
|
||||
cfg = FlatRampTerrainCfg()
|
||||
cfg.size = (8.0, 4.0)
|
||||
easy = cfg.function(0.0, _empty_terrain_spec(), np.random.default_rng(0))
|
||||
hard = cfg.function(1.0, _empty_terrain_spec(), np.random.default_rng(0))
|
||||
# la rampe (2e géométrie) est plus basse (centre z plus négatif) en difficile
|
||||
assert hard.geometries[1].geom.pos[2] < easy.geometries[1].geom.pos[2]
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Lancer le test — il doit échouer**
|
||||
|
||||
Run: `uv run pytest tests/test_slope_terrain.py -k flat_ramp -v`
|
||||
Expected: FAIL — `ImportError: cannot import name 'FlatRampTerrainCfg'`
|
||||
|
||||
- [ ] **Step 3: Implémentation minimale**
|
||||
|
||||
```python
|
||||
# src/mjlab_microduck/tasks/slope_terrain.py (ajouter en tête)
|
||||
from dataclasses import dataclass
|
||||
|
||||
import mujoco
|
||||
|
||||
from mjlab.terrains.terrain_generator import (
|
||||
SubTerrainCfg,
|
||||
TerrainGeometry,
|
||||
TerrainOutput,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(kw_only=True)
|
||||
class FlatRampTerrainCfg(SubTerrainCfg):
|
||||
"""Zone plate de départ suivie d'une rampe descendante (angle par difficulté)."""
|
||||
|
||||
flat_length: float = 2.0 # longueur du plat de départ le long de +x (m)
|
||||
ramp_length: float = 5.0 # longueur horizontale de la rampe le long de +x (m)
|
||||
deg_min: float = RAMP_DEG_MIN
|
||||
deg_max: float = RAMP_DEG_MAX
|
||||
thickness: float = 0.5 # épaisseur des box (m)
|
||||
|
||||
def function(
|
||||
self, difficulty: float, spec: mujoco.MjSpec, rng
|
||||
) -> TerrainOutput:
|
||||
del rng # non utilisé
|
||||
body = spec.body("terrain")
|
||||
angle = ramp_angle_by_difficulty(difficulty, self.deg_min, self.deg_max)
|
||||
width = self.size[1]
|
||||
t = self.thickness
|
||||
|
||||
# Plat : box dont la surface supérieure est à z=0, x dans [0, flat_length].
|
||||
flat = body.add_geom(
|
||||
type=mujoco.mjtGeom.mjGEOM_BOX,
|
||||
size=(self.flat_length / 2.0, width / 2.0, t / 2.0),
|
||||
pos=(self.flat_length / 2.0, 0.0, -t / 2.0),
|
||||
)
|
||||
|
||||
# Rampe : box tourné de +angle autour de +y (le bord +x descend).
|
||||
# Longueur de surface = ramp_length / cos(angle).
|
||||
surf_len = self.ramp_length / math.cos(angle)
|
||||
ramp_cx = self.flat_length + self.ramp_length / 2.0
|
||||
# Centre z : mi-descente de la surface, moins la demi-épaisseur projetée.
|
||||
ramp_cz = -(self.ramp_length * math.tan(angle) / 2.0) - (t / 2.0) * math.cos(angle)
|
||||
half = angle / 2.0
|
||||
ramp = body.add_geom(
|
||||
type=mujoco.mjtGeom.mjGEOM_BOX,
|
||||
size=(surf_len / 2.0, width / 2.0, t / 2.0),
|
||||
pos=(ramp_cx, 0.0, ramp_cz),
|
||||
quat=(math.cos(half), 0.0, math.sin(half), 0.0),
|
||||
)
|
||||
|
||||
origin = np.array([self.flat_length * 0.4, 0.0, 0.0])
|
||||
return TerrainOutput(
|
||||
origin=origin,
|
||||
geometries=[
|
||||
TerrainGeometry(geom=flat, color=(0.5, 0.5, 0.5, 1.0)),
|
||||
TerrainGeometry(geom=ramp, color=(0.45, 0.55, 0.75, 1.0)),
|
||||
],
|
||||
)
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Lancer les tests — ils doivent passer**
|
||||
|
||||
Run: `uv run pytest tests/test_slope_terrain.py -v`
|
||||
Expected: PASS (5 tests)
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/slope_terrain.py tests/test_slope_terrain.py
|
||||
git commit -m "roller-slope: terrain custom plat+rampe (FlatRampTerrainCfg + tests)"
|
||||
```
|
||||
|
||||
- [ ] **Step 6: Vérification visuelle (checkpoint humain)**
|
||||
|
||||
La géométrie (surtout `ramp_cz` et le signe du quaternion) doit être confirmée à l'œil.
|
||||
Après la Task 4 (env assemblé), lancer le viewer play (voir Task 4 Step 6) et vérifier :
|
||||
la zone plate rejoint la rampe **sans marche ni trou**, et la rampe **descend** dans
|
||||
la direction `+x` (devant le robot). Si un décalage vertical apparaît, ajuster `ramp_cz` ;
|
||||
si la rampe monte au lieu de descendre, inverser le signe (`-half`) du quaternion.
|
||||
|
||||
---
|
||||
|
||||
## Task 3 : curriculum de raideur `terrain_levels_slope`
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/mdp.py`
|
||||
- Test: `tests/test_slope_curriculum.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Produces:
|
||||
- `slope_move_masks(distance: torch.Tensor, size_x: float) -> tuple[torch.Tensor, torch.Tensor]` — helper pur. `move_up = distance > size_x * 0.5` (a atteint le bas → rampe plus raide) ; `move_down = (distance < size_x * 0.2) & ~move_up` (chute/blocage tôt → rampe plus douce). Retourne `(move_up, move_down)` en `bool`.
|
||||
- `terrain_levels_slope(env, env_ids) -> torch.Tensor` — signature curriculum mjlab ; calcule la distance parcourue en `x` depuis l'origine, applique `slope_move_masks`, appelle `terrain.update_env_origins`, retourne le niveau moyen.
|
||||
|
||||
- [ ] **Step 1: Écrire le test qui échoue**
|
||||
|
||||
```python
|
||||
# tests/test_slope_curriculum.py
|
||||
import torch
|
||||
from mjlab_microduck.tasks.mdp import slope_move_masks
|
||||
|
||||
|
||||
def test_move_up_when_reached_bottom():
|
||||
# distance > size_x/2 → monte en difficulté
|
||||
dist = torch.tensor([5.0, 4.1])
|
||||
up, down = slope_move_masks(dist, size_x=8.0)
|
||||
assert bool(up[0]) and bool(up[1])
|
||||
assert not bool(down[0]) and not bool(down[1])
|
||||
|
||||
|
||||
def test_move_down_when_stuck_early():
|
||||
# distance < size_x*0.2 (=1.6) → descend en difficulté
|
||||
dist = torch.tensor([0.5, 1.0])
|
||||
up, down = slope_move_masks(dist, size_x=8.0)
|
||||
assert not bool(up[0]) and not bool(up[1])
|
||||
assert bool(down[0]) and bool(down[1])
|
||||
|
||||
|
||||
def test_stay_in_middle_band():
|
||||
# entre 1.6 et 4.0 → ni haut ni bas
|
||||
dist = torch.tensor([2.5])
|
||||
up, down = slope_move_masks(dist, size_x=8.0)
|
||||
assert not bool(up[0]) and not bool(down[0])
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Lancer le test — il doit échouer**
|
||||
|
||||
Run: `uv run pytest tests/test_slope_curriculum.py -v`
|
||||
Expected: FAIL — `ImportError: cannot import name 'slope_move_masks'`
|
||||
|
||||
- [ ] **Step 3: Implémentation minimale**
|
||||
|
||||
Ajouter dans `src/mjlab_microduck/tasks/mdp.py` (près des autres curriculums, ex. après `com_range_curriculum`). Vérifier en tête de fichier que `torch` est importé (il l'est).
|
||||
|
||||
```python
|
||||
def slope_move_masks(distance: "torch.Tensor", size_x: float):
|
||||
"""Masques de promotion/rétrogradation du curriculum de pente.
|
||||
|
||||
move_up : a parcouru plus de la moitié de la tuile → il a dévalé la rampe,
|
||||
on la rend plus raide.
|
||||
move_down : a à peine avancé (< 20% de la tuile) → chute/blocage précoce,
|
||||
on adoucit la rampe.
|
||||
"""
|
||||
move_up = distance > size_x * 0.5
|
||||
move_down = (distance < size_x * 0.2) & (~move_up)
|
||||
return move_up, move_down
|
||||
|
||||
|
||||
def terrain_levels_slope(env, env_ids):
|
||||
"""Curriculum de raideur pour roller_slope (pas de vitesse commandée).
|
||||
|
||||
Progression basée sur la distance en x parcourue depuis l'origine de spawn.
|
||||
"""
|
||||
asset = env.scene["robot"]
|
||||
terrain = env.scene.terrain
|
||||
assert terrain is not None
|
||||
terrain_generator = terrain.cfg.terrain_generator
|
||||
assert terrain_generator is not None
|
||||
|
||||
distance = (
|
||||
asset.data.root_link_pos_w[env_ids, 0] - env.scene.env_origins[env_ids, 0]
|
||||
)
|
||||
move_up, move_down = slope_move_masks(distance, terrain_generator.size[0])
|
||||
terrain.update_env_origins(env_ids, move_up, move_down)
|
||||
return torch.mean(terrain.terrain_levels.float())
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Lancer le test — il doit passer**
|
||||
|
||||
Run: `uv run pytest tests/test_slope_curriculum.py -v`
|
||||
Expected: PASS (3 tests)
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/mdp.py tests/test_slope_curriculum.py
|
||||
git commit -m "roller-slope: curriculum de raideur terrain_levels_slope (+ helper pur teste)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Task 4 : env cfg `roller_slope` + enregistrement
|
||||
|
||||
**Files:**
|
||||
- Create: `src/mjlab_microduck/tasks/microduck_roller_slope_env_cfg.py`
|
||||
- Modify: `src/mjlab_microduck/tasks/__init__.py`
|
||||
- Test: `tests/test_roller_slope_cfg.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `make_microduck_velocity_rollers_env_cfg` (base physique/DR/obs), `FlatRampTerrainCfg` (Task 2), `terrain_levels_slope` (Task 3), fonctions mdp existantes : `body_upright_gaussian`, `is_alive`, `pose_target_match`, `pose_l1_penalty`, `feet_flat_penalty`, `neck_action_rate_l2`, `joint_torques_l2`, `robot_state_is_nan`, `reset_action_history`, `zero_command_padding`.
|
||||
- Produces: `make_microduck_roller_slope_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg` et `MicroduckRollerSlopeRlCfg` (`RslRlOnPolicyRunnerCfg`, `experiment_name="roller_slope"`).
|
||||
|
||||
> Réutiliser les blocs DR/obs/reset du roller env : on **part** de `make_microduck_velocity_rollers_env_cfg()` et on ne modifie QUE terrain, commande, récompenses, terminaisons, curriculum. Ne pas réécrire la DR.
|
||||
|
||||
- [ ] **Step 1: Écrire le test qui échoue**
|
||||
|
||||
```python
|
||||
# tests/test_roller_slope_cfg.py
|
||||
from mjlab_microduck.tasks.microduck_roller_slope_env_cfg import (
|
||||
make_microduck_roller_slope_env_cfg,
|
||||
)
|
||||
from mjlab_microduck.tasks.slope_terrain import FlatRampTerrainCfg
|
||||
|
||||
|
||||
def test_terrain_is_flat_ramp_generator():
|
||||
cfg = make_microduck_roller_slope_env_cfg()
|
||||
assert cfg.scene.terrain.terrain_type == "generator"
|
||||
gen = cfg.scene.terrain.terrain_generator
|
||||
assert gen is not None and gen.curriculum is True
|
||||
assert any(isinstance(st, FlatRampTerrainCfg) for st in gen.sub_terrains.values())
|
||||
|
||||
|
||||
def test_command_is_neutralised():
|
||||
cfg = make_microduck_roller_slope_env_cfg()
|
||||
cmd = cfg.commands["twist"]
|
||||
assert cmd.rel_standing_envs == 1.0
|
||||
assert cmd.rel_heading_envs == 0.0
|
||||
|
||||
|
||||
def test_entry_velocity_set_on_reset_base():
|
||||
cfg = make_microduck_roller_slope_env_cfg()
|
||||
vr = cfg.events["reset_base"].params["velocity_range"]
|
||||
assert vr["x"][0] > 0.0 # impulsion vers l'avant
|
||||
|
||||
|
||||
def test_has_upright_and_pose_rewards():
|
||||
cfg = make_microduck_roller_slope_env_cfg()
|
||||
for name in ("upright", "alive", "standing_pose", "feet_flat"):
|
||||
assert name in cfg.rewards
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Lancer le test — il doit échouer**
|
||||
|
||||
Run: `uv run pytest tests/test_roller_slope_cfg.py -v`
|
||||
Expected: FAIL — `ModuleNotFoundError` (module env cfg absent)
|
||||
|
||||
- [ ] **Step 3: Implémentation**
|
||||
|
||||
```python
|
||||
# src/mjlab_microduck/tasks/microduck_roller_slope_env_cfg.py
|
||||
"""Microduck roller slope — descente passive équilibrée.
|
||||
|
||||
Le robot spawne sur du plat (impulsion vers l'avant), roule sur une rampe
|
||||
descendante et se laisse glisser en restant debout. Aucun pilotage : la
|
||||
commande twist est neutralisée (rel_standing_envs=1.0). Terrain custom
|
||||
plat+rampe (FlatRampTerrainCfg), curriculum de raideur (terrain_levels_slope).
|
||||
Obs 61D unifié → interchangeable au runtime (--new-cmd-obs).
|
||||
"""
|
||||
|
||||
from mjlab.envs import ManagerBasedRlEnvCfg
|
||||
from mjlab.managers import CurriculumTermCfg, EventTermCfg, RewardTermCfg, TerminationTermCfg
|
||||
from mjlab.managers.scene_entity_config import SceneEntityCfg
|
||||
from mjlab.rl import RslRlOnPolicyRunnerCfg, RslRlModelCfg
|
||||
from mjlab.terrains import TerrainEntityCfg
|
||||
from mjlab.terrains.terrain_generator import TerrainGeneratorCfg
|
||||
from mjlab.tasks.velocity import mdp
|
||||
from mjlab.envs import mdp as base_mdp
|
||||
|
||||
from mjlab_microduck.tasks import mdp as microduck_mdp
|
||||
from mjlab_microduck.tasks.slope_terrain import FlatRampTerrainCfg
|
||||
from mjlab_microduck.tasks.microduck_velocity_rollers_env_cfg import (
|
||||
make_microduck_velocity_rollers_env_cfg,
|
||||
)
|
||||
from mjlab_microduck.tasks.symmetry import PpoWithSymmetryCfg
|
||||
|
||||
ENTRY_VELOCITY_X = (0.2, 0.5) # impulsion vers l'avant au reset (m/s)
|
||||
|
||||
|
||||
def make_microduck_roller_slope_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
|
||||
cfg = make_microduck_velocity_rollers_env_cfg(play=play)
|
||||
|
||||
# === TERRAIN : plat + rampe, curriculum de raideur ===
|
||||
cfg.scene.terrain = TerrainEntityCfg(
|
||||
terrain_type="generator",
|
||||
terrain_generator=TerrainGeneratorCfg(
|
||||
size=(8.0, 4.0),
|
||||
curriculum=True,
|
||||
num_rows=10, # 10 niveaux de raideur
|
||||
num_cols=1,
|
||||
difficulty_range=(0.0, 1.0),
|
||||
sub_terrains={"flat_ramp": FlatRampTerrainCfg(flat_length=2.0, ramp_length=5.0)},
|
||||
),
|
||||
max_init_terrain_level=0, # démarrer sur la rampe la plus douce
|
||||
)
|
||||
|
||||
# === COMMANDE neutralisée (équilibre pur) ===
|
||||
command = cfg.commands["twist"]
|
||||
command.rel_standing_envs = 1.0
|
||||
command.rel_heading_envs = 0.0
|
||||
command.ranges.lin_vel_x = (0.0, 0.0)
|
||||
command.ranges.lin_vel_y = (0.0, 0.0)
|
||||
if getattr(command.ranges, "ang_vel_z", None) is not None:
|
||||
command.ranges.ang_vel_z = (0.0, 0.0)
|
||||
|
||||
# === RESET : impulsion vers l'avant sur le plat ===
|
||||
cfg.events["reset_base"].params["velocity_range"] = {"x": ENTRY_VELOCITY_X}
|
||||
|
||||
# === RÉCOMPENSES : équilibre + posture debout nominale ===
|
||||
keep = {"action_rate_l2"}
|
||||
for name in list(cfg.rewards.keys()):
|
||||
if name not in keep:
|
||||
del cfg.rewards[name]
|
||||
|
||||
cfg.rewards["upright"] = RewardTermCfg(
|
||||
func=microduck_mdp.body_upright_gaussian,
|
||||
weight=3.0,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", body_names=("trunk_base",)), "std": 0.2},
|
||||
)
|
||||
cfg.rewards["alive"] = RewardTermCfg(func=microduck_mdp.is_alive, weight=1.0)
|
||||
# posture debout nominale (cible fixe = default_joint_pos, aucun override)
|
||||
cfg.rewards["standing_pose"] = RewardTermCfg(
|
||||
func=microduck_mdp.pose_target_match, weight=3.0, params={"std": 0.4},
|
||||
)
|
||||
cfg.rewards["standing_pose_l1"] = RewardTermCfg(
|
||||
func=microduck_mdp.pose_l1_penalty, weight=1.0,
|
||||
)
|
||||
cfg.rewards["feet_flat"] = RewardTermCfg(
|
||||
func=microduck_mdp.feet_flat_penalty,
|
||||
weight=-2.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", site_names=("left_foot", "right_foot")),
|
||||
"sensor_name": "feet_ground_contact",
|
||||
},
|
||||
)
|
||||
cfg.rewards["neck_action_rate_l2"] = RewardTermCfg(
|
||||
func=microduck_mdp.neck_action_rate_l2, weight=-0.5,
|
||||
)
|
||||
cfg.rewards["joint_torques_l2"] = RewardTermCfg(
|
||||
func=microduck_mdp.joint_torques_l2, weight=-1e-3,
|
||||
)
|
||||
cfg.rewards["action_rate_l2"].weight = -1.0
|
||||
|
||||
# === TERMINATIONS : chute + bas atteint ===
|
||||
cfg.terminations["fell_over"] = TerminationTermCfg(
|
||||
func=base_mdp.bad_orientation,
|
||||
params={"limit_angle": 1.0, "asset_cfg": SceneEntityCfg("robot", body_names=("trunk_base",))},
|
||||
)
|
||||
cfg.terminations["out_of_bounds"] = TerminationTermCfg(func=mdp.out_of_terrain_bounds)
|
||||
cfg.terminations["nan_state"] = TerminationTermCfg(
|
||||
func=microduck_mdp.robot_state_is_nan, time_out=False,
|
||||
)
|
||||
|
||||
# === EVENTS ===
|
||||
cfg.events["reset_action_history"] = EventTermCfg(
|
||||
func=microduck_mdp.reset_action_history, mode="reset",
|
||||
)
|
||||
|
||||
# === CURRICULUM : raideur de la rampe ===
|
||||
for name in list(cfg.curriculum.keys()):
|
||||
del cfg.curriculum[name]
|
||||
cfg.curriculum["terrain_levels"] = CurriculumTermCfg(func=microduck_mdp.terrain_levels_slope)
|
||||
|
||||
return cfg
|
||||
|
||||
|
||||
MicroduckRollerSlopeRlCfg = RslRlOnPolicyRunnerCfg(
|
||||
actor=RslRlModelCfg(
|
||||
hidden_dims=(512, 256, 128),
|
||||
activation="elu",
|
||||
obs_normalization=True,
|
||||
distribution_cfg={"class_name": "GaussianDistribution", "init_std": 1.0, "std_type": "scalar"},
|
||||
),
|
||||
critic=RslRlModelCfg(hidden_dims=(512, 256, 128), activation="elu", obs_normalization=True),
|
||||
algorithm=PpoWithSymmetryCfg(
|
||||
value_loss_coef=1.0, use_clipped_value_loss=True, clip_param=0.2,
|
||||
entropy_coef=0.01, num_learning_epochs=5, 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, symmetry_cfg=None,
|
||||
),
|
||||
wandb_project="mjlab_microduck",
|
||||
experiment_name="roller_slope",
|
||||
run_name="roller_slope",
|
||||
save_interval=250,
|
||||
num_steps_per_env=24,
|
||||
max_iterations=8_000,
|
||||
)
|
||||
```
|
||||
|
||||
Puis enregistrer dans `src/mjlab_microduck/tasks/__init__.py`, en suivant EXACTEMENT le pattern d'enregistrement de `roller_crouch` déjà présent (import de `make_...` + `Microduck...RlCfg`, puis `register_mjlab_task(...)` avec un id du style `"Microduck-Roller-Slope"`). Copier le bloc `roller_crouch` et remplacer `crouch`→`slope`.
|
||||
|
||||
- [ ] **Step 4: Lancer les tests — ils doivent passer**
|
||||
|
||||
Run: `uv run pytest tests/test_roller_slope_cfg.py -v`
|
||||
Expected: PASS (4 tests)
|
||||
|
||||
- [ ] **Step 5: Vérifier l'enregistrement de la tâche + build complet**
|
||||
|
||||
Run:
|
||||
```bash
|
||||
uv run python -c "import gymnasium as gym; import mjlab_microduck.tasks; print([e for e in gym.registry if 'Slope' in e])"
|
||||
```
|
||||
Expected: la liste contient l'id `Microduck-Roller-Slope` (ou variante enregistrée).
|
||||
|
||||
- [ ] **Step 6: Vérification visuelle du terrain + descente (checkpoint humain — clôt Task 2 Step 6)**
|
||||
|
||||
Lancer un court entraînement puis le play (ou `scripts/play_latest.py` selon l'usage du dépôt) et observer :
|
||||
1. Plat + rampe assemblés sans marche/trou ; la rampe **descend** devant le robot.
|
||||
2. Le robot spawne sur le plat, part vers l'avant, atteint la rampe.
|
||||
Si la géométrie est fausse, corriger `slope_terrain.py` (voir Task 2 Step 6) et re-commit.
|
||||
|
||||
- [ ] **Step 7: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/microduck_roller_slope_env_cfg.py src/mjlab_microduck/tasks/__init__.py tests/test_roller_slope_cfg.py
|
||||
git commit -m "roller-slope: env descente passive (terrain plat+rampe, cmd nulle, rewards equilibre) + enregistrement"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Task 5 : déploiement — flag `--slope` + touche `Y`
|
||||
|
||||
**Files:**
|
||||
- Modify: `scripts/infer_policy.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: le `.onnx` exporté de la politique `roller_slope`.
|
||||
- Produces: argument CLI `--slope <path>` ; attribut `self.slope_session` + flag `self.slope_mode` ; méthode `toggle_slope_mode()` ; touche `GLFW_KEY_Y = 89` câblée.
|
||||
|
||||
> La politique pente tourne avec commande twist nulle (comme le mode standing). En slope mode, la bascule automatique walking/standing doit être neutralisée.
|
||||
|
||||
- [ ] **Step 1: Ajouter l'argument CLI et charger la session**
|
||||
|
||||
Dans `main()` (près des autres `add_argument`, ~ligne 471) :
|
||||
```python
|
||||
parser.add_argument("--slope", type=str, default=None, help="Path to slope policy ONNX file (press Y to toggle)")
|
||||
```
|
||||
Passer `slope_onnx_path=args.slope` au constructeur du contrôleur (ajouter le paramètre `slope_onnx_path=None` à `__init__`, ~ligne 51-57, et charger comme les autres) :
|
||||
```python
|
||||
self.slope_session = None
|
||||
self.slope_mode = False
|
||||
if slope_onnx_path:
|
||||
print(f"\nLoading slope policy from: {slope_onnx_path}")
|
||||
self.slope_session = ort.InferenceSession(slope_onnx_path)
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Ajouter `toggle_slope_mode` et neutraliser la bascule auto**
|
||||
|
||||
Après `toggle_body_pose_mode` (~ligne 285) :
|
||||
```python
|
||||
def toggle_slope_mode(self):
|
||||
"""Bascule vers/depuis la politique pente (descente passive)."""
|
||||
if self.slope_session is None:
|
||||
print("Slope unavailable: no --slope policy loaded")
|
||||
return
|
||||
self.slope_mode = not self.slope_mode
|
||||
if self.slope_mode:
|
||||
self.ort_session = self.slope_session
|
||||
self.current_policy = "slope"
|
||||
self.set_vel_cmd(0.0, 0.0, 0.0) # descente passive : commande nulle
|
||||
print("Slope mode: ON (descente passive)")
|
||||
else:
|
||||
self.ort_session = self.walking_session or self.standing_session
|
||||
self.current_policy = "walking" if self.walking_session else "standing"
|
||||
print("Slope mode: OFF")
|
||||
```
|
||||
Dans `_update_policy_session` (~ligne 250), ajouter le garde en tête (après le garde `ground_pick_mode`) :
|
||||
```python
|
||||
if self.slope_mode:
|
||||
return # Ne pas basculer pendant le mode pente
|
||||
```
|
||||
|
||||
- [ ] **Step 3: Câbler la touche `Y`**
|
||||
|
||||
Ajouter le code de touche près des autres (~ligne 680) :
|
||||
```python
|
||||
GLFW_KEY_Y = 89
|
||||
```
|
||||
Dans `key_callback`, ajouter une branche (ex. après la branche `GLFW_KEY_B`) :
|
||||
```python
|
||||
elif key == GLFW_KEY_Y:
|
||||
policy.toggle_slope_mode()
|
||||
```
|
||||
Ajouter la ligne d'aide clavier (près des `print` ~ligne 821) :
|
||||
```python
|
||||
print(" Y: toggle slope mode (requires --slope, descente passive)")
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Vérifier que le script se charge sans erreur**
|
||||
|
||||
Run: `uv run python scripts/infer_policy.py --help`
|
||||
Expected: l'aide s'affiche et liste `--slope`.
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add scripts/infer_policy.py
|
||||
git commit -m "roller-slope: deploiement --slope + touche Y (bascule mode pente)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Self-Review (fait par l'auteur du plan)
|
||||
|
||||
- **Couverture spec** : tâche dédiée (Task 4) ✓ ; terrain plat+rampe custom (Task 2) ✓ ; départ plat + impulsion (Task 4 reset velocity_range) ✓ ; commande nulle (Task 4) ✓ ; récompenses équilibre + pose debout + anti-écrasement (Task 4) ✓ ; terminaisons chute/bas/nan (Task 4) ✓ ; curriculum 0→20° (Task 1 angle + Task 3 promotion) ✓ ; obs 61D interchangeable (hérité du roller env, non modifié) ✓ ; bouton Y (Task 5) ✓.
|
||||
- **Placeholders** : aucun « TBD/TODO » ; les deux checkpoints humains (géométrie viewer) sont des vérifications explicites, pas des trous d'implémentation.
|
||||
- **Cohérence des types** : `ramp_angle_by_difficulty` (Task 1) réutilisé par `FlatRampTerrainCfg` (Task 2) ; `slope_move_masks` (Task 3) consommé par `terrain_levels_slope` (Task 3) ; noms de récompenses testés en Task 4 (`upright`, `alive`, `standing_pose`, `feet_flat`) alignés sur l'implémentation.
|
||||
- **Risques signalés** : géométrie de la rampe (`ramp_cz`, signe du quaternion) à confirmer au viewer ; noms exacts d'API mjlab (`terrain.terrain_levels`, `TerrainEntityCfg`, id d'enregistrement) à valider contre le pattern `roller_crouch` existant lors de l'implémentation.
|
||||
@@ -0,0 +1,618 @@
|
||||
# Ground-pick par suivi de pose — Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** Réécrire la tâche `Mjlab-GroundPick-Flat-MicroDuck` pour piloter le geste par un suivi de pose articulaire interpolé par la phase (STAND→DOWN→STAND) au lieu de l'objectif espace-tâche actuel (proximité bouche-sol + retour de pose).
|
||||
|
||||
**Architecture:** On ajoute trois fonctions mdp pures/quasi-pures (`phase_pose_blend`, `phase_pose_track`, `phase_pose_track_l1`) qui calculent une cible articulaire interpolée entre HOME (STAND) et un dict `DOWN_POSE` selon un profil de phase à 4 segments, résolue **par nom**. On ajoute un flag `randomize_phase` à la commande de phase existante. On réécrit ensuite le bloc rewards de `microduck_ground_pick_env_cfg.py` en gardant tout le reste (DR, obs 61D, curricula, RlCfg).
|
||||
|
||||
**Tech Stack:** Python, PyTorch, mjlab 1.3.0, MuJoCo, uv, pytest (via `uv run --with pytest`).
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- Résolution des joints **PAR NOM** (`asset.find_joints([name])[0][0]`), jamais par index en dur.
|
||||
- Obs 61D unifié **inchangé** (padding head/body zéro) → policy interchangeable dans le slot runtime.
|
||||
- Task id inchangé : `Mjlab-GroundPick-Flat-MicroDuck` (+ variante `-Rough-`).
|
||||
- Période de phase = **4.0 s** (défaut du slot `--ground-pick-period`).
|
||||
- Profil de phase (fractions) : `DESCENT_END=0.15`, `HOLD_END=0.50`, `RISE_END=0.65`.
|
||||
- `randomize_phase=False` pour la tâche ground_pick (parité déploiement bouton A à φ=0) ; défaut `True` de la cfg pour ne pas casser sit/stand.
|
||||
- STAND = HOME (`asset.data.default_joint_pos`, ne pas redéfinir). DOWN = dict `DOWN_POSE` par nom.
|
||||
- 14 joints actifs (mouth exclu). Robot `MICRODUCK_GROUND_PICK_ROBOT_CFG` (pas de roues → indices 0-4 jambe G, 5-8 cou/tête, 9-13 jambe D, mais on résout quand même par nom).
|
||||
- Fichiers mdp : imports déjà présents (`torch`, `Optional`, `Entity`, `SceneEntityCfg`, `ManagerBasedRlEnv`, `_DEFAULT_ASSET_CFG`).
|
||||
|
||||
---
|
||||
|
||||
### Task 1: Fonction `phase_pose_blend` (blend 4 segments, pure)
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/mdp.py` (ajout d'une fonction ; l'insérer juste avant `phase_pose_match` ~ligne 2041)
|
||||
- Test: `tests/test_ground_pick_pose.py` (create)
|
||||
|
||||
**Interfaces:**
|
||||
- Produces: `phase_pose_blend(phase: torch.Tensor, descent_end: float, hold_end: float, rise_end: float) -> torch.Tensor` — renvoie un blend ∈ [0,1] de même shape que `phase` (0 = STAND, 1 = DOWN).
|
||||
|
||||
- [ ] **Step 1: Write the failing test**
|
||||
|
||||
Créer `tests/test_ground_pick_pose.py` :
|
||||
|
||||
```python
|
||||
import torch
|
||||
from mjlab_microduck.tasks.mdp import phase_pose_blend
|
||||
|
||||
DESCENT_END, HOLD_END, RISE_END = 0.15, 0.50, 0.65
|
||||
|
||||
|
||||
def test_phase_pose_blend_keypoints():
|
||||
phase = torch.tensor([0.0, 0.075, 0.15, 0.30, 0.50, 0.575, 0.65, 0.80])
|
||||
b = phase_pose_blend(phase, DESCENT_END, HOLD_END, RISE_END)
|
||||
expected = torch.tensor([0.0, 0.5, 1.0, 1.0, 1.0, 0.5, 0.0, 0.0])
|
||||
assert torch.allclose(b, expected, atol=1e-6), b
|
||||
|
||||
|
||||
def test_phase_pose_blend_range():
|
||||
phase = torch.linspace(0.0, 1.0, 101)
|
||||
b = phase_pose_blend(phase, DESCENT_END, HOLD_END, RISE_END)
|
||||
assert b.min() >= 0.0 and b.max() <= 1.0
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_ground_pick_pose.py -q`
|
||||
Expected: FAIL — `ImportError: cannot import name 'phase_pose_blend'`
|
||||
|
||||
- [ ] **Step 3: Write minimal implementation**
|
||||
|
||||
Dans `src/mjlab_microduck/tasks/mdp.py`, juste avant `def phase_pose_match(` (~ligne 2041) :
|
||||
|
||||
```python
|
||||
def phase_pose_blend(
|
||||
phase: torch.Tensor,
|
||||
descent_end: float,
|
||||
hold_end: float,
|
||||
rise_end: float,
|
||||
) -> torch.Tensor:
|
||||
"""Blend 0..1 le long de la phase [0,1) — 0 = pose STAND, 1 = pose DOWN.
|
||||
|
||||
[0, descent_end) : 0 -> 1 (se baisser)
|
||||
[descent_end, hold_end): 1 (bas)
|
||||
[hold_end, rise_end) : 1 -> 0 (se lever)
|
||||
[rise_end, 1.0) : 0 (haut / repos)
|
||||
"""
|
||||
b = torch.zeros_like(phase)
|
||||
descend = phase < descent_end
|
||||
b = torch.where(descend, phase / descent_end, b)
|
||||
low = (phase >= descent_end) & (phase < hold_end)
|
||||
b = torch.where(low, torch.ones_like(phase), b)
|
||||
rise = (phase >= hold_end) & (phase < rise_end)
|
||||
b = torch.where(rise, 1.0 - (phase - hold_end) / (rise_end - hold_end), b)
|
||||
return b
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Run test to verify it passes**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_ground_pick_pose.py -q`
|
||||
Expected: PASS (2 passed)
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add tests/test_ground_pick_pose.py src/mjlab_microduck/tasks/mdp.py
|
||||
git commit -m "feat(mdp): phase_pose_blend — blend 4 segments STAND<->DOWN par la phase"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Task 2: Rewards `phase_pose_track` / `phase_pose_track_l1` (+ helper `_phase_pose_error`)
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/mdp.py` (ajout juste après `phase_pose_blend`)
|
||||
- Test: `tests/test_ground_pick_pose.py` (append)
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `phase_pose_blend` (Task 1).
|
||||
- Produces:
|
||||
- `_phase_pose_error(env, asset_cfg, command_name, target_pose: dict, descent_end, hold_end, rise_end, source_pose: dict | None = None) -> (cur: Tensor, target: Tensor)` — tenseurs (B, k) résolus par nom.
|
||||
- `phase_pose_track(env, command_name="twist", target_pose: dict | None = None, source_pose: dict | None = None, std=0.3, descent_end=0.15, hold_end=0.50, rise_end=0.65, asset_cfg=_DEFAULT_ASSET_CFG) -> Tensor` — gaussienne `exp(-((cur-target)/std)²).mean(-1)`.
|
||||
- `phase_pose_track_l1(env, command_name="twist", target_pose=None, source_pose=None, descent_end=0.15, hold_end=0.50, rise_end=0.65, asset_cfg=_DEFAULT_ASSET_CFG) -> Tensor` — `-(cur-target).abs().mean(-1)`.
|
||||
|
||||
- [ ] **Step 1: Write the failing test**
|
||||
|
||||
Ajouter à `tests/test_ground_pick_pose.py` un faux env léger + les assertions :
|
||||
|
||||
```python
|
||||
from mjlab_microduck.tasks.mdp import phase_pose_track, phase_pose_track_l1
|
||||
|
||||
|
||||
class _FakeData:
|
||||
def __init__(self, joint_pos, default_pos):
|
||||
self.joint_pos = joint_pos
|
||||
self.default_joint_pos = default_pos
|
||||
|
||||
|
||||
class _FakeAsset:
|
||||
def __init__(self, names, joint_pos, default_pos):
|
||||
self._ids = {n: i for i, n in enumerate(names)}
|
||||
self.data = _FakeData(joint_pos, default_pos)
|
||||
|
||||
def find_joints(self, query):
|
||||
# mjlab renvoie (ids, names) ; on ne gère que la requête [name]
|
||||
(name,) = query
|
||||
return ([self._ids[name]], [name])
|
||||
|
||||
|
||||
class _FakeCmdMgr:
|
||||
def __init__(self, cmd):
|
||||
self._cmd = cmd
|
||||
|
||||
def get_command(self, _name):
|
||||
return self._cmd
|
||||
|
||||
|
||||
class _FakeEnv:
|
||||
def __init__(self, names, joint_pos, default_pos, phase):
|
||||
import math
|
||||
self.device = "cpu"
|
||||
self.scene = {"robot": _FakeAsset(names, joint_pos, default_pos)}
|
||||
ang = 2 * math.pi * phase
|
||||
cmd = torch.tensor([[math.cos(ang), math.sin(ang), 0.0]])
|
||||
self.command_manager = _FakeCmdMgr(cmd)
|
||||
|
||||
|
||||
NAMES = ["j0", "j1"]
|
||||
DOWN = {"j0": 1.0, "j1": -1.0}
|
||||
# HOME (STAND source) = 0 pour les deux joints
|
||||
HOME = torch.tensor([[0.0, 0.0]])
|
||||
|
||||
|
||||
def _env(cur, phase):
|
||||
return _FakeEnv(NAMES, torch.tensor([cur]), HOME.clone(), phase)
|
||||
|
||||
|
||||
def test_phase_pose_track_perfect_at_down():
|
||||
# phase 0.30 -> blend 1 -> cible = DOWN ; cur == DOWN -> gaussienne 1, l1 0
|
||||
from mjlab.managers.scene_entity_config import SceneEntityCfg
|
||||
cfg = SceneEntityCfg("robot")
|
||||
env = _env([1.0, -1.0], phase=0.30)
|
||||
r = phase_pose_track(env, target_pose=DOWN, asset_cfg=cfg)
|
||||
assert torch.allclose(r, torch.tensor([1.0]), atol=1e-6), r
|
||||
env2 = _env([1.0, -1.0], phase=0.30)
|
||||
l1 = phase_pose_track_l1(env2, target_pose=DOWN, asset_cfg=cfg)
|
||||
assert torch.allclose(l1, torch.tensor([0.0]), atol=1e-6), l1
|
||||
|
||||
|
||||
def test_phase_pose_track_l1_at_home_when_down_target():
|
||||
# phase 0.30 -> cible DOWN=[1,-1] ; cur=HOME=[0,0] -> l1 = -mean(|1|,|1|) = -1
|
||||
from mjlab.managers.scene_entity_config import SceneEntityCfg
|
||||
cfg = SceneEntityCfg("robot")
|
||||
env = _env([0.0, 0.0], phase=0.30)
|
||||
l1 = phase_pose_track_l1(env, target_pose=DOWN, asset_cfg=cfg)
|
||||
assert torch.allclose(l1, torch.tensor([-1.0]), atol=1e-6), l1
|
||||
|
||||
|
||||
def test_phase_pose_track_returns_to_stand():
|
||||
# phase 0.80 -> blend 0 -> cible = HOME ; cur=HOME -> gaussienne 1
|
||||
from mjlab.managers.scene_entity_config import SceneEntityCfg
|
||||
cfg = SceneEntityCfg("robot")
|
||||
env = _env([0.0, 0.0], phase=0.80)
|
||||
r = phase_pose_track(env, target_pose=DOWN, asset_cfg=cfg)
|
||||
assert torch.allclose(r, torch.tensor([1.0]), atol=1e-6), r
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_ground_pick_pose.py -q`
|
||||
Expected: FAIL — `ImportError: cannot import name 'phase_pose_track'`
|
||||
|
||||
- [ ] **Step 3: Write minimal implementation**
|
||||
|
||||
Dans `src/mjlab_microduck/tasks/mdp.py`, juste après `phase_pose_blend` :
|
||||
|
||||
```python
|
||||
def _phase_pose_error(
|
||||
env: ManagerBasedRlEnv,
|
||||
asset_cfg: SceneEntityCfg,
|
||||
command_name: str,
|
||||
target_pose: dict,
|
||||
descent_end: float,
|
||||
hold_end: float,
|
||||
rise_end: float,
|
||||
source_pose: Optional[dict] = None,
|
||||
):
|
||||
"""(cur, target) pour la pose interpolée par la phase, résolue PAR NOM.
|
||||
|
||||
Cible = source + blend(phase)·(target_pose - source), source = STAND
|
||||
(`source_pose` si fourni, sinon le DEFAULT/HOME du modèle). blend ∈ [0,1]
|
||||
(0 = STAND, 1 = target_pose) via `phase_pose_blend`.
|
||||
"""
|
||||
asset: Entity = env.scene[asset_cfg.name]
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
phase = (torch.atan2(cmd[:, 1], cmd[:, 0]) / (2 * torch.pi)) % 1.0 # (B,)
|
||||
blend = phase_pose_blend(phase, descent_end, hold_end, rise_end) # (B,)
|
||||
|
||||
names = list(target_pose.keys())
|
||||
ids = [int(asset.find_joints([n])[0][0]) for n in names]
|
||||
default = asset.data.default_joint_pos[:, ids] # (B,k)
|
||||
|
||||
source = default.clone()
|
||||
if source_pose:
|
||||
for j, n in enumerate(names):
|
||||
if n in source_pose:
|
||||
source[:, j] = source_pose[n]
|
||||
target_vec = torch.tensor(
|
||||
[target_pose[n] for n in names], device=env.device, dtype=default.dtype
|
||||
).unsqueeze(0) # (1,k)
|
||||
|
||||
target = source + blend.unsqueeze(-1) * (target_vec - source) # (B,k)
|
||||
cur = asset.data.joint_pos[:, ids] # (B,k)
|
||||
return cur, target
|
||||
|
||||
|
||||
def phase_pose_track(
|
||||
env: ManagerBasedRlEnv,
|
||||
command_name: str = "twist",
|
||||
target_pose: Optional[dict] = None,
|
||||
source_pose: Optional[dict] = None,
|
||||
std: float = 0.3,
|
||||
descent_end: float = 0.15,
|
||||
hold_end: float = 0.50,
|
||||
rise_end: float = 0.65,
|
||||
asset_cfg: SceneEntityCfg = _DEFAULT_ASSET_CFG,
|
||||
) -> torch.Tensor:
|
||||
"""Gaussienne sur la pose articulaire vs cible interpolée STAND<->DOWN.
|
||||
|
||||
Reward directif : indique la config articulaire exacte à chaque phase. Se
|
||||
relever (cible → STAND) est récompensé exactement comme se baisser (cible →
|
||||
DOWN) — symétrique par construction. Résolution PAR NOM.
|
||||
"""
|
||||
cur, target = _phase_pose_error(
|
||||
env, asset_cfg, command_name, target_pose or {},
|
||||
descent_end, hold_end, rise_end, source_pose,
|
||||
)
|
||||
return torch.exp(-((cur - target) / std) ** 2).mean(dim=-1)
|
||||
|
||||
|
||||
def phase_pose_track_l1(
|
||||
env: ManagerBasedRlEnv,
|
||||
command_name: str = "twist",
|
||||
target_pose: Optional[dict] = None,
|
||||
source_pose: Optional[dict] = None,
|
||||
descent_end: float = 0.15,
|
||||
hold_end: float = 0.50,
|
||||
rise_end: float = 0.65,
|
||||
asset_cfg: SceneEntityCfg = _DEFAULT_ASSET_CFG,
|
||||
) -> torch.Tensor:
|
||||
"""Bootstrap L1 vers la cible interpolée (pénalité négative).
|
||||
|
||||
Gradient constant partout — donne une direction vers la cible même quand la
|
||||
gaussienne ci-dessus a saturé à ~0 loin de la cible.
|
||||
"""
|
||||
cur, target = _phase_pose_error(
|
||||
env, asset_cfg, command_name, target_pose or {},
|
||||
descent_end, hold_end, rise_end, source_pose,
|
||||
)
|
||||
return -(cur - target).abs().mean(dim=-1)
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Run test to verify it passes**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_ground_pick_pose.py -q`
|
||||
Expected: PASS (5 passed)
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add tests/test_ground_pick_pose.py src/mjlab_microduck/tasks/mdp.py
|
||||
git commit -m "feat(mdp): phase_pose_track/_l1 — suivi de pose interpolée par la phase (par nom)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Task 3: Flag `randomize_phase` sur `GroundPickPhaseCommandCfg`
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/mdp.py` (classe `GroundPickPhaseCommand` ~3611/3626, cfg ~3644)
|
||||
- Test: `tests/test_ground_pick_pose.py` (append)
|
||||
|
||||
**Interfaces:**
|
||||
- Produces: `GroundPickPhaseCommandCfg.randomize_phase: bool = True` ; `GroundPickPhaseCommand.reset()` met la phase à 0 quand `randomize_phase=False`, sinon `torch.rand`.
|
||||
|
||||
- [ ] **Step 1: Write the failing test**
|
||||
|
||||
Ajouter à `tests/test_ground_pick_pose.py` :
|
||||
|
||||
```python
|
||||
def test_ground_pick_cmd_cfg_has_randomize_phase_default_true():
|
||||
from mjlab_microduck.tasks.mdp import GroundPickPhaseCommandCfg
|
||||
from mjlab.tasks.velocity.mdp import UniformVelocityCommandCfg
|
||||
# construit une cfg minimale en copiant une cfg velocity par défaut
|
||||
base = UniformVelocityCommandCfg(
|
||||
asset_name="robot", resampling_time_range=(10.0, 10.0),
|
||||
ranges=UniformVelocityCommandCfg.Ranges(
|
||||
lin_vel_x=(0.0, 0.0), lin_vel_y=(0.0, 0.0), ang_vel_z=(0.0, 0.0),
|
||||
),
|
||||
)
|
||||
cfg = GroundPickPhaseCommandCfg(**{**vars(base)})
|
||||
assert cfg.randomize_phase is True
|
||||
assert cfg.period == 4.0
|
||||
```
|
||||
|
||||
Note : si la signature de `UniformVelocityCommandCfg.Ranges` diffère localement, adapter les champs — l'assertion clé est `cfg.randomize_phase is True`.
|
||||
|
||||
- [ ] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_ground_pick_pose.py::test_ground_pick_cmd_cfg_has_randomize_phase_default_true -q`
|
||||
Expected: FAIL — `AttributeError: 'GroundPickPhaseCommandCfg' object has no attribute 'randomize_phase'`
|
||||
|
||||
- [ ] **Step 3: Write minimal implementation**
|
||||
|
||||
Dans `src/mjlab_microduck/tasks/mdp.py`, classe `GroundPickPhaseCommand`, modifier `__init__` et `reset` :
|
||||
|
||||
Remplacer (dans `__init__`, ~ligne 3614) :
|
||||
```python
|
||||
self._period = float(getattr(cfg, "period", self.PERIOD))
|
||||
```
|
||||
par :
|
||||
```python
|
||||
self._period = float(getattr(cfg, "period", self.PERIOD))
|
||||
self._randomize_phase = bool(getattr(cfg, "randomize_phase", True))
|
||||
```
|
||||
|
||||
Remplacer la méthode `reset` (~ligne 3626) :
|
||||
```python
|
||||
def reset(self, env_ids: torch.Tensor | None) -> dict:
|
||||
if env_ids is not None and len(env_ids) > 0:
|
||||
self._gp_phase[env_ids] = torch.rand(len(env_ids), device=self.device)
|
||||
return {}
|
||||
```
|
||||
par :
|
||||
```python
|
||||
def reset(self, env_ids: torch.Tensor | None) -> dict:
|
||||
if env_ids is not None and len(env_ids) > 0:
|
||||
if self._randomize_phase:
|
||||
self._gp_phase[env_ids] = torch.rand(len(env_ids), device=self.device)
|
||||
else:
|
||||
self._gp_phase[env_ids] = 0.0
|
||||
return {}
|
||||
```
|
||||
|
||||
Dans la cfg `GroundPickPhaseCommandCfg` (~ligne 3644), ajouter le champ après `period` :
|
||||
```python
|
||||
@_dataclass(kw_only=True)
|
||||
class GroundPickPhaseCommandCfg(UniformVelocityCommandCfg):
|
||||
class_type: type = GroundPickPhaseCommand
|
||||
period: float = 4.0 # cycle length in seconds; sitstand uses 8.0
|
||||
randomize_phase: bool = True # False = chaque épisode démarre à φ=0 (parité slot bouton A)
|
||||
|
||||
def build(self, env: ManagerBasedRlEnv) -> "GroundPickPhaseCommand":
|
||||
return GroundPickPhaseCommand(self, env)
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Run test to verify it passes**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_ground_pick_pose.py::test_ground_pick_cmd_cfg_has_randomize_phase_default_true -q`
|
||||
Expected: PASS. Si la construction de `UniformVelocityCommandCfg` échoue pour une raison d'API locale, ajuster les champs du `base` dans le test (l'implémentation, elle, est correcte).
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add tests/test_ground_pick_pose.py src/mjlab_microduck/tasks/mdp.py
|
||||
git commit -m "feat(mdp): flag randomize_phase sur GroundPickPhaseCommandCfg (défaut True)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Task 4: Réécriture du bloc rewards + poses dans l'env cfg
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/microduck_ground_pick_env_cfg.py`
|
||||
- Test: `tests/test_ground_pick_cfg.py` (create)
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `phase_pose_track`, `phase_pose_track_l1` (Task 2) ; `randomize_phase` (Task 3).
|
||||
- Produces: `make_microduck_ground_pick_env_cfg(play=False, rough=False)` renvoie une cfg dont : commande `GroundPickPhaseCommand` avec `randomize_phase=False`, `period=4.0` ; rewards contiennent `phase_pose_track` (6.0) et `phase_pose_track_l1` (2.0), `mouth_ground_proximity` (1.0) ; ne contiennent plus `mouth_perpendicular_to_ground`, `ground_pick_return_pose_legs`, `ground_pick_return_pose_neck`.
|
||||
|
||||
- [ ] **Step 1: Write the failing test**
|
||||
|
||||
Créer `tests/test_ground_pick_cfg.py` :
|
||||
|
||||
```python
|
||||
from mjlab_microduck.tasks.microduck_ground_pick_env_cfg import (
|
||||
make_microduck_ground_pick_env_cfg,
|
||||
)
|
||||
from mjlab_microduck.tasks.mdp import GroundPickPhaseCommand
|
||||
|
||||
|
||||
def test_ground_pick_cfg_builds_with_pose_rewards():
|
||||
cfg = make_microduck_ground_pick_env_cfg()
|
||||
rewards = cfg.rewards
|
||||
assert "phase_pose_track" in rewards
|
||||
assert "phase_pose_track_l1" in rewards
|
||||
assert rewards["phase_pose_track"].weight == 6.0
|
||||
assert rewards["phase_pose_track_l1"].weight == 2.0
|
||||
# filet bouche-sol conservé mais allégé
|
||||
assert "mouth_ground_proximity" in rewards
|
||||
assert rewards["mouth_ground_proximity"].weight == 1.0
|
||||
# anciennes mécaniques retirées
|
||||
assert "mouth_perpendicular_to_ground" not in rewards
|
||||
assert "ground_pick_return_pose_legs" not in rewards
|
||||
assert "ground_pick_return_pose_neck" not in rewards
|
||||
|
||||
|
||||
def test_ground_pick_cfg_command_is_phase_no_randomize():
|
||||
cfg = make_microduck_ground_pick_env_cfg()
|
||||
cmd = cfg.commands["twist"]
|
||||
assert cmd.class_type is GroundPickPhaseCommand
|
||||
assert cmd.period == 4.0
|
||||
assert cmd.randomize_phase is False
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_ground_pick_cfg.py -q`
|
||||
Expected: FAIL — `assert 'phase_pose_track' in rewards` (KeyError/False).
|
||||
|
||||
- [ ] **Step 3: Write minimal implementation**
|
||||
|
||||
Dans `src/mjlab_microduck/tasks/microduck_ground_pick_env_cfg.py` :
|
||||
|
||||
(a) Ajouter les constantes de poses/phase juste avant `def make_microduck_ground_pick_env_cfg(` :
|
||||
|
||||
```python
|
||||
# ── Poses cibles du geste (rad, par NOM) ──────────────────────────────────────
|
||||
# STAND = HOME (default_joint_pos du modèle) — ne pas redéfinir ici : source du
|
||||
# blend. DOWN = pli avant profond (bouche vers le sol), valeurs initiales tirées
|
||||
# du keyframe FOLD de scene_walk.xml. ⚠️ REMPLAÇABLE par une lecture read_pose.py
|
||||
# du vrai robot posé bouche-au-sol quand disponible.
|
||||
DOWN_POSE = {
|
||||
"left_hip_yaw": 0.0, "left_hip_roll": 0.0, "left_hip_pitch": 1.57,
|
||||
"left_knee": 1.57, "left_ankle": 0.0,
|
||||
"neck_pitch": 1.0, "head_pitch": 1.0, "head_yaw": 0.0, "head_roll": 0.0,
|
||||
"right_hip_yaw": 0.0, "right_hip_roll": 0.0, "right_hip_pitch": -1.57,
|
||||
"right_knee": -1.57, "right_ankle": 0.0,
|
||||
}
|
||||
|
||||
# Timing du cycle (fractions de phase), période 4 s :
|
||||
# descente [0, DESCENT_END) ~0.6s / bas [DESCENT_END, HOLD_END) ~1.4s /
|
||||
# remontée [HOLD_END, RISE_END) ~0.6s / repos [RISE_END, 1) ~1.4s
|
||||
GP_PERIOD = 4.0
|
||||
DESCENT_END = 0.15
|
||||
HOLD_END = 0.50
|
||||
RISE_END = 0.65
|
||||
POSE_STD = 0.3
|
||||
```
|
||||
|
||||
(b) Dans la boucle de suppression des rewards (~ligne 145-155), remplacer le contenu du geste. **Retirer** les deux blocs `mouth_perpendicular_to_ground` (~176-183) et les deux `ground_pick_return_pose_*` (~189-212), et **retuner** `mouth_ground_proximity` à `weight=1.0` (~163-172, changer `weight=2.0` → `weight=1.0`).
|
||||
|
||||
Concrètement :
|
||||
- Éditer le bloc `cfg.rewards["mouth_ground_proximity"]` : `weight=2.0` → `weight=1.0`.
|
||||
- Supprimer entièrement le bloc `cfg.rewards["mouth_perpendicular_to_ground"] = RewardTermCfg(...)`.
|
||||
- Supprimer les blocs `_LEG_JOINTS = [...]` / `cfg.rewards["ground_pick_return_pose_legs"]` et `_NECK_JOINTS = [...]` / `cfg.rewards["ground_pick_return_pose_neck"]`.
|
||||
- Retirer `"pose"` de la liste de suppression de rewards si présent (inchangé) — mais **retirer** aussi la ligne de commentaire `# replaced by phase-conditioned ground_pick_return_pose` devenue obsolète (optionnel).
|
||||
|
||||
(c) Ajouter les deux nouveaux rewards de suivi de pose (à la place des blocs retirés, dans la section « main ground pick objectives ») :
|
||||
|
||||
```python
|
||||
# Suivi de pose interpolée par la phase (STAND<->DOWN<->STAND). Directif et
|
||||
# symétrique : le retour debout est récompensé exactement comme la descente.
|
||||
cfg.rewards["phase_pose_track"] = RewardTermCfg(
|
||||
func=microduck_mdp.phase_pose_track,
|
||||
weight=6.0,
|
||||
params={
|
||||
"command_name": "twist",
|
||||
"target_pose": DOWN_POSE,
|
||||
"std": POSE_STD,
|
||||
"descent_end": DESCENT_END,
|
||||
"hold_end": HOLD_END,
|
||||
"rise_end": RISE_END,
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
},
|
||||
)
|
||||
cfg.rewards["phase_pose_track_l1"] = RewardTermCfg(
|
||||
func=microduck_mdp.phase_pose_track_l1,
|
||||
weight=2.0,
|
||||
params={
|
||||
"command_name": "twist",
|
||||
"target_pose": DOWN_POSE,
|
||||
"descent_end": DESCENT_END,
|
||||
"hold_end": HOLD_END,
|
||||
"rise_end": RISE_END,
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
},
|
||||
)
|
||||
```
|
||||
|
||||
(d) Dans le bloc « Command » (~ligne 368), passer la période et désactiver la randomisation de phase :
|
||||
|
||||
Remplacer :
|
||||
```python
|
||||
cfg.commands["twist"] = microduck_mdp.GroundPickPhaseCommandCfg(
|
||||
**{**vars(command), "class_type": microduck_mdp.GroundPickPhaseCommand}
|
||||
)
|
||||
```
|
||||
par :
|
||||
```python
|
||||
cfg.commands["twist"] = microduck_mdp.GroundPickPhaseCommandCfg(
|
||||
**{
|
||||
**vars(command),
|
||||
"class_type": microduck_mdp.GroundPickPhaseCommand,
|
||||
"period": GP_PERIOD,
|
||||
"randomize_phase": False,
|
||||
}
|
||||
)
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Run test to verify it passes**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_ground_pick_cfg.py -q`
|
||||
Expected: PASS (2 passed).
|
||||
|
||||
Puis vérifier que l'ensemble de la suite passe :
|
||||
Run: `uv run --with pytest pytest tests/ -q`
|
||||
Expected: PASS (tous).
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add tests/test_ground_pick_cfg.py src/mjlab_microduck/tasks/microduck_ground_pick_env_cfg.py
|
||||
git commit -m "feat(ground_pick): suivi de pose interpolée par la phase (STAND->DOWN->STAND)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Task 5: Vérification de bout en bout (construction runtime de la tâche)
|
||||
|
||||
**Files:**
|
||||
- Test: `tests/test_ground_pick_cfg.py` (append)
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: tout ce qui précède.
|
||||
|
||||
- [ ] **Step 1: Write the failing/uncovered test**
|
||||
|
||||
Ajouter à `tests/test_ground_pick_cfg.py` :
|
||||
|
||||
```python
|
||||
def test_ground_pick_rough_variant_builds():
|
||||
cfg = make_microduck_ground_pick_env_cfg(rough=True)
|
||||
assert "phase_pose_track" in cfg.rewards
|
||||
|
||||
|
||||
def test_ground_pick_play_variant_builds():
|
||||
cfg = make_microduck_ground_pick_env_cfg(play=True)
|
||||
assert cfg.commands["twist"].randomize_phase is False
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run to verify**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_ground_pick_cfg.py -q`
|
||||
Expected: PASS.
|
||||
|
||||
- [ ] **Step 3: Vérifier l'enregistrement de la tâche (import du package)**
|
||||
|
||||
Run: `uv run python -c "import mjlab_microduck.tasks; print('ok')"`
|
||||
Expected: affiche les lignes `✓ ... registered` dont `GroundPick`, puis `ok`, sans exception.
|
||||
|
||||
- [ ] **Step 4: Commit**
|
||||
|
||||
```bash
|
||||
git add tests/test_ground_pick_cfg.py
|
||||
git commit -m "test(ground_pick): variantes rough/play + import du package"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Self-Review
|
||||
|
||||
**1. Spec coverage :**
|
||||
- §1 objectif directif par pose → Tasks 1,2,4. ✓
|
||||
- §2 poses (STAND=HOME source, DOWN=FOLD par nom) → Task 4 (a), Task 2 (`source_pose=None`→default). ✓
|
||||
- §3 profil 4 segments période 4 s + `randomize_phase=False` → Task 1, Task 3, Task 4 (a,d). ✓
|
||||
- §4 fonctions mdp `phase_pose_blend/track/_l1` par nom → Tasks 1,2. ✓
|
||||
- §5 rewards (ajouts + retraits + retune mouth 1.0) → Task 4 (b,c), test Task 4. ✓
|
||||
- §6 déploiement (période 4, kp-ratio 1.0) → documenté dans spec ; period=4 vérifié en test Task 4. ✓
|
||||
- §7 tests (fonctions pures + construction env) → Tasks 1,2,4,5. ✓
|
||||
- §9 doublon `pose_target_match` hors scope → non modifié (conforme). ✓
|
||||
|
||||
**2. Placeholder scan :** aucun TODO/TBD ; tout le code est fourni. ✓
|
||||
|
||||
**3. Type consistency :** `phase_pose_track(target_pose=..., std=..., asset_cfg=...)` et `phase_pose_track_l1(target_pose=..., asset_cfg=...)` identiques entre Task 2 (def), Task 4 (appel) et tests. `randomize_phase` cohérent entre Task 3 (def) et Task 4/tests (usage). `GroundPickPhaseCommand`/`GroundPickPhaseCommandCfg` noms inchangés. ✓
|
||||
@@ -0,0 +1,774 @@
|
||||
# Tâche shoot par suivi de poses — Plan d'implémentation
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** Ajouter une tâche RL `Mjlab-Shoot-Flat-MicroDuck` qui apprend un geste de shoot one-shot (jambe droite) par suivi d'une trajectoire de poses à 4 keyframes (STAND → PIED_ARRIÈRE → PIED_AVANT → STAND) interpolée par la phase.
|
||||
|
||||
**Architecture:** Même moule que la tâche `ground_pick` de cette branche. Une commande de phase (`GroundPickPhaseCommand`, `[cos,sin,0]`) pilote une cible articulaire interpolée entre 3 poses ; des rewards gaussien + L1 récompensent le suivi ; obs 61D unifiée pour déploiement dans un slot bouton du runtime. Aucune balle simulée.
|
||||
|
||||
**Tech Stack:** Python, PyTorch, mjlab 1.3.0, MuJoCo, uv, pytest.
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- Obs **61D unifiée** identique aux autres policies microduck (`[gyro(3), projected_gravity(3), joint_pos(14), joint_vel(14), last_action(14), command(13)]`, head+body command zero-paddés). Ne pas casser cette forme.
|
||||
- Résolution des joints **PAR NOM** (`asset.find_joints([name])`), jamais par index en dur.
|
||||
- **14 joints** actifs (mouth exclu). Robot `MICRODUCK_WALK_ROBOT_CFG`.
|
||||
- Ne pas modifier le runtime Rust ni la classe de commande de façon cassante : le flag `randomize_phase` ajouté DOIT défaut à `True` pour préserver `ground_pick`.
|
||||
- Jambe **droite** frappe, **gauche** en appui.
|
||||
- Tests : `uv run --with pytest pytest tests/ -q`.
|
||||
- Convention commits : messages en français, style `feat:`/`docs:`/`test:`.
|
||||
|
||||
---
|
||||
|
||||
## File Structure
|
||||
|
||||
- `src/mjlab_microduck/tasks/mdp.py` — MODIFIER : ajouter `kick_pose_target` (pure), `_kick_pose_error`, `kick_pose_track`, `kick_pose_track_l1` ; ajouter le flag `randomize_phase` à `GroundPickPhaseCommand` / `GroundPickPhaseCommandCfg`.
|
||||
- `src/mjlab_microduck/tasks/microduck_shoot_env_cfg.py` — CRÉER : `make_microduck_shoot_env_cfg`, `MicroduckShootRlCfg`, `STAND_POSE`/`KICK_BACK_POSE`/`KICK_FWD_POSE`, timings.
|
||||
- `src/mjlab_microduck/tasks/__init__.py` — MODIFIER : import + `register_mjlab_task("Mjlab-Shoot-Flat-MicroDuck", …)`.
|
||||
- `tests/test_shoot.py` — CRÉER : tests des fonctions pures (`kick_pose_target`) + rewards via stub-env.
|
||||
- `tests/test_shoot_cfg.py` — CRÉER : test d'intégration (l'env se construit, bonne commande/rewards).
|
||||
|
||||
---
|
||||
|
||||
### Task 1: Flag `randomize_phase` sur la commande de phase
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/mdp.py:3618-3672` (`GroundPickPhaseCommand` + `GroundPickPhaseCommandCfg`)
|
||||
- Test: `tests/test_shoot.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Produces: `GroundPickPhaseCommandCfg(randomize_phase: bool = True, period: float = 4.0, …)` ; à l'exécution `reset()` met φ=0 quand `randomize_phase=False`, sinon `rand()`.
|
||||
|
||||
- [ ] **Step 1: Écrire le test qui échoue**
|
||||
|
||||
Créer `tests/test_shoot.py` avec :
|
||||
|
||||
```python
|
||||
from mjlab_microduck.tasks.mdp import GroundPickPhaseCommandCfg
|
||||
|
||||
|
||||
def test_phase_cmd_randomize_flag_default_true():
|
||||
cfg = GroundPickPhaseCommandCfg()
|
||||
assert cfg.randomize_phase is True
|
||||
|
||||
|
||||
def test_phase_cmd_randomize_flag_settable_false():
|
||||
cfg = GroundPickPhaseCommandCfg(randomize_phase=False)
|
||||
assert cfg.randomize_phase is False
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Lancer le test, vérifier l'échec**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_shoot.py -q`
|
||||
Expected: FAIL — `TypeError: __init__() got an unexpected keyword argument 'randomize_phase'`.
|
||||
|
||||
- [ ] **Step 3: Ajouter le champ au cfg + threading dans la classe**
|
||||
|
||||
Dans `GroundPickPhaseCommandCfg` (dataclass, ~ligne 3667) ajouter le champ :
|
||||
|
||||
```python
|
||||
@_dataclass(kw_only=True)
|
||||
class GroundPickPhaseCommandCfg(UniformVelocityCommandCfg):
|
||||
class_type: type = GroundPickPhaseCommand
|
||||
period: float = 4.0 # cycle length in seconds; sitstand uses 8.0
|
||||
randomize_phase: bool = True # False -> chaque épisode démarre à φ=0 (STAND)
|
||||
|
||||
def build(self, env: ManagerBasedRlEnv) -> "GroundPickPhaseCommand":
|
||||
return GroundPickPhaseCommand(self, env)
|
||||
```
|
||||
|
||||
Dans `GroundPickPhaseCommand.__init__` (~ligne 3634) lire le flag :
|
||||
|
||||
```python
|
||||
def __init__(self, cfg, env: ManagerBasedRlEnv):
|
||||
super().__init__(cfg, env)
|
||||
self._gp_phase = torch.zeros(self.num_envs, device=self.device)
|
||||
self._period = float(getattr(cfg, "period", self.PERIOD))
|
||||
self._randomize_phase = bool(getattr(cfg, "randomize_phase", True))
|
||||
```
|
||||
|
||||
Dans `GroundPickPhaseCommand.reset` (~ligne 3649) respecter le flag :
|
||||
|
||||
```python
|
||||
def reset(self, env_ids: torch.Tensor | None) -> dict:
|
||||
if env_ids is not None and len(env_ids) > 0:
|
||||
if self._randomize_phase:
|
||||
self._gp_phase[env_ids] = torch.rand(len(env_ids), device=self.device)
|
||||
else:
|
||||
self._gp_phase[env_ids] = 0.0
|
||||
return {}
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Lancer le test, vérifier le succès**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_shoot.py -q`
|
||||
Expected: PASS (2 tests).
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/mdp.py tests/test_shoot.py
|
||||
git commit -m "feat: flag randomize_phase sur GroundPickPhaseCommand (défaut True)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Task 2: Fonction pure `kick_pose_target`
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/mdp.py` (ajouter près de `phase_pose_blend`, ~ligne 2062)
|
||||
- Test: `tests/test_shoot.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Produces: `kick_pose_target(phase: Tensor(B,), stand, back, forward, windup_end: float, kick_end: float, return_end: float) -> Tensor(B,k)`. `stand/back/forward` sont des tenseurs `(k,)` ou `(1,k)`. Segments : [0,windup_end) STAND→BACK, [windup_end,kick_end) BACK→FORWARD, [kick_end,return_end) FORWARD→STAND, [return_end,1) STAND.
|
||||
|
||||
- [ ] **Step 1: Écrire les tests qui échouent**
|
||||
|
||||
Ajouter à `tests/test_shoot.py` :
|
||||
|
||||
```python
|
||||
import torch
|
||||
from mjlab_microduck.tasks.mdp import kick_pose_target
|
||||
|
||||
W, K, R = 0.35, 0.45, 0.75 # windup_end, kick_end, return_end
|
||||
STAND = torch.tensor([0.0, 0.0])
|
||||
BACK = torch.tensor([1.0, -1.0])
|
||||
FWD = torch.tensor([-1.0, 2.0])
|
||||
|
||||
|
||||
def _t(phase):
|
||||
return kick_pose_target(torch.tensor([phase]), STAND, BACK, FWD, W, K, R)[0]
|
||||
|
||||
|
||||
def test_kick_target_keypoints():
|
||||
assert torch.allclose(_t(0.0), STAND) # début: STAND
|
||||
assert torch.allclose(_t(W), BACK) # fin armement: BACK
|
||||
assert torch.allclose(_t(K), FWD) # fin frappe: FORWARD
|
||||
assert torch.allclose(_t(R), STAND) # fin retour: STAND
|
||||
assert torch.allclose(_t(0.9), STAND) # repos: STAND
|
||||
|
||||
|
||||
def test_kick_target_midsegments():
|
||||
assert torch.allclose(_t(W / 2), 0.5 * BACK) # mi-armement
|
||||
assert torch.allclose(_t((W + K) / 2), 0.5 * (BACK + FWD)) # mi-frappe
|
||||
assert torch.allclose(_t((K + R) / 2), 0.5 * FWD) # mi-retour
|
||||
|
||||
|
||||
def test_kick_target_batch_shape():
|
||||
phase = torch.linspace(0.0, 1.0, 50)
|
||||
out = kick_pose_target(phase, STAND, BACK, FWD, W, K, R)
|
||||
assert out.shape == (50, 2)
|
||||
# chaque composante reste dans l'enveloppe des 3 poses
|
||||
lo = torch.minimum(torch.minimum(STAND, BACK), FWD)
|
||||
hi = torch.maximum(torch.maximum(STAND, BACK), FWD)
|
||||
assert (out >= lo - 1e-6).all() and (out <= hi + 1e-6).all()
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Lancer, vérifier l'échec**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_shoot.py -q`
|
||||
Expected: FAIL — `ImportError: cannot import name 'kick_pose_target'`.
|
||||
|
||||
- [ ] **Step 3: Implémenter la fonction pure**
|
||||
|
||||
Ajouter dans `mdp.py` juste après `phase_pose_blend` (~ligne 2062) :
|
||||
|
||||
```python
|
||||
def kick_pose_target(
|
||||
phase: torch.Tensor,
|
||||
stand: torch.Tensor,
|
||||
back: torch.Tensor,
|
||||
forward: torch.Tensor,
|
||||
windup_end: float,
|
||||
kick_end: float,
|
||||
return_end: float,
|
||||
) -> torch.Tensor:
|
||||
"""Cible articulaire interpolée d'un geste de shoot à 4 keyframes.
|
||||
|
||||
phase (B,) ∈ [0,1). stand/back/forward (k,) ou (1,k). Retour (B,k).
|
||||
|
||||
[0, windup_end) STAND -> BACK (armement)
|
||||
[windup_end, kick_end) BACK -> FORWARD (frappe sèche)
|
||||
[kick_end, return_end) FORWARD -> STAND (retour)
|
||||
[return_end, 1.0) STAND (repos)
|
||||
"""
|
||||
p = phase.unsqueeze(-1) # (B,1)
|
||||
|
||||
def interp(a, b, s):
|
||||
return a + s * (b - a)
|
||||
|
||||
s1 = (p / windup_end).clamp(0.0, 1.0)
|
||||
s2 = ((p - windup_end) / (kick_end - windup_end)).clamp(0.0, 1.0)
|
||||
s3 = ((p - kick_end) / (return_end - kick_end)).clamp(0.0, 1.0)
|
||||
|
||||
seg1 = interp(stand, back, s1)
|
||||
seg2 = interp(back, forward, s2)
|
||||
seg3 = interp(forward, stand, s3) # à s3=1 (phase>=return_end) => STAND
|
||||
|
||||
out = seg1
|
||||
out = torch.where(p >= windup_end, seg2, out)
|
||||
out = torch.where(p >= kick_end, seg3, out)
|
||||
return out
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Lancer, vérifier le succès**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_shoot.py -q`
|
||||
Expected: PASS (tous les tests kick_target).
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/mdp.py tests/test_shoot.py
|
||||
git commit -m "feat: kick_pose_target — cible interpolée du geste de shoot (4 keyframes)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Task 3: Rewards de suivi `kick_pose_track` / `kick_pose_track_l1`
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/mdp.py` (ajouter après `kick_pose_target`)
|
||||
- Test: `tests/test_shoot.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `kick_pose_target` (Task 2).
|
||||
- Produces:
|
||||
- `kick_pose_track(env, command_name="twist", stand_pose=None, back_pose=None, forward_pose=None, std=0.4, windup_end=0.35, kick_end=0.45, return_end=0.75, asset_cfg=_DEFAULT_ASSET_CFG) -> Tensor(B,)` — gaussienne `exp(-((q-cible)/std)²).mean`.
|
||||
- `kick_pose_track_l1(env, …mêmes args sauf std) -> Tensor(B,)` — `-(|q-cible|).mean`.
|
||||
- Helper `_kick_pose_error(env, asset_cfg, command_name, stand_pose, back_pose, forward_pose, windup_end, kick_end, return_end) -> (cur, target)`.
|
||||
|
||||
- [ ] **Step 1: Écrire le test qui échoue (stub-env)**
|
||||
|
||||
Ajouter à `tests/test_shoot.py` :
|
||||
|
||||
```python
|
||||
from mjlab_microduck.tasks.mdp import kick_pose_track, kick_pose_track_l1
|
||||
|
||||
STAND_D = {"a": 0.0, "b": 0.0}
|
||||
BACK_D = {"a": 1.0, "b": -1.0}
|
||||
FWD_D = {"a": -1.0, "b": 2.0}
|
||||
_IDX = {"a": 0, "b": 1}
|
||||
|
||||
|
||||
class _FakeData:
|
||||
def __init__(self, joint_pos):
|
||||
self.joint_pos = joint_pos
|
||||
self.default_joint_pos = torch.zeros_like(joint_pos)
|
||||
|
||||
|
||||
class _FakeAsset:
|
||||
def __init__(self, joint_pos):
|
||||
self.data = _FakeData(joint_pos)
|
||||
|
||||
def find_joints(self, names):
|
||||
return ([_IDX[names[0]]], names)
|
||||
|
||||
|
||||
class _FakeScene:
|
||||
def __init__(self, asset):
|
||||
self._a = asset
|
||||
|
||||
def __getitem__(self, name):
|
||||
return self._a
|
||||
|
||||
|
||||
class _FakeCmdMgr:
|
||||
def __init__(self, cmd):
|
||||
self._cmd = cmd
|
||||
|
||||
def get_command(self, name):
|
||||
return self._cmd
|
||||
|
||||
|
||||
class _FakeEnv:
|
||||
def __init__(self, joint_pos, phase):
|
||||
self.scene = _FakeScene(_FakeAsset(joint_pos))
|
||||
# cmd = [cos, sin, 0]
|
||||
cmd = torch.stack(
|
||||
[torch.cos(2 * torch.pi * phase), torch.sin(2 * torch.pi * phase),
|
||||
torch.zeros_like(phase)], dim=-1)
|
||||
self.command_manager = _FakeCmdMgr(cmd)
|
||||
self.device = "cpu"
|
||||
self.num_envs = joint_pos.shape[0]
|
||||
|
||||
|
||||
def test_kick_track_perfect_at_stand_phase():
|
||||
# phase=0 -> cible STAND=[0,0] ; joint_pos exactement STAND -> reward ~1
|
||||
env = _FakeEnv(torch.tensor([[0.0, 0.0]]), torch.tensor([0.0]))
|
||||
r = kick_pose_track(env, stand_pose=STAND_D, back_pose=BACK_D, forward_pose=FWD_D)
|
||||
assert torch.allclose(r, torch.tensor([1.0]), atol=1e-4)
|
||||
|
||||
|
||||
def test_kick_track_lower_when_off_target():
|
||||
# phase=0.45 (kick_end) -> cible FORWARD=[-1,2] ; joint_pos=STAND -> reward < 0.5
|
||||
env = _FakeEnv(torch.tensor([[0.0, 0.0]]), torch.tensor([0.45]))
|
||||
r = kick_pose_track(env, stand_pose=STAND_D, back_pose=BACK_D, forward_pose=FWD_D)
|
||||
assert (r < 0.5).all()
|
||||
|
||||
|
||||
def test_kick_track_l1_zero_when_perfect():
|
||||
env = _FakeEnv(torch.tensor([[0.0, 0.0]]), torch.tensor([0.0]))
|
||||
r = kick_pose_track_l1(env, stand_pose=STAND_D, back_pose=BACK_D, forward_pose=FWD_D)
|
||||
assert torch.allclose(r, torch.tensor([0.0]), atol=1e-6)
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Lancer, vérifier l'échec**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_shoot.py -q`
|
||||
Expected: FAIL — `ImportError: cannot import name 'kick_pose_track'`.
|
||||
|
||||
- [ ] **Step 3: Implémenter helper + rewards**
|
||||
|
||||
Ajouter dans `mdp.py` après `kick_pose_target` :
|
||||
|
||||
```python
|
||||
def _kick_pose_error(
|
||||
env: ManagerBasedRlEnv,
|
||||
asset_cfg: SceneEntityCfg,
|
||||
command_name: str,
|
||||
stand_pose: dict,
|
||||
back_pose: dict,
|
||||
forward_pose: dict,
|
||||
windup_end: float,
|
||||
kick_end: float,
|
||||
return_end: float,
|
||||
):
|
||||
"""(cur, target) pour le geste de shoot, joints résolus PAR NOM.
|
||||
|
||||
Les 3 poses partagent les mêmes clés (14 joints). L'ordre des noms est
|
||||
donné par `stand_pose`.
|
||||
"""
|
||||
if not stand_pose:
|
||||
raise ValueError("_kick_pose_error requires a non-empty stand_pose dict")
|
||||
asset: Entity = env.scene[asset_cfg.name]
|
||||
names = list(stand_pose.keys())
|
||||
ids = [int(asset.find_joints([n])[0][0]) for n in names]
|
||||
|
||||
def vec(d):
|
||||
return torch.tensor([d[n] for n in names], device=env.device,
|
||||
dtype=asset.data.joint_pos.dtype)
|
||||
|
||||
stand_v, back_v, fwd_v = vec(stand_pose), vec(back_pose), vec(forward_pose)
|
||||
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
phase = (torch.atan2(cmd[:, 1], cmd[:, 0]) / (2 * torch.pi)) % 1.0 # (B,)
|
||||
target = kick_pose_target(phase, stand_v, back_v, fwd_v,
|
||||
windup_end, kick_end, return_end) # (B,k)
|
||||
cur = asset.data.joint_pos[:, ids] # (B,k)
|
||||
return cur, target
|
||||
|
||||
|
||||
def kick_pose_track(
|
||||
env: ManagerBasedRlEnv,
|
||||
command_name: str = "twist",
|
||||
stand_pose: Optional[dict] = None,
|
||||
back_pose: Optional[dict] = None,
|
||||
forward_pose: Optional[dict] = None,
|
||||
std: float = 0.4,
|
||||
windup_end: float = 0.35,
|
||||
kick_end: float = 0.45,
|
||||
return_end: float = 0.75,
|
||||
asset_cfg: SceneEntityCfg = _DEFAULT_ASSET_CFG,
|
||||
) -> torch.Tensor:
|
||||
"""Gaussienne sur la pose articulaire vs cible interpolée du shoot.
|
||||
|
||||
Reward directif et symétrique : chaque phase impose la config articulaire
|
||||
exacte. Résolution PAR NOM.
|
||||
"""
|
||||
cur, target = _kick_pose_error(
|
||||
env, asset_cfg, command_name, stand_pose or {}, back_pose or {},
|
||||
forward_pose or {}, windup_end, kick_end, return_end,
|
||||
)
|
||||
return torch.exp(-((cur - target) / std) ** 2).mean(dim=-1)
|
||||
|
||||
|
||||
def kick_pose_track_l1(
|
||||
env: ManagerBasedRlEnv,
|
||||
command_name: str = "twist",
|
||||
stand_pose: Optional[dict] = None,
|
||||
back_pose: Optional[dict] = None,
|
||||
forward_pose: Optional[dict] = None,
|
||||
windup_end: float = 0.35,
|
||||
kick_end: float = 0.45,
|
||||
return_end: float = 0.75,
|
||||
asset_cfg: SceneEntityCfg = _DEFAULT_ASSET_CFG,
|
||||
) -> torch.Tensor:
|
||||
"""Bootstrap L1 vers la cible interpolée (gradient constant, pénalité<=0)."""
|
||||
cur, target = _kick_pose_error(
|
||||
env, asset_cfg, command_name, stand_pose or {}, back_pose or {},
|
||||
forward_pose or {}, windup_end, kick_end, return_end,
|
||||
)
|
||||
return -(cur - target).abs().mean(dim=-1)
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Lancer, vérifier le succès**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_shoot.py -q`
|
||||
Expected: PASS (tous les tests, y compris les 3 nouveaux).
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/mdp.py tests/test_shoot.py
|
||||
git commit -m "feat: rewards kick_pose_track + kick_pose_track_l1 (suivi du geste de shoot)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Task 4: Env config `microduck_shoot_env_cfg.py`
|
||||
|
||||
**Files:**
|
||||
- Create: `src/mjlab_microduck/tasks/microduck_shoot_env_cfg.py`
|
||||
- Test: (via Task 5)
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `kick_pose_track`, `kick_pose_track_l1` (Task 3) ; `GroundPickPhaseCommandCfg(randomize_phase=…)` (Task 1) ; `feet_grounded_reward`, `feet_flat_penalty`, `neck_action_rate_l2`, `joint_torques_l2`, `zero_command_padding`, `robot_state_is_nan`, DR events (existants dans `mdp.py`).
|
||||
- Produces: `make_microduck_shoot_env_cfg(play=False, rough=False) -> ManagerBasedRlEnvCfg` ; `MicroduckShootRlCfg` ; constantes `SHOOT_PERIOD`, `WINDUP_END`, `KICK_END`, `RETURN_END`, `STAND_POSE`, `KICK_BACK_POSE`, `KICK_FWD_POSE`.
|
||||
|
||||
- [ ] **Step 1: Partir du fichier ground_pick comme base**
|
||||
|
||||
```bash
|
||||
cp src/mjlab_microduck/tasks/microduck_ground_pick_env_cfg.py \
|
||||
src/mjlab_microduck/tasks/microduck_shoot_env_cfg.py
|
||||
```
|
||||
|
||||
Ce fichier fournit déjà TOUT le boilerplate sim2real à conserver tel quel : DR (CoM, head CoM, mass/inertia, friction BAM, armature, IMU misalignment obs-level, encoder-bias, pushes), le bloc obs 61D (`del base_lin_vel` actor, critic base_lin_vel, suppression `foot_height`/`height_scan`, delays/noise, `head_command`/`body_command` zero-padding), la terminaison `nan_state`, les events `expand_bam_friction_fields` / `reset_action_history`, le curriculum action_rate/CoM. On ne modifie que : robot cfg, capteurs, commande, et le bloc rewards.
|
||||
|
||||
- [ ] **Step 2: Adapter l'en-tête, le nom de fonction et les constantes**
|
||||
|
||||
Remplacer le docstring de tête par une description shoot, et juste avant `def make_microduck_ground_pick_env_cfg`, ajouter les constantes + poses (placeholders — à remplacer par lecture `read_pose.py`). Renommer la fonction en `make_microduck_shoot_env_cfg`.
|
||||
|
||||
```python
|
||||
# ── Timings du geste (phase normalisée [0,1)) ────────────────────────────────
|
||||
SHOOT_PERIOD = 2.5 # s — durée d'un cycle (doit matcher --ground-pick-period au déploiement)
|
||||
WINDUP_END = 0.35 # STAND -> BACK
|
||||
KICK_END = 0.45 # BACK -> FORWARD (segment court = frappe sèche)
|
||||
RETURN_END = 0.75 # FORWARD -> STAND, puis repos jusqu'à 1.0
|
||||
|
||||
# ── Poses (rad, 14 joints, mouth exclu) ──────────────────────────────────────
|
||||
# Convention: jambe droite frappe (hanche/genou droit actifs), gauche en appui.
|
||||
# STAND_POSE = pose HOME du sim (HOME_FRAME / default_joint_pos) pour que φ=0
|
||||
# coïncide avec la config de reset (invariant randomize_phase=False). BACK/FWD
|
||||
# sont des PLACEHOLDERS jambe droite, à affiner via read_pose.py.
|
||||
STAND_POSE = {
|
||||
"left_hip_yaw": 0.0, "left_hip_roll": -0.0873, "left_hip_pitch": -0.4579,
|
||||
"left_knee": -0.0049, "left_ankle": 0.4530,
|
||||
"neck_pitch": 0.3491, "head_pitch": 0.3491, "head_yaw": 0.0, "head_roll": 0.0,
|
||||
"right_hip_yaw": 0.0, "right_hip_roll": 0.0873, "right_hip_pitch": 0.4579,
|
||||
"right_knee": 0.0049, "right_ankle": -0.4530,
|
||||
}
|
||||
KICK_BACK_POSE = { # armement: hanche droite en extension arrière + genou fléchi
|
||||
**STAND_POSE,
|
||||
"right_hip_pitch": -0.6,
|
||||
"right_knee": 0.8,
|
||||
"right_ankle": -0.2,
|
||||
}
|
||||
KICK_FWD_POSE = { # frappe: hanche droite fléchie avant + genou tendu
|
||||
**STAND_POSE,
|
||||
"right_hip_pitch": 0.7,
|
||||
"right_knee": -0.1,
|
||||
"right_ankle": 0.1,
|
||||
}
|
||||
```
|
||||
|
||||
> NOTE au releveur de poses : remplacer ces valeurs par des lectures `read_pose.py` (couple coupé, robot posé à la main dans chaque position). Garder les 14 clés identiques dans les 3 dicts.
|
||||
|
||||
- [ ] **Step 3: Robot cfg et import**
|
||||
|
||||
Dans les imports, remplacer `MICRODUCK_GROUND_PICK_ROBOT_CFG` par `MICRODUCK_WALK_ROBOT_CFG` :
|
||||
|
||||
```python
|
||||
from mjlab_microduck.robot.microduck_constants import MICRODUCK_WALK_ROBOT_CFG
|
||||
```
|
||||
|
||||
Dans la fonction, la ligne d'entités :
|
||||
|
||||
```python
|
||||
cfg.scene.entities = {"robot": MICRODUCK_WALK_ROBOT_CFG}
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Capteurs — garder self_collision, remplacer les capteurs pied**
|
||||
|
||||
Remplacer la définition du capteur `feet_ground_contact` (2 pieds) par un capteur **pied gauche seul** (appui), et SUPPRIMER le capteur `head_impact_cfg` (inutile ici). Le capteur `self_collision_cfg` reste.
|
||||
|
||||
```python
|
||||
left_foot_ground_cfg = ContactSensorCfg(
|
||||
name="left_foot_ground_contact",
|
||||
primary=ContactMatch(
|
||||
mode="geom",
|
||||
pattern=r"^left_foot_collision$",
|
||||
entity="robot",
|
||||
),
|
||||
secondary=ContactMatch(mode="body", pattern="terrain"),
|
||||
fields=("found", "force"),
|
||||
reduce="netforce",
|
||||
num_slots=1,
|
||||
track_air_time=True,
|
||||
)
|
||||
```
|
||||
|
||||
Et la ligne des capteurs de scène :
|
||||
|
||||
```python
|
||||
cfg.scene.sensors = (left_foot_ground_cfg, self_collision_cfg)
|
||||
```
|
||||
|
||||
Supprimer la définition de `head_impact_cfg` et toute référence (le reward `head_impact_penalty` est retiré au Step 6).
|
||||
|
||||
- [ ] **Step 5: Commande de phase (randomize_phase=False, période shoot)**
|
||||
|
||||
Remplacer le bloc commande (celui qui crée `GroundPickPhaseCommandCfg`) par :
|
||||
|
||||
```python
|
||||
command: UniformVelocityCommandCfg = cfg.commands["twist"]
|
||||
command.rel_standing_envs = 0.0
|
||||
command.rel_heading_envs = 0.0
|
||||
cfg.commands["twist"] = microduck_mdp.GroundPickPhaseCommandCfg(
|
||||
**{**vars(command), "class_type": microduck_mdp.GroundPickPhaseCommand}
|
||||
)
|
||||
cfg.commands["twist"].period = SHOOT_PERIOD
|
||||
cfg.commands["twist"].randomize_phase = False
|
||||
```
|
||||
|
||||
- [ ] **Step 6: Rewards — retirer ground_pick, ajouter shoot**
|
||||
|
||||
Supprimer les rewards spécifiques ground_pick : `mouth_ground_proximity`, `mouth_perpendicular_to_ground`, `ground_pick_return_pose_legs`, `ground_pick_return_pose_neck`, `feet_grounded` (les 2 pieds), `head_impact_penalty`. Remplacer par le bloc shoot :
|
||||
|
||||
```python
|
||||
# ── Objectif : suivi de la pose interpolée du shoot ───────────────────────
|
||||
_pose_params = {
|
||||
"command_name": "twist",
|
||||
"stand_pose": STAND_POSE,
|
||||
"back_pose": KICK_BACK_POSE,
|
||||
"forward_pose": KICK_FWD_POSE,
|
||||
"windup_end": WINDUP_END,
|
||||
"kick_end": KICK_END,
|
||||
"return_end": RETURN_END,
|
||||
}
|
||||
cfg.rewards["kick_pose_track"] = RewardTermCfg(
|
||||
func=microduck_mdp.kick_pose_track,
|
||||
weight=6.0,
|
||||
params={**_pose_params, "std": 0.4},
|
||||
)
|
||||
cfg.rewards["kick_pose_l1"] = RewardTermCfg(
|
||||
func=microduck_mdp.kick_pose_track_l1,
|
||||
weight=2.0,
|
||||
params=dict(_pose_params),
|
||||
)
|
||||
|
||||
# ── Équilibre / appui (jambe unique) ──────────────────────────────────────
|
||||
cfg.rewards["upright"].params["asset_cfg"].body_names = ("trunk_base",)
|
||||
cfg.rewards["upright"].weight = 2.0
|
||||
cfg.rewards["body_ang_vel"].params["asset_cfg"].body_names = ("trunk_base",)
|
||||
cfg.rewards["body_ang_vel"].weight = -0.05
|
||||
|
||||
# Pied GAUCHE planté (appui). feet_grounded_reward avec un capteur mono-pied
|
||||
# -> found ∈ {0,1} -> reward ∈ {0,0.5} ; poids 6.0 => contribution max ~3.0.
|
||||
cfg.rewards["support_foot_grounded"] = RewardTermCfg(
|
||||
func=microduck_mdp.feet_grounded_reward,
|
||||
weight=6.0,
|
||||
params={"sensor_name": left_foot_ground_cfg.name},
|
||||
)
|
||||
|
||||
# Pied gauche à plat.
|
||||
cfg.rewards["feet_flat_left"] = RewardTermCfg(
|
||||
func=microduck_mdp.feet_flat_penalty,
|
||||
weight=-1.0,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", site_names=("left_foot",))},
|
||||
)
|
||||
|
||||
cfg.rewards["self_collisions"] = RewardTermCfg(
|
||||
func=mdp.self_collision_cost,
|
||||
weight=-1.0,
|
||||
params={"sensor_name": self_collision_cfg.name},
|
||||
)
|
||||
```
|
||||
|
||||
- [ ] **Step 7: Régularisation allégée (laisser passer le snap)**
|
||||
|
||||
Le fichier ground_pick met `action_rate_l2=-2.0`, `neck_action_rate_l2=-1.0`, `joint_torques_l2=-5e-3` + un curriculum action_rate qui finit à -2.0. Pour le shoot on allège. Remplacer ces 3 blocs par :
|
||||
|
||||
```python
|
||||
cfg.rewards["action_rate_l2"] = RewardTermCfg(
|
||||
func=mdp.action_rate_l2, weight=-0.5
|
||||
)
|
||||
cfg.rewards["neck_action_rate_l2"] = RewardTermCfg(
|
||||
func=microduck_mdp.neck_action_rate_l2, weight=-0.5
|
||||
)
|
||||
cfg.rewards["joint_torques_l2"] = RewardTermCfg(
|
||||
func=microduck_mdp.joint_torques_l2, weight=-1e-3
|
||||
)
|
||||
```
|
||||
|
||||
Et alléger le curriculum action_rate (garder la structure, viser -0.5) :
|
||||
|
||||
```python
|
||||
cfg.curriculum["action_rate_weight"] = CurriculumTermCfg(
|
||||
func=microduck_mdp.reward_weight,
|
||||
params={
|
||||
"reward_name": "action_rate_l2",
|
||||
"weight_stages": [
|
||||
{"step": 0, "weight": -0.2},
|
||||
{"step": 250 * 24, "weight": -0.4},
|
||||
{"step": 500 * 24, "weight": -0.5},
|
||||
],
|
||||
},
|
||||
)
|
||||
```
|
||||
|
||||
- [ ] **Step 8: Reset — hauteur de station debout**
|
||||
|
||||
Garder la **hauteur debout** `(0.12, 0.13)` — c'est la valeur de l'env velocity
|
||||
(marche) ET de ground_pick. ⚠️ Ce n'est PAS un offset additif « station accroupie » :
|
||||
le `pos` racine par défaut de `InitialStateCfg` est (0,0,0), donc la hauteur de reset
|
||||
est z ∈ [0.12, 0.13] m **absolue** = debout (aucune chute). Vérifier/mettre :
|
||||
|
||||
```python
|
||||
cfg.events["reset_base"].params["pose_range"]["z"] = (0.12, 0.13)
|
||||
```
|
||||
|
||||
(Ne PAS injecter de vitesse d'entrée — c'est un shoot debout, pas de glisse.)
|
||||
|
||||
- [ ] **Step 9: Renommer la RlCfg**
|
||||
|
||||
En bas du fichier, renommer `MicroduckGroundPickRlCfg` en `MicroduckShootRlCfg` et changer les noms d'expérience :
|
||||
|
||||
```python
|
||||
MicroduckShootRlCfg = RslRlOnPolicyRunnerCfg(
|
||||
# … (garder actor/critic/algorithm identiques) …
|
||||
wandb_project="mjlab_microduck",
|
||||
experiment_name="shoot",
|
||||
run_name="shoot",
|
||||
save_interval=250,
|
||||
num_steps_per_env=24,
|
||||
max_iterations=20_000,
|
||||
)
|
||||
```
|
||||
|
||||
- [ ] **Step 10: Vérifier que le module s'importe**
|
||||
|
||||
Run: `uv run python -c "from mjlab_microduck.tasks.microduck_shoot_env_cfg import make_microduck_shoot_env_cfg, MicroduckShootRlCfg; print('ok')"`
|
||||
Expected: `ok` (pas d'ImportError / NameError — en particulier plus aucune référence à `head_impact_cfg`, `MICRODUCK_GROUND_PICK_ROBOT_CFG`, ni aux rewards ground_pick supprimés).
|
||||
|
||||
- [ ] **Step 11: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/microduck_shoot_env_cfg.py
|
||||
git commit -m "feat: env config Mjlab-Shoot (geste de shoot par suivi de poses)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Task 5: Enregistrement + test d'intégration
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/__init__.py`
|
||||
- Test: `tests/test_shoot_cfg.py`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `make_microduck_shoot_env_cfg`, `MicroduckShootRlCfg` (Task 4).
|
||||
- Produces: tâche enregistrée `Mjlab-Shoot-Flat-MicroDuck`.
|
||||
|
||||
- [ ] **Step 1: Écrire le test d'intégration qui échoue**
|
||||
|
||||
Créer `tests/test_shoot_cfg.py` :
|
||||
|
||||
```python
|
||||
from mjlab_microduck.tasks.microduck_shoot_env_cfg import (
|
||||
make_microduck_shoot_env_cfg,
|
||||
STAND_POSE, KICK_BACK_POSE, KICK_FWD_POSE, SHOOT_PERIOD,
|
||||
)
|
||||
from mjlab_microduck.tasks import mdp as microduck_mdp
|
||||
|
||||
|
||||
def test_poses_have_same_14_keys():
|
||||
assert set(STAND_POSE) == set(KICK_BACK_POSE) == set(KICK_FWD_POSE)
|
||||
assert len(STAND_POSE) == 14
|
||||
assert "mouth" not in STAND_POSE
|
||||
|
||||
|
||||
def test_shoot_cfg_builds_with_phase_command():
|
||||
cfg = make_microduck_shoot_env_cfg()
|
||||
twist = cfg.commands["twist"]
|
||||
assert isinstance(twist, microduck_mdp.GroundPickPhaseCommandCfg)
|
||||
assert twist.randomize_phase is False
|
||||
assert twist.period == SHOOT_PERIOD
|
||||
|
||||
|
||||
def test_shoot_cfg_has_kick_rewards_and_no_walking():
|
||||
cfg = make_microduck_shoot_env_cfg()
|
||||
assert "kick_pose_track" in cfg.rewards
|
||||
assert "kick_pose_l1" in cfg.rewards
|
||||
assert "support_foot_grounded" in cfg.rewards
|
||||
for gone in ("track_linear_velocity", "track_angular_velocity",
|
||||
"mouth_ground_proximity", "ground_pick_return_pose_legs"):
|
||||
assert gone not in cfg.rewards
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Lancer, vérifier l'échec**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/test_shoot_cfg.py -q`
|
||||
Expected: PASS possible sur les tests de poses, mais l'ensemble doit être vert seulement une fois l'env construit sans erreur ; si `make_...` lève, FAIL. (À ce stade l'import du fichier fonctionne déjà via Task 4.)
|
||||
|
||||
- [ ] **Step 3: Enregistrer la tâche**
|
||||
|
||||
Dans `src/mjlab_microduck/tasks/__init__.py`, après le bloc d'import ground_pick (~ligne 50), ajouter :
|
||||
|
||||
```python
|
||||
from .microduck_shoot_env_cfg import (
|
||||
make_microduck_shoot_env_cfg,
|
||||
MicroduckShootRlCfg,
|
||||
)
|
||||
```
|
||||
|
||||
Après le bloc `register_mjlab_task` de GroundPick-Rough (~ligne 161), ajouter :
|
||||
|
||||
```python
|
||||
register_mjlab_task(
|
||||
task_id="Mjlab-Shoot-Flat-MicroDuck",
|
||||
env_cfg=make_microduck_shoot_env_cfg(),
|
||||
play_env_cfg=make_microduck_shoot_env_cfg(play=True),
|
||||
rl_cfg=MicroduckShootRlCfg,
|
||||
runner_cls=MicroduckOnPolicyRunner,
|
||||
)
|
||||
print("✓ Shoot task registered: Mjlab-Shoot-Flat-MicroDuck")
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Lancer tout, vérifier le succès**
|
||||
|
||||
Run: `uv run --with pytest pytest tests/ -q`
|
||||
Expected: PASS (test_shoot.py + test_shoot_cfg.py + tests existants).
|
||||
|
||||
- [ ] **Step 5: Vérifier l'enregistrement de la tâche**
|
||||
|
||||
Run: `uv run python -c "import mjlab_microduck.tasks"`
|
||||
Expected: la sortie contient `✓ Shoot task registered: Mjlab-Shoot-Flat-MicroDuck`.
|
||||
|
||||
- [ ] **Step 6: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/__init__.py tests/test_shoot_cfg.py
|
||||
git commit -m "feat: enregistre Mjlab-Shoot-Flat-MicroDuck + test d'intégration"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Après implémentation (hors plan TDD)
|
||||
|
||||
1. **Relever les vraies poses** avec `read_pose.py` (STAND, PIED_ARRIÈRE, PIED_AVANT), remplacer les placeholders dans `microduck_shoot_env_cfg.py`.
|
||||
2. **Entraîner** : `uv run train Mjlab-Shoot-Flat-MicroDuck --env.scene.num-envs 4096 --agent.max_iterations 8000`. Surveiller `Episode_Reward/kick_pose_track` (doit monter).
|
||||
3. **Play** : script play_latest ; vérifier l'équilibre sur le pied gauche pendant la frappe.
|
||||
4. **Export ONNX** + déploiement dans un slot phase (`--ground-pick shoot.onnx --ground-pick-period 2.5 --ground-pick-kp-ratio 1.0`).
|
||||
5. **Réglages probables** : période/timings (snap), poids `action_rate`, et éventuel reward « vitesse pied vers l'avant » (segment frappe) si le suivi manque de punch.
|
||||
|
||||
## Self-review — couverture de la spec
|
||||
|
||||
- Fichier & enregistrement → Tasks 4, 5. ✅
|
||||
- Poses placeholders 14 joints → Task 4 Step 2, testé Task 5. ✅
|
||||
- Commande de phase + `randomize_phase=False` + période → Tasks 1, 4 Step 5, testé Task 5. ✅
|
||||
- `kick_pose_target` + `kick_pose_track` + `kick_pose_track_l1` → Tasks 2, 3. ✅
|
||||
- Équilibre/appui (upright, pied gauche planté, feet_flat gauche, self_collisions, body_ang_vel) → Task 4 Step 6. ✅
|
||||
- Régularisation allégée → Task 4 Step 7. ✅
|
||||
- Obs 61D parité (hérité ground_pick, conservé) → Task 4 Step 1. ✅
|
||||
- Tests pures + cfg → Tasks 2, 3, 5. ✅
|
||||
@@ -0,0 +1,204 @@
|
||||
# Swizzle Head Control Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** Add operator head-pose control (Y button) to the swizzle roller task so the policy moves its head to commanded poses while staying balanced.
|
||||
|
||||
**Architecture:** Policy-managed head via the observation command (matches the walking `--new-cmd-obs` path). The swizzle env currently zero-pads the `head_command` obs slot; we feed a real `head_pose` command into it, reward `head_pose_tracking`, remove the two reward terms that pull the neck/head to HOME (which would fight the command), and ramp the head in LATE via a curriculum so the already-working swizzle isn't disturbed. Config-only change to one file; requires retraining.
|
||||
|
||||
**Tech Stack:** mjlab / mjlab_microduck task configs (Python), rsl_rl PPO. Reuses machinery already in `microduck_velocity_env_cfg.py` (`UniformPoseCommandCfg`, `head_pose_tracking`, `pose_command_range_curriculum`, `reward_weight`).
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- Only the swizzle task changes: `src/mjlab_microduck/tasks/microduck_velocity_swizzle_env_cfg.py`. The stride, velocity, standup, roller-slope/crouch tasks and `mdp.py` are NOT modified.
|
||||
- Keep the 61D obs layout `[twist(3), head(4), body(6)]`: replace the `head_command` slot's contents (zero-pad → real command) but keep `body_command` zero-padded (no body-pose control here).
|
||||
- No new mdp functions — all reward/command/curriculum functions already exist in `microduck_mdp`.
|
||||
- Runtime unchanged: the `microduck_runtime` Y button already drives the `head_command` obs slot.
|
||||
|
||||
---
|
||||
|
||||
### Task 1: Wire head-pose control into the swizzle env
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/mjlab_microduck/tasks/microduck_velocity_swizzle_env_cfg.py`
|
||||
- Test: `tests/test_swizzle_head_cfg.py` (create)
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes (already exist, do not redefine):
|
||||
- `microduck_mdp.UniformPoseCommandCfg(resampling_time_range, ranges)` — head-pose command term.
|
||||
- `mdp.generated_commands` (from `mjlab.tasks.velocity`) — obs func reading a command by name; used as `params={"command_name": "head_pose"}`.
|
||||
- `microduck_mdp.head_pose_tracking` — reward `func`, params `{"command_name": "head_pose", "std": 0.5}`.
|
||||
- `microduck_mdp.reward_weight` — curriculum func, params `{"reward_name", "weight_stages": [{"step","weight"}, ...]}`.
|
||||
- `microduck_mdp.pose_command_range_curriculum` — curriculum func, params `{"command_name", "range_stages": [{"step","ranges"}, ...]}`.
|
||||
- Produces: the swizzle env cfg with a `head_pose` command, a real `head_command` obs, a `head_pose_tracking` reward, `neck_joint_pos_l2` removed, the `pose` reward scoped to leg joints, and two head curricula.
|
||||
|
||||
- [ ] **Step 1: Write the failing test**
|
||||
|
||||
Create `tests/test_swizzle_head_cfg.py`:
|
||||
|
||||
```python
|
||||
from mjlab.tasks.velocity import mdp
|
||||
from mjlab_microduck.tasks.microduck_velocity_swizzle_env_cfg import (
|
||||
make_microduck_velocity_swizzle_env_cfg,
|
||||
)
|
||||
|
||||
|
||||
def test_swizzle_head_control_wired():
|
||||
cfg = make_microduck_velocity_swizzle_env_cfg()
|
||||
|
||||
# Head-pose command term exists.
|
||||
assert "head_pose" in cfg.commands
|
||||
|
||||
# head_command obs is the REAL command (not zero-padded) on both groups.
|
||||
for group in ("actor", "critic"):
|
||||
term = cfg.observations[group].terms["head_command"]
|
||||
assert term.func is mdp.generated_commands
|
||||
assert term.params["command_name"] == "head_pose"
|
||||
|
||||
# head_pose_tracking reward exists.
|
||||
assert "head_pose_tracking" in cfg.rewards
|
||||
|
||||
# The two HOME-pullers that would fight the head command are handled:
|
||||
# - neck_joint_pos_l2 removed
|
||||
assert "neck_joint_pos_l2" not in cfg.rewards
|
||||
# - pose reward scoped to leg joints via a negative-lookahead regex that
|
||||
# excludes neck/head (and passive wheels)
|
||||
pose_joints = cfg.rewards["pose"].params["asset_cfg"].joint_names
|
||||
assert any(
|
||||
"(?!" in j and "neck" in j and "head" in j for j in pose_joints
|
||||
), f"pose reward not scoped away from neck/head: {pose_joints}"
|
||||
|
||||
# Late head curricula exist.
|
||||
assert "head_pose_tracking_weight" in cfg.curriculum
|
||||
assert "head_pose_range" in cfg.curriculum
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `MUJOCO_GL=egl uv run pytest tests/test_swizzle_head_cfg.py -v`
|
||||
Expected: FAIL (head_pose command / head_pose_tracking reward absent; head_command obs is still `zero_command_padding`).
|
||||
|
||||
- [ ] **Step 3: Add imports to the swizzle env cfg**
|
||||
|
||||
In `microduck_velocity_swizzle_env_cfg.py`, extend the imports (currently `from mjlab.managers import CurriculumTermCfg, RewardTermCfg`) to add `ObservationTermCfg`, and import the velocity mdp for `generated_commands`:
|
||||
|
||||
```python
|
||||
from mjlab.managers import CurriculumTermCfg, ObservationTermCfg, RewardTermCfg
|
||||
from mjlab.managers.scene_entity_config import SceneEntityCfg
|
||||
from mjlab.tasks.velocity import mdp
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Add the head_pose command + real head_command obs + head_pose_tracking reward + neck reconciliation**
|
||||
|
||||
Inside `make_microduck_velocity_swizzle_env_cfg`, AFTER the existing reward/heading setup and BEFORE `return cfg`, add:
|
||||
|
||||
```python
|
||||
# --- Head-pose control (Y button): the policy produces the head pose ---------
|
||||
# Head-pose command (4D deltas from HOME: [neck_pitch, head_pitch, head_yaw,
|
||||
# head_roll]). Ported from the velocity env; ranges start small (widened by the
|
||||
# curriculum below). Resample every 2-5 s.
|
||||
cfg.commands["head_pose"] = microduck_mdp.UniformPoseCommandCfg(
|
||||
resampling_time_range=(2.0, 5.0),
|
||||
ranges=(
|
||||
(-0.05, 0.05), # neck_pitch
|
||||
(-0.05, 0.05), # head_pitch
|
||||
(-0.07, 0.07), # head_yaw
|
||||
(-0.015, 0.015), # head_roll (tighter — small mechanical range)
|
||||
),
|
||||
)
|
||||
|
||||
# Feed the REAL head command into the obs (replaces zero_command_padding) on
|
||||
# both groups. body_command stays zero-padded (no body-pose control here).
|
||||
for group in ("actor", "critic"):
|
||||
cfg.observations[group].terms["head_command"] = ObservationTermCfg(
|
||||
func=mdp.generated_commands,
|
||||
params={"command_name": "head_pose"},
|
||||
)
|
||||
|
||||
# Reward the head tracking its command. Weight 0 here — ramped in LATE by the
|
||||
# curriculum so it doesn't disturb the swizzle before it's solid.
|
||||
cfg.rewards["head_pose_tracking"] = RewardTermCfg(
|
||||
func=microduck_mdp.head_pose_tracking,
|
||||
weight=0.0,
|
||||
params={"command_name": "head_pose", "std": 0.5},
|
||||
)
|
||||
|
||||
# Reconcile the two HOME-pullers that would fight head_pose_tracking:
|
||||
# 1) neck_joint_pos_l2 pulls the neck/head joints to HOME -> remove it.
|
||||
if "neck_joint_pos_l2" in cfg.rewards:
|
||||
del cfg.rewards["neck_joint_pos_l2"]
|
||||
# 2) the pose reward includes neck/head -> scope it to LEG joints only.
|
||||
cfg.rewards["pose"].params["asset_cfg"] = SceneEntityCfg(
|
||||
"robot", joint_names=(r"^(?!passive_|.*neck.*|.*head.*).*",)
|
||||
)
|
||||
```
|
||||
|
||||
- [ ] **Step 5: Add the late head curricula**
|
||||
|
||||
Immediately after the block from Step 4 (still before `return cfg`):
|
||||
|
||||
```python
|
||||
# head_pose_tracking ramps 0 -> 4.0, staying 0 until ~1500 it. (swizzle solid),
|
||||
# so head control is added on top of a stable swizzle.
|
||||
cfg.curriculum["head_pose_tracking_weight"] = CurriculumTermCfg(
|
||||
func=microduck_mdp.reward_weight,
|
||||
params={
|
||||
"reward_name": "head_pose_tracking",
|
||||
"weight_stages": [
|
||||
{"step": 0, "weight": 0.0}, # must match initial weight
|
||||
{"step": 1500 * 24, "weight": 0.0}, # head off while swizzle solidifies
|
||||
{"step": 2250 * 24, "weight": 2.0},
|
||||
{"step": 3000 * 24, "weight": 4.0},
|
||||
],
|
||||
},
|
||||
)
|
||||
# Head-command range widens over the SAME window (tiny until 1500, full by 3000),
|
||||
# so the commanded head barely moves early and reaches full range once the policy
|
||||
# can handle it.
|
||||
cfg.curriculum["head_pose_range"] = CurriculumTermCfg(
|
||||
func=microduck_mdp.pose_command_range_curriculum,
|
||||
params={
|
||||
"command_name": "head_pose",
|
||||
"range_stages": [
|
||||
# step, ((neck_pitch), (head_pitch), (head_yaw), (head_roll))
|
||||
{"step": 0, "ranges": ((-0.05, 0.05), (-0.05, 0.05), (-0.07, 0.07), (-0.015, 0.015))},
|
||||
{"step": 1500 * 24, "ranges": ((-0.05, 0.05), (-0.05, 0.05), (-0.07, 0.07), (-0.015, 0.015))},
|
||||
{"step": 2250 * 24, "ranges": ((-0.55, 0.55), (-0.55, 0.55), (-0.70, 0.70), (-0.15, 0.15))},
|
||||
{"step": 3000 * 24, "ranges": ((-1.10, 1.10), (-1.10, 1.10), (-1.40, 1.40), (-0.31, 0.31))},
|
||||
],
|
||||
},
|
||||
)
|
||||
```
|
||||
|
||||
- [ ] **Step 6: Run the cfg test to verify it passes**
|
||||
|
||||
Run: `MUJOCO_GL=egl uv run pytest tests/test_swizzle_head_cfg.py -v`
|
||||
Expected: PASS.
|
||||
|
||||
- [ ] **Step 7: Smoke test the env end-to-end**
|
||||
|
||||
Run: `MUJOCO_GL=egl uv run train Mjlab-Velocity-Swizzle-MicroDuck --env.scene.num-envs 16 --agent.max-iterations 2`
|
||||
Expected: no error; the reward log lists `head_pose_tracking` and no longer lists `neck_joint_pos_l2`; a `Curriculum/head_pose_tracking_weight` line appears at value 0.0.
|
||||
|
||||
- [ ] **Step 8: Commit**
|
||||
|
||||
```bash
|
||||
git add src/mjlab_microduck/tasks/microduck_velocity_swizzle_env_cfg.py tests/test_swizzle_head_cfg.py
|
||||
git commit -m "swizzle: add head-pose control (Y button, policy-managed, late curriculum)"
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Notes for the full training run (not part of the task)
|
||||
|
||||
Because the head curriculum only finishes at ~3000 iters, train longer than the
|
||||
2500 used before:
|
||||
|
||||
```bash
|
||||
uv run train Mjlab-Velocity-Swizzle-MicroDuck --env.scene.num-envs 4096 --agent.max-iterations 3500
|
||||
```
|
||||
|
||||
Watch: `head_pose_tracking` rises after ~1500 it.; the swizzle fall rate does NOT
|
||||
spike when it kicks in. If the head disturbs the swizzle → push the kick-in later /
|
||||
widen the range more slowly. If the head doesn't follow → raise the final weight.
|
||||
Deploy unchanged (`--roller --new-cmd-obs`, Y button moves the head).
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user