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,195 @@
|
||||
# Policy `roller_standup` — se relever sur rollers
|
||||
|
||||
**But** : le microduck (sur rollers) part du sol — à plat ventre ou à plat dos — et se remet **debout sur ses roues**, puis **tient** la station.
|
||||
|
||||
- **Tâche** : `Mjlab-RollerStandUp-Flat-MicroDuck`
|
||||
- **Fichier** : `src/mjlab_microduck/tasks/microduck_roller_standup_env_cfg.py`
|
||||
- **Base** : dérivée de l'env roller (`velocity_rollers`) → même robot, même physique/DR, **même observation 61D** (interchangeable au runtime, chargeable via `--new-cmd-obs`).
|
||||
- **Spec** : `docs/superpowers/specs/2026-08-04-roller-standup-design.md`
|
||||
- **Politique aveugle** : pas de scan de terrain ; proprioception + `projected_gravity`.
|
||||
|
||||
## Hauteurs (mesurées, pas devinées)
|
||||
|
||||
| pose | modèle pieds | modèle rollers |
|
||||
|---|---|---|
|
||||
| debout | 0.1172 → `STAND_Z=0.115` sous charge | 0.1407 → **`ROLLER_STAND_Z=0.138`** |
|
||||
| à plat ventre (repos) | 0.075 | 0.075 |
|
||||
| à plat dos (repos) | 0.048 | 0.048 |
|
||||
|
||||
Les hauteurs de repos au sol sont identiques aux deux modèles : c'est la coque du tronc qui touche, pas les pieds.
|
||||
|
||||
## ⚠️ Indices de joints — les roues sont INTERCALÉES
|
||||
|
||||
```
|
||||
0-4 jambe gauche 5-6 roues gauches
|
||||
7-10 cou / tête 11-15 jambe droite 16-17 roues droites
|
||||
```
|
||||
`_LEG_JOINTS = [0-4, 11-15]`. Les indices du `standup` (`[0-4, 9-13]`) valent pour le modèle **sans** roues et pointeraient sur des roues ici. Verrouillé par `tests/test_roller_standup_cfg.py::test_joint_indices_match_actual_roller_model`.
|
||||
|
||||
## Reset — départ au sol
|
||||
|
||||
`set_random_ground_state` : ventre (`prone_z` 0.076–0.09, plancher relevé car le ventre ne décolle du sol qu'à 0.0752) / dos / **déjà debout** (`standing_z` 0.134–0.144), ± 10° de bruit en pitch/roll. Pas de bucket « assis ». Le bucket « debout » est nécessaire : sans lui la policy monte mais ne tient pas.
|
||||
|
||||
**Curriculum `ground_state_mix`** (easy → hard, le dos en dernier) :
|
||||
|
||||
| iter | debout | ventre | dos |
|
||||
|---|---|---|---|
|
||||
| 0 | 0.50 | 0.50 | 0.00 |
|
||||
| 600 | 0.35 | 0.45 | 0.20 |
|
||||
| 1500 | 0.25 | 0.40 | 0.35 |
|
||||
| 2500 | 0.20 | 0.40 | 0.40 |
|
||||
|
||||
## Récompenses
|
||||
|
||||
Dix termes repris du `standup` avec leurs poids déjà réglés : `pose_stand_legs` (+8), `pose_stand_l1` (+5), `height_stand` (+4, std 0.04), `height_stand_sharp` (+4, std 0.015), `height_stand_l1` (+30), `com_upward_velocity` (+3), `gentle_rise` (−0.02), `upright_linear` (+6), `upright_sharp` (+6), `standing_composite` (+15). Plus `joint_torque_rate_l2` (−2e-3), l'anti-jitter qui n'empêche pas le retournement.
|
||||
|
||||
Régularisateurs hérités : `body_ang_vel` **−0.05** (bloqueur de mouvement, à garder LÉGER), `angular_momentum` −0.02, `action_rate_l2` (rampe −0.4 → −1.0, **pas** le −2.0 du roller), `neck_action_rate_l2` −0.5, `neck_joint_pos_l2` −0.5 (tête droite), `joint_torques_l2` −1e-3, `action_over_limit` −0.5, `self_collisions` −1.0.
|
||||
|
||||
Retirées : toutes les récompenses de patinage, plus `feet_flat` (les lames ne sont pas à plat pendant la montée) et `hip_roll_neutral` (se relever demande d'écarter les jambes).
|
||||
|
||||
## ⚠️ Le point dur : les roues roulent
|
||||
|
||||
Aucune adhérence longitudinale pour pousser sur le sol. Le **curriculum de friction de roulement est INVERSÉ** (l'env roller la fait monter, ici elle descend) :
|
||||
|
||||
| iter | frictionloss | |
|
||||
|---|---|---|
|
||||
| 0 | 0.05 | roues quasi bloquées → se relève comme avec des pieds |
|
||||
| 1000 | 0.02 | |
|
||||
| 2000 | 0.008 | |
|
||||
| 3000 | 0.003 | |
|
||||
| 4000 | 0.0015 | la vraie valeur du roulement |
|
||||
|
||||
**Surveiller `Episode_Reward/standing_composite` aux paliers.** S'il s'écroule, le geste « pieds adhérents » ne transfère pas aux roues libres → il faudra guider une technique de patineur (appui genou intermédiaire, un patin à la fois). C'est un résultat, pas un échec.
|
||||
|
||||
**Surveiller AUSSI la dérive horizontale du robot en play**, à chaque palier de friction. `standing_composite` ne voit ni `root_link_pos_w[:2]` ni la vitesse horizontale : une policy qui se relève en glissant loin de son point de départ collecte exactement le même score qu'une qui se relève et s'arrête. Tant que cette dérive n'a pas été mesurée visuellement, le résultat du curriculum de friction (la question même que cet env existe pour trancher) n'est pas fiable.
|
||||
|
||||
**Sim2real** : seuls les checkpoints d'après iter 4000 sont candidats au déploiement. Avant, la policy s'appuie sur une friction qui n'existe pas sur le vrai robot.
|
||||
|
||||
## Commande
|
||||
|
||||
Slot `twist` neutralisé : `lin_vel_x`/`lin_vel_y` ± 0.01, `ang_vel_z` **± 0.05** (5× plus large — même
|
||||
choix que le `standup`). Slots `head_pose` / `body_pose` **zero-paddés** (convention roller). Déploiement visé : en `--standing` face à la policy roller en `--walking`, avec la bascule automatique sur la magnitude de la commande (`infer_policy.py:262`, seuil 0.05) ; le slot twist y est laissé à zéro (`infer_policy.py:239`).
|
||||
|
||||
**Réserve** : `infer_policy.py` est le script de sim/clavier local. Le runtime robot est le binaire Rust `microduck_runtime`, absent du repo — il n'est pas vérifié qu'il expose un équivalent `--standing`. Le doc de passation du crouch ne liste que `--model`, `--ground-pick`, `--fold-policy`. À confirmer.
|
||||
|
||||
## Terminaisons
|
||||
|
||||
`fell_over` **supprimée** (le robot démarre tombé). `nan_state` héritée. `nan_policy="sanitize"` sur les obs actor/critic.
|
||||
|
||||
## Réseau / PPO
|
||||
|
||||
Actor et critic `(512, 256, 128)` elu, `obs_normalization=True`. PPO `lr=1e-3` adaptive, `desired_kl=0.01`, `gamma=0.99`, `lam=0.95`, `num_steps_per_env=24`, épisode 6 s, `max_iterations=15000`. **Symétrie OFF** (`SYMMETRY_CFG` est câblé pour le layout 51D).
|
||||
|
||||
## Commandes
|
||||
|
||||
```bash
|
||||
uv run train Mjlab-RollerStandUp-Flat-MicroDuck --env.scene.num-envs 4096 --agent.max_iterations 15000
|
||||
uv run scripts/play_latest.py # alias md-play
|
||||
uv run scripts/export_latest.py # alias md-export
|
||||
uv run --with pytest pytest tests/test_roller_standup_cfg.py -q
|
||||
```
|
||||
|
||||
### ⚠️ Voir les départs sur le dos au play
|
||||
|
||||
Un play ne montre **jamais** de départ sur le dos par défaut : l'env de play est
|
||||
reconstruit à neuf, donc `common_step_counter` repart à 0 et le curriculum applique son
|
||||
palier 0, où `face_up_prob = 0`. On ne voit que 50 % ventre / 50 % debout, quelle que soit
|
||||
la maturité du checkpoint chargé. Or le dos est le cas le plus dur, celui qu'on veut
|
||||
justement inspecter.
|
||||
|
||||
`STANDUP_PLAY_FACE_UP` force le mélange (même motif que `SLOPE_PLAY_DIFFICULTY` dans
|
||||
`roller_slope`), **uniquement sur le chemin `play=True`** — l'entraînement et son
|
||||
curriculum easy → hard sont intouchés :
|
||||
|
||||
```bash
|
||||
STANDUP_PLAY_FACE_UP=1.0 md-play # 100 % de départs sur le dos
|
||||
STANDUP_PLAY_FACE_UP=0.4 md-play # le mélange du dernier palier du curriculum
|
||||
STANDUP_PLAY_FACE_UP=none md-play # défaut (palier 0, pas de dos)
|
||||
```
|
||||
|
||||
Le reste (`1 - face_up`) est réparti ventre:debout dans le rapport 2:1 du dernier palier,
|
||||
si bien que `0.4` reproduit exactement le mélange de fin d'entraînement (0.40 / 0.20 / 0.40).
|
||||
|
||||
## 🔧 Correction anti-violence (après premier test robot)
|
||||
|
||||
**Symptômes** sur un checkpoint 4000+ : mouvements très brusques, la tête tape le sol,
|
||||
échec du relevé depuis le dos sur le robot. **Présents en simu aussi** → ce n'était donc
|
||||
ni du sim2real, ni un checkpoint trop jeune, mais la conception des récompenses.
|
||||
|
||||
**Root cause : `gentle_rise` récompensait la violence.** `trunk_vertical_accel_penalty`
|
||||
renvoie déjà `-|a_z|` (`mdp.py:2171`) ; multiplié par le poids **−0.02** hérité du
|
||||
`standup`, ça faisait un double négatif, donc `+0.02·|a_z|` — **plus le tronc accélérait
|
||||
brutalement, plus la policy était payée**. Confirmé par le log : `Episode_Reward/gentle_rise
|
||||
= +0.0118` sur le run `vweolw91`, seul terme de pénalité loggé positif.
|
||||
|
||||
`mdp.py` mélange deux conventions de signe, et c'est le piège :
|
||||
|
||||
| terme | la fonction renvoie | poids correct |
|
||||
|---|---|---|
|
||||
| `height_stand_l1`, `pose_stand_l1`, `gentle_rise` | `-abs(...)`, déjà négatif | **positif** |
|
||||
| `joint_torques_l2`, `joint_torque_rate_l2`, `action_rate_l2`, `body_impact_cost` | magnitude positive | **négatif** |
|
||||
|
||||
Verrouillé par `test_already_negative_penalties_use_positive_weights`.
|
||||
|
||||
⚠️ **Le `standup` du marcheur a exactement le même bug** (même fonction, même poids −0.02).
|
||||
Ça explique la série de tentatives d'amortissement infructueuses documentées dans ses
|
||||
commentaires (« *violent / shaky / overshoot-tip-repeat on the real robot* ») : elles
|
||||
combattaient un terme qui poussait activement dans l'autre sens. **Non corrigé ici** — c'est
|
||||
un autre env, à trancher séparément.
|
||||
|
||||
**Problème structurel associé.** À convergence les récompenses de tâche totalisaient **≈ +41.6**
|
||||
saturées à 95–99 %, contre **≈ −1.2** pour tous les amortisseurs réunis — dont
|
||||
`joint_torque_rate_l2` à **−0.0002/pas** et `joint_torques_l2` à **−0.0001/pas**, soit rien.
|
||||
Rapport ~35:1 : aucune raison d'être doux.
|
||||
|
||||
**État actuel des corrections :**
|
||||
|
||||
| | avant | maintenant | pourquoi |
|
||||
|---|---|---|---|
|
||||
| `gentle_rise` | −0.02 (récompense) | **+0.02** (pénalité) | signe corrigé ; magnitude gardée PETITE exprès — `\|a_z\|` est forcément élevé pendant un retournement, un gros poids serait un bloqueur de mouvement |
|
||||
| `joint_torque_rate_l2` | −2e-3 | **−0.2** | le levier SÛR : pénalise la variation de couple, pas le mouvement |
|
||||
| `head_impact_penalty` | absent | **toujours absent** | essayé à −1.0, a gelé la policy — voir ci-dessous |
|
||||
|
||||
### ⚠️ La pénalité d'impact tête a gelé la policy — ne pas la remettre telle quelle
|
||||
|
||||
Tentative avec les valeurs de `velstand` (`body_impact_cost`, sous-arbre `neck`, −1.0,
|
||||
seuil 2.0) : **la policy a convergé vers rester couchée, inerte.** Mesuré (run `d8rnko6p`) :
|
||||
|
||||
| terme | avant (violent) | avec head_impact (gelé) |
|
||||
|---|---|---|
|
||||
| `standing_composite` | +14.32 | **+3.26** |
|
||||
| `upright_sharp` | +5.76 | +1.06 |
|
||||
| `head_impact_penalty` | — | **−1.01** ← plus gros terme négatif |
|
||||
| `joint_torque_rate_l2` | −0.0002 | −0.255 (donc **pas** le coupable) |
|
||||
|
||||
L'erreur de raisonnement : croire qu'une pénalité « ciblée » ne bride pas le mouvement.
|
||||
**Faux ici — pour se relever du dos, ce robot pivote sur sa tête et ses épaules.** La tête
|
||||
est le point d'appui du retournement, pas un dégât collatéral ; la pénaliser bloque le seul
|
||||
mécanisme disponible, et le dos était déjà le cas qui échouait.
|
||||
|
||||
**L'optimum paresseux qui rend ce gel possible** : `pose_stand_legs` restait à **+7.72 sur 8**
|
||||
alors que le robot était allongé — les jambes sont à HOME en position couchée, donc cette
|
||||
récompense est encaissée quasi gratuitement. C'est `height_stand_l1` (poids +30) qui doit
|
||||
rendre « rester au sol » net négatif ; il ne faut pas l'affaiblir.
|
||||
|
||||
**Hypothèse en cours de test** : taper la tête était un *symptôme* de la violence (le bug de
|
||||
signe payait la brutalité, et une montée brutale finit sur la tête), pas un défaut séparé.
|
||||
Si le slam revient maintenant que le signe est corrigé, la reprise doit être une pénalité
|
||||
**gatée en hauteur** (comme `upright_sharp` l'est), qui épargne la phase de retournement au sol.
|
||||
|
||||
**Leçon de méthode** : les trois corrections ont été appliquées d'un coup, donc le gel n'a pas
|
||||
pu être attribué avec certitude — seul le suspect le plus probable a pu être désigné. Une
|
||||
correction à la fois, à l'avenir.
|
||||
|
||||
**Recalibrage si c'est encore violent** : `|Δτ|²` vaut ~0.1 à convergence, donc la
|
||||
contribution de `joint_torque_rate_l2` ≈ `0.1 × |poids|`. Monter **ce** terme, **pas**
|
||||
`body_ang_vel` (−0.05) ni `action_rate_l2` (rampe → −1.0) : ceux-là sont des bloqueurs de
|
||||
mouvement et le `standup` documente qu'à −0.15 et −1.2 respectivement, ils **gelaient** le
|
||||
relevé depuis le dos. Si au contraire le dos cesse de fonctionner, **baisser**
|
||||
`joint_torque_rate_l2` en premier.
|
||||
|
||||
## Hors périmètre
|
||||
|
||||
Intégrer le relevé dans la policy de roulage (recette `velstand`) ; buckets de départ sur le côté ; variante rough ; pénalités d'impact tronc/tête.
|
||||
|
||||
Aucune récompense ne pénalise la vitesse horizontale du tronc (`root_link_lin_vel_w[:, :2]`) : « se relever en roulant loin » est un résultat non pénalisé et qui score à plein. Décision volontaire (pas un oubli) : une récompense d'immobilité qui ne serait pas gatée en hauteur pénaliserait aussi la translation que le relevé depuis le sol exige physiquement — le mode d'échec « bloqueur de mouvement » que le `standup` documente. Candidat si le problème se confirme : une immobilité gatée en hauteur (proche de `ROLLER_STAND_Z` seulement).
|
||||
@@ -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
@@ -0,0 +1,158 @@
|
||||
# Design — Roller Crouch-Glide (« s'accroupir en glissant » au bouton)
|
||||
|
||||
**Date :** 2026-07-17
|
||||
**Statut :** conception validée, prêt pour le plan d'implémentation
|
||||
|
||||
## Contexte
|
||||
|
||||
Le robot microduck sait patiner (policy roller, tâche `Mjlab-Velocity-Flat-MicroDuck-Rollers`).
|
||||
On veut un nouveau geste : sur un appui bouton, il **s'accroupit et continue de glisser
|
||||
sur son élan** (comme un patineur en position basse), maintient ~1 s, puis **se relève**
|
||||
tout seul et reprend le patinage.
|
||||
|
||||
Contrainte forte de l'utilisatrice : **ne pas modifier le runtime Rust**
|
||||
(`apirrone/microduck_runtime`, installé en binaire). Le geste doit donc réutiliser un
|
||||
mécanisme déjà présent dans le runtime.
|
||||
|
||||
**Découverte clé :** le runtime a déjà un slot « comportement one-shot déclenché au
|
||||
bouton » : `--ground-pick`. Il est déclenché par le **bouton A** (front montant),
|
||||
joue une policy ONNX pilotée par une **phase** pendant une durée fixe, puis revient
|
||||
automatiquement à la policy principale. Surtout, il utilise **exactement le même
|
||||
layout d'observation 61D** que la policy roller — les deux sont interchangeables au
|
||||
runtime. C'est le véhicule idéal, sans une ligne de Rust.
|
||||
|
||||
Compromis accepté : le geste est **one-shot** (durée fixe, pas de « bascule maintenue »).
|
||||
La durée de l'accroupi est fixée par la période du slot.
|
||||
|
||||
## Approche retenue (approche B)
|
||||
|
||||
Créer une **nouvelle tâche mjlab** entraînée sur le robot rollers, qui joue
|
||||
descente → glisse accroupi → remontée, piloté par la phase du slot ground-pick.
|
||||
L'exporter en ONNX et la charger via `--ground-pick`. Aucune modif Rust.
|
||||
|
||||
### Fichiers concernés
|
||||
|
||||
| Fichier | Action |
|
||||
|---|---|
|
||||
| `src/mjlab_microduck/tasks/microduck_roller_crouch_env_cfg.py` | **Nouveau.** L'env, hybride roller + ground-pick. |
|
||||
| `src/mjlab_microduck/tasks/mdp.py` | **Ajout** de la reward `crouch_glide_height_by_phase`. |
|
||||
| `src/mjlab_microduck/tasks/__init__.py` | **Ajout** : enregistrer `Mjlab-RollerCrouch-Flat-MicroDuck`. |
|
||||
|
||||
### Réutilisation (ne rien réinventer)
|
||||
|
||||
- **Physique / robot roller** ← `microduck_velocity_rollers_env_cfg.py` :
|
||||
`MICRODUCK_WALK_ROLLERS_ROBOT_CFG` (14 joints actifs + 4 roues passives),
|
||||
capteur de contact sur les `roller_blade`, DR friction des roulements
|
||||
(`randomize_wheel_friction` + curriculum), obs 14-dim (roues exclues via
|
||||
`SceneEntityCfg("robot", joint_names=(r"^(?!passive_).*",))`), `action.scale=1.0`,
|
||||
`kp_fw=200`.
|
||||
- **Machinerie phase / one-shot** ← `microduck_ground_pick_env_cfg.py` :
|
||||
commande `microduck_mdp.GroundPickPhaseCommand` **réutilisée telle quelle**
|
||||
(produit le `[cos(2πφ), sin(2πφ), 0]` que le runtime enverra dans le slot twist),
|
||||
padding head/body à zéro (`zero_command_padding`), terminaison `robot_state_is_nan`,
|
||||
`reset_action_history`.
|
||||
- **DR sim2real** ← repris du roller env sans changement (IMU misalignment obs-level,
|
||||
encoder bias, masse/inertie, friction BAM, armature, pushes doux ±0.2).
|
||||
|
||||
## Le cœur : cible de hauteur « en trapèze » pilotée par la phase
|
||||
|
||||
Seule vraie nouveauté. Au lieu de descendre la bouche (ground-pick), on pilote la
|
||||
**hauteur du tronc** (`com_height` du `trunk_base`) selon la phase, avec un palier bas :
|
||||
|
||||
```
|
||||
hauteur
|
||||
haute ┐ ┌── debout (rend la main à la policy roller)
|
||||
│ \ /
|
||||
basse│ \_______________/ accroupi + glisse (palier 1 s)
|
||||
└───────────────────────► phase
|
||||
0 0.375 0.625 1
|
||||
```
|
||||
|
||||
- φ ∈ [0, 0.375] : descente vers la hauteur accroupie
|
||||
- φ ∈ [0.375, 0.625] : **maintien accroupi** (= 1 s sur une période de 4 s) → glisse
|
||||
- φ ∈ [0.625, 1.0] : remontée vers la pose roller debout
|
||||
|
||||
**Nouvelle reward `crouch_glide_height_by_phase(env, command_name, height_low,
|
||||
height_high, hold_lo=0.375, hold_hi=0.625, std=...)`** dans `mdp.py` :
|
||||
lit la phase depuis la commande, calcule la hauteur-cible (interpolée haut→bas→haut,
|
||||
plate sur le palier), récompense `exp(-((h_mesurée - h_cible)/std)²)`.
|
||||
S'inspirer des fonctions `com_height_target` (mdp.py:694) et des
|
||||
`interpolated/multistage height target` déjà présentes.
|
||||
|
||||
Valeurs de départ : `height_high ≈ 0.11` m (hauteur roller debout, cf. bande
|
||||
`com_height_target` roller 0.0935–0.1235), `height_low ≈ 0.075` m (accroupi ;
|
||||
à affiner en play). La phase est reconstruite depuis `atan2(sin, cos)` de la commande.
|
||||
|
||||
## Récompenses
|
||||
|
||||
| Reward | Rôle | Origine |
|
||||
|---|---|---|
|
||||
| `crouch_glide_height_by_phase` | Cible principale (haut→bas→haut) | **nouveau** |
|
||||
| `wheel_speed` (poids réduit ~2–3) | Garder l'élan, ne pas freiner pendant l'accroupi | roller env (`wheel_speed_reward`) |
|
||||
| `upright` (≈2), `body_ang_vel` (−0.05), `angular_momentum` (−0.02) | Équilibre / stabilité | roller env |
|
||||
| `return_pose` (fin de phase) | Converger vers la pose roller debout pour rendre la main proprement | adapté de `ground_pick_return_pose` |
|
||||
| `feet_flat` (−2) | Lames à plat → glisse stable | roller env |
|
||||
| `action_rate_l2`, `neck_action_rate_l2`, `joint_torques_l2`, `self_collisions` | Lissage / transfert sim2real | les deux envs |
|
||||
|
||||
**Explicitement PAS inclus :** `braking` (on ne veut pas s'arrêter), `mouth_ground_proximity`
|
||||
/ `mouth_perpendicular_to_ground` (on ne touche pas le sol), `skating_air_time` /
|
||||
`single_support` / `glide` (pas de stride pendant le trick — on glisse passivement).
|
||||
|
||||
## Entraînement
|
||||
|
||||
- `MicroduckRollerCrouchRlCfg` = copie de `MicroduckRollersRlCfg`
|
||||
(MLP 512/256/128, ELU, obs_normalization, PPO, `experiment_name="roller_crouch"`).
|
||||
- Enregistrer dans `tasks/__init__.py` :
|
||||
`register_mjlab_task(task_id="Mjlab-RollerCrouch-Flat-MicroDuck", ...)`.
|
||||
- Lancer :
|
||||
```bash
|
||||
uv run train Mjlab-RollerCrouch-Flat-MicroDuck \
|
||||
--env.scene.num-envs 4096 --agent.max_iterations 8000
|
||||
```
|
||||
- Épisodes démarrés avec une **vitesse d'entrée réaliste** (le robot arrive en roulant),
|
||||
sinon il n'aura pas d'élan à conserver pendant l'accroupi. À câbler via un event de
|
||||
reset (vitesse initiale non nulle) ou un push au début d'épisode.
|
||||
|
||||
## Export + déploiement (flags runtime exacts)
|
||||
|
||||
Export ONNX (le normaliseur est baké par `export.py`), puis :
|
||||
|
||||
```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 roller_crouch.onnx \
|
||||
--ground-pick-period 5.0 \
|
||||
--ground-pick-kp-ratio 1.0 \
|
||||
--ground-pick-action-scale 0.8
|
||||
```
|
||||
|
||||
Bouton **A** → crouch-glide, puis retour auto à la policy roller.
|
||||
|
||||
**Pièges de parité entraînement/déploiement (importants pour le sim2real) :**
|
||||
- `--ground-pick-kp-ratio 1.0` : le défaut est **0.6** (baisse kp à 120 pendant le trick).
|
||||
On entraîne à kp=200 → il faut forcer **1.0** pour que ça corresponde.
|
||||
- `--ground-pick-action-scale` doit matcher l'`action_scale` d'entraînement (0.8 ci-dessus).
|
||||
- `--ground-pick-period 5.0` doit matcher la période/longueur de mouvement entraînée
|
||||
(défaut 4.0, on le garde).
|
||||
|
||||
## Risques et vérification
|
||||
|
||||
- **One-shot, durée fixe :** l'accroupi dure `ground-pick-period` puis remonte tout seul.
|
||||
Pas de maintien libre — limite acceptée de l'approche B.
|
||||
- **Élan pendant le trick :** la phase remplace la commande de vitesse → **pas de poussée
|
||||
active** pendant l'accroupi. Si l'élan d'entrée est trop faible, il ralentit. D'où
|
||||
l'entraînement avec vitesse d'entrée réaliste.
|
||||
- **Vérification :**
|
||||
1. En sim (`play`) : il descend, garde les roues qui tournent pendant le palier,
|
||||
se relève sans tomber, et la pose finale rejoint proprement la pose roller debout.
|
||||
2. Sur le vrai robot : lancer à petite vitesse, appuyer sur A, observer.
|
||||
3. Confirmer que la policy roller reprend la main proprement après le retour.
|
||||
|
||||
## Questions ouvertes / à confirmer pendant l'implémentation
|
||||
|
||||
- Valeur exacte de `height_low` (accroupi) — à régler en play.
|
||||
- Meilleure façon d'injecter la vitesse d'entrée à l'épisode (event reset vs push initial).
|
||||
- Poids relatif `wheel_speed` vs `crouch_glide_height_by_phase` (garder l'élan sans
|
||||
empêcher de s'accroupir).
|
||||
@@ -0,0 +1,145 @@
|
||||
# Mode pente — `roller_slope` (descente passive équilibrée)
|
||||
|
||||
Date : 2026-07-22
|
||||
Statut : design validé, prêt pour le plan d'implémentation.
|
||||
|
||||
## Objectif
|
||||
|
||||
Entraîner une politique dédiée où **microduck (sur rollers) démarre sur du plat
|
||||
avec une petite impulsion vers l'avant, roule jusqu'à une rampe descendante, et
|
||||
se laisse glisser jusqu'en bas en restant debout et équilibré**. Aucun pilotage
|
||||
pendant la descente : le seul objectif de la politique est de **ne pas tomber**.
|
||||
|
||||
La politique doit gérer des rampes de raideur croissante (**~2° → ~20°**) grâce à
|
||||
un curriculum de difficulté.
|
||||
|
||||
## Décisions cadrées (brainstorming)
|
||||
|
||||
| Sujet | Décision |
|
||||
|---|---|
|
||||
| Comportement | Descente passive équilibrée (la gravité fait avancer, pas de pédalage imposé) |
|
||||
| Pilotage | Aucun — équilibre pur, commande `twist` forcée à zéro |
|
||||
| Approche | **A** — tâche dédiée, isolée (comme `roller_crouch`) |
|
||||
| Forme du terrain | **Rampe simple** : plat de départ + rampe descendante (pas de pyramide) |
|
||||
| Scénario épisode | Spawn sur le plat → vitesse d'impulsion vers l'avant → glisse sur la rampe |
|
||||
| Raideur | Curriculum **0/2° → 20°** |
|
||||
| Déploiement | Flag `--slope <onnx>` + touche **`Y`** dans `infer_policy.py` (Y est libre) |
|
||||
|
||||
## Architecture
|
||||
|
||||
### 1. Nouvelle tâche
|
||||
|
||||
Fichier : `src/mjlab_microduck/tasks/microduck_roller_slope_env_cfg.py`, cloné de
|
||||
`microduck_velocity_rollers_env_cfg.py`.
|
||||
|
||||
- Même robot rollers (`MICRODUCK_WALK_ROLLERS_ROBOT_CFG`), même physique, même
|
||||
domain randomization / bruit / délais.
|
||||
- **Même observation 61D** (twist + head/body en zéro-padding) → la politique
|
||||
charge par le chemin `--new-cmd-obs` du runtime et reste interchangeable avec
|
||||
les autres politiques rollers.
|
||||
- Enregistrement dans `src/mjlab_microduck/tasks/__init__.py` via
|
||||
`register_mjlab_task`, avec une config PPO `MicroduckRollerSlopeRlCfg`
|
||||
(`experiment_name`/`run_name` = `roller_slope`).
|
||||
|
||||
### 2. Terrain « plat + rampe » (custom)
|
||||
|
||||
Les terrains inclinés fournis par mjlab sont des pyramides ; on écrit donc un
|
||||
`SubTerrainCfg` dédié (p. ex. `FlatRampTerrainCfg`) dont la méthode
|
||||
`function(difficulty, spec, rng)` construit :
|
||||
|
||||
- une **zone plate de départ** (longueur ~1–2 m) où le robot spawne ;
|
||||
- une **rampe descendante** à la suite, dont l'angle est
|
||||
**interpolé par `difficulty`** sur `[~2°, ~20°]`.
|
||||
|
||||
Le terrain est monté via `TerrainEntityCfg(terrain_type="generator", ...)` avec un
|
||||
`TerrainGeneratorCfg` qui génère plusieurs niveaux de difficulté (donc plusieurs
|
||||
angles de rampe). L'origine de chaque environnement doit tomber **sur la zone
|
||||
plate**, la rampe devant lui.
|
||||
|
||||
> Risque d'implémentation à traiter dans le plan : positionnement de l'origine de
|
||||
> spawn sur le plat (pas au centre de la tuile), et orientation de la rampe pour
|
||||
> que « devant » = « vers le bas ».
|
||||
|
||||
### 3. Commande = aucune
|
||||
|
||||
Slot `twist` neutralisé : `rel_standing_envs = 1.0`, ranges de vitesse à 0,
|
||||
`rel_heading_envs = 0.0`. Head/body restent en zéro-padding. La politique ne
|
||||
reçoit aucune consigne de déplacement.
|
||||
|
||||
### 4. Reset & vitesse d'impulsion
|
||||
|
||||
- `reset_base` : spawn au repos sur le plat, hauteur `z` nominale rollers
|
||||
(~`0.1335–0.1435`, comme le roller env).
|
||||
- **Vitesse d'entrée** injectée via le `velocity_range` de
|
||||
`reset_root_state_uniform` (état propre + range), **pas** via
|
||||
`push_by_setting_velocity` (qui s'additionne à l'état courant et peut faire
|
||||
diverger le free-joint → NaN — leçon déjà apprise sur `roller_crouch`) :
|
||||
`x ≈ (0.2, 0.5) m/s` vers l'avant.
|
||||
- Pushs aléatoires légers conservés pendant l'épisode (robustesse), comme le
|
||||
roller env.
|
||||
|
||||
### 5. Récompenses
|
||||
|
||||
Cœur « rester droit + posture naturelle », anti-optimum-paresseux (éviter qu'il
|
||||
s'écrase au sol pour maximiser la stabilité) :
|
||||
|
||||
- `upright` (tronc vertical) — **principale**
|
||||
- `alive` (bonus de survie par pas)
|
||||
- **pose debout nominale** : récompense vers la pose HOME (mécanique
|
||||
d'interpolation de pose reprise de `roller_crouch`, mais cible fixe = debout),
|
||||
pour garder une stance rollers normale plutôt qu'un accroupi défensif
|
||||
- `feet_flat` (rollers à plat au sol)
|
||||
- `body_ang_vel`, `angular_momentum` (pas de tremblement / vrille)
|
||||
- `action_rate_l2`, `neck_action_rate_l2`, `joint_torques_l2`,
|
||||
`self_collisions` (douceur + sim2real)
|
||||
|
||||
> Pas de récompense de vitesse/frein : la descente est passive. On ne récompense
|
||||
> pas « aller vite », seulement « rester droit en descendant ».
|
||||
|
||||
### 6. Terminaisons
|
||||
|
||||
- **Chute** : `bad_orientation` (tronc trop incliné).
|
||||
- **Bas atteint** : `out_of_terrain_bounds` (le robot est arrivé en bas de la
|
||||
rampe → reset).
|
||||
- `nan_state`, time-out.
|
||||
|
||||
### 7. Curriculum de difficulté (raideur)
|
||||
|
||||
Progression **doux → raide** : commencer sur des rampes quasi plates, augmenter
|
||||
l'angle vers 20° au fur et à mesure des réussites.
|
||||
|
||||
> Risque d'implémentation : le curriculum standard `terrain_levels_vel` promeut
|
||||
> selon la distance parcourue par rapport à la vitesse commandée. Ici la commande
|
||||
> est nulle, donc **il faut un critère de promotion custom** : promouvoir si le
|
||||
> robot a survécu / atteint le bas sans tomber, rétrograder s'il chute tôt.
|
||||
|
||||
### 8. Déploiement — bouton `Y`
|
||||
|
||||
Dans `scripts/infer_policy.py` :
|
||||
|
||||
- nouveau flag `--slope <onnx>` chargeant la politique pente comme session
|
||||
supplémentaire (même schéma que `--walking` / `--standing` / `--ground-pick`) ;
|
||||
- `GLFW_KEY_Y = 89` (aujourd'hui **libre** — la tête est sur `H`) qui **bascule**
|
||||
la session active vers/depuis la politique pente ;
|
||||
- ligne d'aide clavier ajoutée.
|
||||
|
||||
Aucun contrôle existant n'est cassé (contrairement à un partage de la touche `H`
|
||||
du contrôle de la tête).
|
||||
|
||||
## Ce qui n'est PAS dans le périmètre (YAGNI)
|
||||
|
||||
- Pas de pilotage gauche/droite ni de freinage en descente.
|
||||
- Pas de montée de pente ni de traversée.
|
||||
- Pas de pyramide ni de terrains multi-directions.
|
||||
- Pas de fine-tuning depuis les poids roller existants (entraînement from
|
||||
scratch).
|
||||
|
||||
## Livrables
|
||||
|
||||
1. `microduck_roller_slope_env_cfg.py` (env + `FlatRampTerrainCfg` + PPO cfg).
|
||||
2. Enregistrement de la tâche dans `tasks/__init__.py`.
|
||||
3. Récompenses/curriculum custom nécessaires dans `tasks/mdp.py` (pose-debout,
|
||||
promotion de niveau).
|
||||
4. Branchement `--slope` + touche `Y` dans `scripts/infer_policy.py`.
|
||||
5. Tests unitaires pour les fonctions pures (angle de rampe par difficulté,
|
||||
éventuel critère de promotion).
|
||||
@@ -0,0 +1,103 @@
|
||||
# Swizzle roller environment — design
|
||||
|
||||
**Date:** 2026-07-23
|
||||
**Branch:** `new_pre_alpha_rollers`
|
||||
|
||||
## Goal
|
||||
|
||||
A **separate** roller task that produces a **clean classic swizzle**: both blades
|
||||
stay on the ground, the legs spread out and pull back in **symmetrically**
|
||||
(hourglass pattern), propelling the duck forward. This is a simpler, more stable
|
||||
alternative to the alternating stride (`Mjlab-Velocity-Flat-MicroDuck-Rollers`),
|
||||
motivated by the stride not transferring well to the real robot. The stride env is
|
||||
left untouched.
|
||||
|
||||
Sim2real is a target: same robot, observations, command semantics, domain
|
||||
randomization and ONNX export as the stride env, so it **deploys identically**
|
||||
(`microduck_runtime ... --roller`, same flags).
|
||||
|
||||
## Approach (chosen: A — remove anti-swizzle + reward symmetry)
|
||||
|
||||
The base roller velocity recipe *naturally* converges to a swizzle (this is the
|
||||
attractor we fought against for the stride). So the simplest way to a clean swizzle
|
||||
is to **remove the anti-swizzle machinery** and **reward the swizzle's defining
|
||||
features** (symmetry, feet grounded). No phase scripting.
|
||||
|
||||
Rejected: B (explicit hourglass foot-pattern shaping) and C (phase-driven scripted
|
||||
trajectory) — more complex, only needed if A's swizzle looks messy (rhythm/amplitude).
|
||||
|
||||
## Structure
|
||||
|
||||
- New file `src/mjlab_microduck/tasks/microduck_velocity_swizzle_env_cfg.py` with
|
||||
`make_microduck_velocity_swizzle_env_cfg(play=False)` and `MicroduckSwizzleRlCfg`.
|
||||
Built from `make_velocity_env_cfg()` + the roller robot, mirroring
|
||||
`microduck_velocity_rollers_env_cfg.py`'s structure (obs, DR, command, curricula).
|
||||
- Register `Mjlab-Velocity-Swizzle-MicroDuck` in `tasks/__init__.py`.
|
||||
- Reuse everything sim2real from the stride env: robot cfg, 61D obs layout, command
|
||||
(cmd_x push/coast/brake, straight-line: `ang_vel_z=(0,0)`, `heading_hold`), all DR
|
||||
events + curricula (com, wheel_friction), `action_over_limit`, ONNX export path.
|
||||
|
||||
## Reward recipe
|
||||
|
||||
**Kept** (task + stability + sim2real):
|
||||
`wheel_speed` (forward propulsion, the task), `braking`, `upright`, `com_height_target`,
|
||||
`pose`, `forward_lean`, `heading_hold`, `action_over_limit`, `feet_flat`,
|
||||
`self_collisions`, regularizers (`action_rate_l2` + curriculum, `neck_action_rate_l2`,
|
||||
`neck_joint_pos_l2`, `joint_torques_l2`).
|
||||
|
||||
**Removed** (stride / anti-swizzle machinery):
|
||||
`single_support`, `glide`, `skating_air_time`, `gait_symmetry`, `hip_roll_neutral`
|
||||
(the last would fight the swizzle's lateral out-motion).
|
||||
|
||||
**Added** (pro-swizzle):
|
||||
- `leg_symmetry` — reward left/right legs mirroring. The robot uses mirrored L/R
|
||||
sign conventions, so a symmetric config satisfies `q_left + q_right ≈ 0` per pair.
|
||||
Return `-mean_pairs |q_left + q_right|` (L1, constant gradient — same form as the
|
||||
existing `bilateral_symmetry_penalty`) over the leg joint pairs (hip_yaw, hip_roll,
|
||||
hip_pitch, knee, ankle); used with a positive weight so asymmetry is penalised and
|
||||
the symmetric swizzle is favoured. This is the swizzle's defining feature.
|
||||
(Implementation: the existing `bilateral_symmetry_penalty` takes explicit L/R
|
||||
index lists; add a thin wrapper that resolves the L/R leg-joint pairs by name at
|
||||
runtime so it can be configured without hard-coded indices.)
|
||||
- `grounded` — reward both blades in contact (n_contact == 2) while pushing, so the
|
||||
feet stay down (classic swizzle, no lifting). Small weight. New mdp function
|
||||
(mirror of `single_support_reward` but rewarding double support). Gate on
|
||||
`cmd_x >= 0` like the others.
|
||||
|
||||
Leave `hip_roll` pose std loose (as in the stride env) so the legs can spread.
|
||||
|
||||
## New mdp functions (in `tasks/mdp.py`)
|
||||
|
||||
1. `leg_symmetry_reward(env, asset_cfg)` — resolve L/R leg joint pairs by name,
|
||||
return `-mean_pairs |q_left + q_right|` (used with a positive weight).
|
||||
2. `grounded_reward(env, sensor_name, command_name)` — reward exactly-two-blades in
|
||||
contact, scaled by `clamp(cmd_x, 0)`.
|
||||
|
||||
## Command / sim2real (identical to stride)
|
||||
|
||||
`cmd_x` push/coast/brake, `lin_vel_y=0`, `ang_vel_z=(0,0)` (straight-line), full DR
|
||||
(com, head_com, mass/inertia, joint friction, armature, wheel friction, velocity
|
||||
pushes, IMU misalignment, encoder bias, obs delays), 61D obs, `vel_scale=0.3`.
|
||||
Deploys with the same runtime flags as the stride roller policy.
|
||||
|
||||
## PPO config
|
||||
|
||||
Reuse `MicroduckRollersRlCfg`'s hyperparameters (same actor/critic 512-256-128 ELU,
|
||||
PPO settings, `entropy_coef=0.03`), new `experiment_name`/`run_name` = `velocity_swizzle`.
|
||||
|
||||
## Testing / verification
|
||||
|
||||
- Smoke test: `uv run train Mjlab-Velocity-Swizzle-MicroDuck --env.scene.num-envs 16
|
||||
--agent.max-iterations 2` runs without error; `leg_symmetry` and `grounded` appear
|
||||
in the reward log.
|
||||
- Watch on a real run: `leg_symmetry` high (symmetric), `grounded` high (both feet
|
||||
down), `wheel_speed` rising (moves forward). Video: symmetric hourglass swizzle,
|
||||
both blades on the ground.
|
||||
|
||||
## Tuning knobs (post-first-run)
|
||||
|
||||
- If not symmetric enough → raise `leg_symmetry` weight.
|
||||
- If it lifts feet → raise `grounded` weight.
|
||||
- If it barely moves → the symmetry/grounded weights are too high vs `wheel_speed`;
|
||||
lower them.
|
||||
- If the swizzle looks messy (rhythm/amplitude) → escalate to Approach B.
|
||||
@@ -0,0 +1,161 @@
|
||||
# Ground-pick par suivi de pose interpolée par la phase
|
||||
|
||||
**Date** : 2026-07-24
|
||||
**Branche** : `new_pre_alpha_ground_pick`
|
||||
**Fichier cible** : `src/mjlab_microduck/tasks/microduck_ground_pick_env_cfg.py` (réécriture en place)
|
||||
**Task id** : `Mjlab-GroundPick-Flat-MicroDuck` (inchangé)
|
||||
|
||||
## 1. Objectif
|
||||
|
||||
Remplacer l'objectif *espace-tâche* du ground_pick actuel (récompense la bouche
|
||||
qui descend au sol, puis récompense séparément le retour debout) par un objectif
|
||||
**directif de suivi de pose** : on définit deux poses articulaires cibles — STAND
|
||||
et DOWN — et on récompense le suivi de la **pose interpolée par la phase**
|
||||
(STAND→DOWN→STAND).
|
||||
|
||||
Motivation (reprise de l'approche roller_crouch, validée) : l'objectif par pose
|
||||
interpolée est **symétrique par construction** — le « se relever » (cible → STAND)
|
||||
est récompensé exactement comme le « se baisser » (cible → DOWN), ce qui règle le
|
||||
problème d'optimum paresseux où la policy descend mais remonte mal. Le signal est
|
||||
**dense à chaque phase** (cible qui bouge en continu), contrairement à une cible
|
||||
fixe pondérée par `sin` qui ne donne aucun signal aux transitions.
|
||||
|
||||
Le geste reste déclenché au **bouton A** via le slot `--ground-pick` du runtime
|
||||
(one-shot, retour auto à la policy principale). Obs 61D unifié inchangé →
|
||||
policy interchangeable dans le slot.
|
||||
|
||||
## 2. Poses cibles
|
||||
|
||||
Résolution des joints **PAR NOM** (`asset.find_joints([name])`) — robuste, cohérent
|
||||
avec l'approche roller. 14 joints (mouth exclu).
|
||||
|
||||
- **STAND_POSE** = HOME (`default_joint_pos` du modèle). Source du blend ; ne pas
|
||||
la redéfinir en dur — utiliser le défaut du modèle comme source (blend=0).
|
||||
Au déploiement, la policy principale reprend depuis HOME → retour propre.
|
||||
|
||||
- **DOWN_POSE** = valeurs initiales issues du **keyframe FOLD** de `scene_walk.xml`
|
||||
(pli avant profond, tête baissée → bouche vers le sol). Dict par nom en tête de
|
||||
fichier, **commenté comme remplaçable par une lecture `read_pose.py`** du vrai
|
||||
robot posé bouche-au-sol. Valeurs de départ :
|
||||
|
||||
```python
|
||||
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,
|
||||
}
|
||||
```
|
||||
|
||||
## 3. Profil de phase (4 segments)
|
||||
|
||||
Commande `GroundPickPhaseCommand` : `[cos(2πφ), sin(2πφ), 0]`, période **4.0 s**
|
||||
(défaut du slot runtime → pas de flag période à changer au déploiement).
|
||||
|
||||
```
|
||||
DESCENT_END=0.15 HOLD_END=0.50 RISE_END=0.65 (période 4 s)
|
||||
[0, 0.15) descente STAND->DOWN ~0.6 s blend 0->1
|
||||
[0.15, 0.50) bas DOWN ~1.4 s blend 1
|
||||
[0.50, 0.65) remontée DOWN->STAND ~0.6 s blend 1->0
|
||||
[0.65, 1.0) haut STAND (repos) ~1.4 s blend 0
|
||||
```
|
||||
|
||||
`blend ∈ [0,1]` : 0 = STAND (HOME), 1 = DOWN. Cible = `stand + blend·(down - stand)`.
|
||||
Bornes tunables (constantes en tête de fichier).
|
||||
|
||||
**`randomize_phase=False`** : chaque épisode démarre à φ=0 (= debout), comme le
|
||||
déclenchement bouton A au déploiement. Les épisodes se réinitialisant à des
|
||||
instants échelonnés, les envs se décorrèlent naturellement en phase (pas besoin de
|
||||
randomiser). Nécessite d'ajouter un flag `randomize_phase` à
|
||||
`GroundPickPhaseCommandCfg` (défaut `True` → autres tâches sit/stand inchangées),
|
||||
honoré dans `reset()`.
|
||||
|
||||
## 4. Nouvelles fonctions mdp (portées de roller, adaptées, par nom)
|
||||
|
||||
Dans `src/mjlab_microduck/tasks/mdp.py`. Noms distincts du `phase_pose_match`
|
||||
existant (qui est la variante cible-fixe-pondérée-sin) pour éviter la confusion.
|
||||
|
||||
- **`phase_pose_blend(phase, descent_end, hold_end, rise_end) -> Tensor`** — pur,
|
||||
blend 4 segments 0..1 (testable en isolation).
|
||||
- **`_phase_pose_error(env, asset_cfg, command_name, target_pose, descent_end,
|
||||
hold_end, rise_end, source_pose=None) -> (cur, target)`** — résout les joints par
|
||||
nom ; `source_pose` = HOME (`default_joint_pos`) si `None` ; calcule
|
||||
`phase = atan2(sin,cos)/2π % 1`, `blend`, puis `target = source + blend·(target_pose - source)`.
|
||||
- **`phase_pose_track(env, command_name, target_pose, source_pose=None, std=0.3,
|
||||
descent_end, hold_end, rise_end, asset_cfg) -> Tensor`** — gaussienne
|
||||
`exp(-((cur-target)/std)²).mean(-1)`.
|
||||
- **`phase_pose_track_l1(env, ...même args sans std...) -> Tensor`** — bootstrap
|
||||
`-(cur-target).abs().mean(-1)` (gradient constant quand la gaussienne sature).
|
||||
|
||||
`target_pose` = `DOWN_POSE` (dict par nom). `source_pose=None` → HOME.
|
||||
|
||||
## 5. Rewards
|
||||
|
||||
Réécriture minimale par rapport à l'actuel — on remplace la mécanique de retour de
|
||||
pose, on garde la stabilité/régul/sim2real.
|
||||
|
||||
| Reward | Poids | Statut | Rôle |
|
||||
|---|---|---|---|
|
||||
| `phase_pose_track` (std 0.3) | **6.0** | **NOUVEAU** | suivi pose interpolée STAND↔DOWN |
|
||||
| `phase_pose_track_l1` | **2.0** | **NOUVEAU** | bootstrap L1 |
|
||||
| `mouth_ground_proximity` (std 0.10) | **1.0** | retune (était 2.0) | filet : garantit la bouche au sol si DOWN imparfaite ; gaté approche (+sin) |
|
||||
| `upright` | 0.2 | gardé | tronc ~vertical (faible, le robot penche) |
|
||||
| `feet_grounded` | 3.0 | gardé | 2 pieds au sol pendant tout le geste |
|
||||
| `self_collisions` | -1.0 | gardé | |
|
||||
| `head_impact_penalty` (seuil 2 N) | -0.5 | gardé | pas de slam tête (DOWN amène la tête bas) |
|
||||
| `action_rate_l2` | -0.8→-2.0 (curric) | gardé | lissage |
|
||||
| `neck_action_rate_l2` | -1.0 | gardé | |
|
||||
| `joint_torques_l2` | -5e-3 | gardé | |
|
||||
| `body_ang_vel` | -0.05 | gardé | |
|
||||
| `angular_momentum` | -0.02 | gardé | |
|
||||
| `soft_landing` | -1e-5 | gardé | |
|
||||
|
||||
**Retirées** : `mouth_perpendicular_to_ground`, `ground_pick_return_pose_legs`,
|
||||
`ground_pick_return_pose_neck` (remplacées par le suivi de pose).
|
||||
|
||||
Tout le reste **inchangé** : bloc DR (CoM/head-CoM/mass-inertia/friction/armature/
|
||||
IMU-misalign/encoder-bias/pushes), obs 61D + padding head/body zéro, terminaisons
|
||||
(`nan_state`), curricula (`action_rate_weight`, `com_range`, `head_com_range`),
|
||||
RlCfg (`experiment_name="ground_pick"`).
|
||||
|
||||
## 6. Déploiement (parité sim2real)
|
||||
|
||||
```bash
|
||||
microduck_runtime ... \
|
||||
--ground-pick ground_pick.onnx \
|
||||
--ground-pick-period 4.0 \ # = période env (défaut, rien à changer)
|
||||
--ground-pick-kp-ratio 1.0 \ # entraîné kp 200 → forcer 1.0 (défaut 0.6 baisse à 120)
|
||||
--ground-pick-action-scale 1.0 # = action.scale env
|
||||
```
|
||||
|
||||
## 7. Tests
|
||||
|
||||
`tests/` (lancer `uv run --with pytest pytest tests/ -q`) :
|
||||
|
||||
- **Fonctions pures** : `phase_pose_blend` aux points clés
|
||||
(φ=0→0, φ=0.075→0.5, φ=0.3→1, φ=0.575→0.5, φ=0.8→0, monotone par segment) ;
|
||||
`phase_pose_track`/`_l1` : valeur max (cur==target) et signe.
|
||||
- **Construction de l'env** : `make_microduck_ground_pick_env_cfg()` construit ;
|
||||
commande = `GroundPickPhaseCommand` avec `randomize_phase=False`, `period=4.0` ;
|
||||
rewards `phase_pose_track`/`phase_pose_track_l1` présents ;
|
||||
`mouth_perpendicular_to_ground`/`ground_pick_return_pose_*` absents ;
|
||||
`mouth_ground_proximity` présent poids 1.0.
|
||||
|
||||
## 8. Entraînement / play / export
|
||||
|
||||
```bash
|
||||
uv run train Mjlab-GroundPick-Flat-MicroDuck --env.scene.num-envs 4096 --agent.max_iterations 20000
|
||||
uv run scripts/play_latest.py # md-play
|
||||
uv run scripts/export_latest.py # normaliseur baké dans l'ONNX
|
||||
```
|
||||
Surveiller `Episode_Reward/phase_pose_track` (doit monter).
|
||||
|
||||
## 9. Hors scope / notes
|
||||
|
||||
- **Doublon `pose_target_match`** (mdp.py 1577 et 1914) : latent, non traité ici.
|
||||
- **Ajustement DOWN_POSE** : si la bouche ne touche pas assez le sol avec les
|
||||
valeurs FOLD, ajuster le dict (idéalement lecture `read_pose.py` du vrai robot
|
||||
posé bouche-au-sol) plutôt que de gonfler `mouth_ground_proximity`.
|
||||
- **Transition au déploiement** : STAND=HOME = neutre de la policy principale →
|
||||
pas d'à-coup au retour (contrairement au souci noté sur roller où STAND≠HOME).
|
||||
@@ -0,0 +1,202 @@
|
||||
# Spec — Tâche RL « shoot dans une balle » par suivi de poses
|
||||
|
||||
**Date** : 2026-07-24
|
||||
**Branche** : `new_pre_alpha_ground_pick`
|
||||
**Task id** : `Mjlab-Shoot-Flat-MicroDuck`
|
||||
|
||||
## Objectif
|
||||
|
||||
Apprendre un geste de **shoot one-shot** (frappe dans une balle) par **suivi d'une
|
||||
trajectoire de poses articulaires à 4 keyframes** interpolée par la phase :
|
||||
|
||||
```
|
||||
STAND → PIED_ARRIÈRE (armement) → PIED_AVANT (frappe) → STAND (repos)
|
||||
```
|
||||
|
||||
- **Jambe droite** frappe, **jambe gauche** en appui.
|
||||
- **Aucune balle simulée** : on apprend le *geste* par suivi de poses (comme
|
||||
`ground_pick` / crouch). Si une vraie balle est devant le robot au déploiement,
|
||||
elle se fait frapper.
|
||||
- Obs **61D unifiée** identique aux autres policies microduck → l'ONNX exporté se
|
||||
déploie tel quel dans un **slot bouton** du runtime (one-shot : joue le geste
|
||||
puis rend la main à la policy principale).
|
||||
|
||||
Même moule que la tâche `ground_pick` de cette branche (phase encodée `[cos, sin, 0]`
|
||||
dans le slot twist, suivi de pose par phase, obs 61D, DR sim2real héritée de velocity).
|
||||
|
||||
## Non-objectifs (YAGNI)
|
||||
|
||||
- Pas de balle physique, pas de reward de contact/vitesse de balle.
|
||||
- Pas de côté configurable (droite uniquement ; gauche = symétrisable plus tard si besoin).
|
||||
- Pas de marche / récupération de chute : tous les termes de locomotion sont retirés.
|
||||
|
||||
## Architecture
|
||||
|
||||
### Fichier & enregistrement
|
||||
- `src/mjlab_microduck/tasks/microduck_shoot_env_cfg.py`
|
||||
- `make_microduck_shoot_env_cfg(play: bool = False, rough: bool = False) -> ManagerBasedRlEnvCfg`
|
||||
- `MicroduckShootRlCfg` (RslRlOnPolicyRunnerCfg, `experiment_name="shoot"`)
|
||||
- Enregistrement dans `src/mjlab_microduck/tasks/__init__.py` :
|
||||
`Mjlab-Shoot-Flat-MicroDuck` (variante `-Rough-` optionnelle).
|
||||
- Base : hérite de l'env velocity (via `make_velocity_env_cfg` comme ground_pick),
|
||||
puis strip agressif de tout ce qui est locomotion.
|
||||
- Robot : `MICRODUCK_WALK_ROBOT_CFG` (marche standard, 14 joints, pas de rollers).
|
||||
- `action.scale = 1.0`.
|
||||
|
||||
### Poses (placeholders → lues sur le vrai robot via `read_pose.py`)
|
||||
Dicts `{nom_joint: rad}`, **14 joints** (mouth exclu). Sommet du fichier env.
|
||||
- `STAND_POSE` : station neutre (~HOME du sim).
|
||||
- `KICK_BACK_POSE` : hanche droite en **extension arrière** + genou droit fléchi
|
||||
(armement) ; jambe gauche + cou ≈ HOME.
|
||||
- `KICK_FWD_POSE` : hanche droite **fléchie avant** + genou droit tendu (frappe) ;
|
||||
jambe gauche + cou ≈ HOME.
|
||||
|
||||
Placeholders plausibles au départ (ajustables), à remplacer par les lectures réelles.
|
||||
|
||||
### Commande & phase
|
||||
- Réutilise `GroundPickPhaseCommand` : `command = [cos(2π·φ), sin(2π·φ), 0]` dans le
|
||||
slot twist.
|
||||
- **Période** : `SHOOT_PERIOD ≈ 2.5 s` (configurable via `cfg.period`).
|
||||
- **Nouveau flag `randomize_phase`** sur `GroundPickPhaseCommandCfg` /
|
||||
`GroundPickPhaseCommand` :
|
||||
- Défaut `True` (non-cassant : `ground_pick` garde le comportement actuel).
|
||||
- Shoot le met à `False` → `reset()` remet φ=0 au lieu de `rand()`.
|
||||
- Raison : chaque épisode démarre au STAND (état du robot = `default_joint_pos`)
|
||||
avec φ=0 = cible STAND → cohérence état/cible au reset (sinon la policy est
|
||||
sommée d'être instantanément en pose « frappe » depuis une station immobile).
|
||||
- **Invariant de cohérence** : `STAND_POSE` DOIT égaler la pose articulaire de
|
||||
reset du sim (`HOME_FRAME` / `default_joint_pos`, non nulle : hip_pitch ±0.4579,
|
||||
ankle ±0.4530, hip_roll ±0.0873, neck/head_pitch 0.3491). Vérifié par
|
||||
`test_stand_pose_matches_home_standing_pose`. Les placeholders initialement à
|
||||
zéro cassaient cet invariant (corrigé après revue finale).
|
||||
|
||||
### Reset (hauteur debout, pas d'élan)
|
||||
- `reset_base.pose_range.z = (0.12, 0.13)` — **hauteur debout absolue** (le `pos`
|
||||
racine par défaut de `InitialStateCfg` est (0,0,0), donc z de reset = 0.12–0.13 m,
|
||||
pas un offset additif ; valeur identique à l'env velocity qui marche). Pas de chute.
|
||||
- **Pas d'injection de vitesse d'entrée** (shoot debout, contrairement au crouch-glide).
|
||||
|
||||
### Rewards hérités non listés
|
||||
La table ci-dessus n'est pas exhaustive : l'env hérite de velocity quelques
|
||||
régularisateurs génériques de faible poids non spécifiques au shoot —
|
||||
`angular_momentum` (-0.02), `dof_pos_limits` — conservés (stabilité, négligeables).
|
||||
⚠️ `soft_landing` (reward de marche) est **retiré** : il lit le capteur 2-pieds
|
||||
`feet_ground_contact` supprimé au profit du capteur pied gauche → KeyError au 1er step
|
||||
sinon, et il est inerte pour un shoot debout.
|
||||
|
||||
### Gotcha renommage capteur (⚠️)
|
||||
Renommer le capteur pied (`feet_ground_contact` → `left_foot_ground_contact`) casse
|
||||
tout ce que l'héritage velocity/ground_pick référence par ce nom. À traiter :
|
||||
- **obs critic** `foot_air_time`/`foot_contact`/`foot_contact_forces` → repointés vers
|
||||
le capteur pied gauche (le critic garde l'info d'appui ; sinon KeyError à la
|
||||
construction de l'env).
|
||||
- **reward** `soft_landing` → retiré (voir ci-dessus ; sinon KeyError au 1er step).
|
||||
Toujours valider par une construction live + **au moins un `step()`** (le reward
|
||||
manager ne tourne qu'au step), pas seulement le build de cfg ni les tests unitaires.
|
||||
|
||||
### ⚠️ Transfert de poids appris (révision post-1er entraînement)
|
||||
Constat : les poses BACK/FWD relevées **robot tenu à la main (appui bipède)** gardent le
|
||||
CoM **centré entre les deux pieds** (~4-5 cm à l'intérieur du pied gauche) à toutes les
|
||||
phases. Avec `upright` imposé, dès que le pied droit se lève le robot bascule → aucune
|
||||
policy ne peut tenir (géométrique, pas du tuning). Vérifié en sim (CoM vs sites pieds).
|
||||
|
||||
Fix retenu (RL apprend l'équilibre) :
|
||||
- `mdp.com_over_support_foot` : reward gaussien (std 4 cm) tirant la projection du CoM
|
||||
(`root_com_pos_w`) vers le pied d'appui, **gaté** par `mdp.kick_engagement` (0 au repos
|
||||
STAND, 1 pendant la frappe). Poids 3.0.
|
||||
- **suivi de pose scindé** (param `joint_names` sur `kick_pose_track`/`_l1`) :
|
||||
GESTE = jambe droite + cou/tête (std 0.35, serré) ; APPUI = jambe gauche (std 0.9,
|
||||
poids 1.0, **lâche**) → la policy peut adducter/décaler le bassin pour transférer le
|
||||
poids sans que le suivi fige le bassin centré.
|
||||
La table « Équilibre / appui » ci-dessus est donc étendue : `support_leg_pose` (1.0),
|
||||
`com_over_support` (3.0) s'ajoutent, et `kick_pose_track`/`kick_pose_l1` ne portent plus
|
||||
que sur les 9 joints du geste (droite+cou).
|
||||
|
||||
### Objectif : suivi de la pose interpolée par la phase
|
||||
Nouvelle fonction **pure** dans `mdp.py` :
|
||||
```python
|
||||
kick_pose_target(phase, stand, back, forward, windup_end, kick_end, return_end) -> Tensor
|
||||
```
|
||||
Interpole entre les vecteurs de pose selon 4 segments (période normalisée [0,1)) :
|
||||
```
|
||||
[0, windup_end) STAND → BACK (armement, défaut 0.35)
|
||||
[windup_end, kick_end) BACK → FORWARD (frappe sèche, défaut 0.10 = "snap")
|
||||
[kick_end, return_end) FORWARD → STAND (retour, défaut 0.30)
|
||||
[return_end, 1.0) STAND (repos)
|
||||
```
|
||||
Le « snap » vient du segment frappe court : la cible articulaire bouge vite → swing
|
||||
rapide du pied. Les 3 bornes de timing sont paramétrables.
|
||||
|
||||
Résolution des joints **par nom** (`asset.find_joints([name])`) — robuste à l'ordre.
|
||||
|
||||
Rewards de suivi (toujours actifs, symétriques comme crouch) :
|
||||
| Reward | Poids | Rôle |
|
||||
|---|---|---|
|
||||
| `kick_pose_tracking` | 6.0 | suivi gaussien `exp(-((q-cible)/std)²).mean`, std=0.4 |
|
||||
| `kick_pose_l1` | 2.0 | bootstrap L1 (gradient constant tôt) |
|
||||
|
||||
### Équilibre / appui (jambe unique = risque de bascule)
|
||||
| Reward | Poids | Rôle |
|
||||
|---|---|---|
|
||||
| `upright` | 2.0 | tronc vertical |
|
||||
| `support_foot_grounded` (pied gauche) | 6.0 | garder le pied d'appui planté (capteur mono-pied → `found∈{0,1}` → reward∈{0,0.5} après `/2`, donc poids 6.0 ≈ contribution max 3.0) |
|
||||
| `feet_flat` (gauche) | -1.0 | lame gauche à plat |
|
||||
| `self_collisions` | -1.0 | |
|
||||
| `body_ang_vel` | -0.05 | |
|
||||
|
||||
`support_foot_grounded` : réutiliser le mécanisme `feet_grounded_reward` du
|
||||
ground_pick mais restreint au **pied gauche** (capteur de contact sur
|
||||
`left_foot_collision`).
|
||||
|
||||
### Régularisation (allégée vs ground_pick — laisser passer le snap)
|
||||
| Reward | Poids | Rôle |
|
||||
|---|---|---|
|
||||
| `action_rate_l2` | -0.5 | léger : un poids lourd tuerait la frappe rapide |
|
||||
| `neck_action_rate_l2` | -0.5 | tête stable |
|
||||
| `joint_torques_l2` | -1e-3 | |
|
||||
|
||||
**Retirés** (termes de marche) : `track_linear_velocity`, `track_angular_velocity`,
|
||||
`air_time`, `foot_clearance`, `foot_swing_height`, `foot_slip`, `pose`.
|
||||
|
||||
### Observations / déploiement (parité)
|
||||
- Obs **61D identique** à ground_pick/roller : `[gyro(3), projected_gravity(3),
|
||||
joint_pos(14), joint_vel(14), last_action(14), command(13)]` avec les slots
|
||||
head(4)+body(6) **zero-paddés** (`zero_command_padding`).
|
||||
- Même DR sim2real héritée de velocity (CoM, mass/inertia, friction BAM, armature,
|
||||
IMU misalignment obs-level, encoder-bias, pushes ±0.3), termine par NaN guard.
|
||||
- Export ONNX (normaliseur baké) via le script d'export existant.
|
||||
- Déploiement dans un slot phase du runtime, p.ex. :
|
||||
```
|
||||
--ground-pick shoot.onnx --ground-pick-period 2.5 \
|
||||
--ground-pick-kp-ratio 1.0 --ground-pick-action-scale <match>
|
||||
```
|
||||
Bouton → shoot → retour auto à la policy principale.
|
||||
|
||||
## Tests
|
||||
|
||||
- `tests/test_shoot.py` — fonctions pures :
|
||||
- `kick_pose_target` aux keypoints : STAND à φ=0, BACK à `windup_end`,
|
||||
FORWARD à `kick_end`, STAND dans le segment repos ; interpolation à mi-segment ;
|
||||
bornes (chaque composante entre min/max des poses).
|
||||
- Valeurs des rewards `kick_pose_tracking` / `kick_pose_l1` sur cas simples.
|
||||
- `tests/test_shoot_cfg.py` — l'env se construit avec la bonne commande
|
||||
(`GroundPickPhaseCommand`, `randomize_phase=False`, période) et les rewards
|
||||
attendus présents / termes de marche absents.
|
||||
- Lancer : `uv run --with pytest pytest tests/ -q`.
|
||||
|
||||
## Entraînement
|
||||
|
||||
```bash
|
||||
uv run train Mjlab-Shoot-Flat-MicroDuck --env.scene.num-envs 4096 --agent.max_iterations <N>
|
||||
```
|
||||
Surveiller `Episode_Reward/kick_pose_tracking` (doit monter). Play : script play_latest.
|
||||
|
||||
## Points ouverts / à régler à l'entraînement
|
||||
- **Timings** (windup/kick/return) et **période** : défauts snap raisonnables,
|
||||
à ajuster selon la vitesse de pied obtenue et la stabilité.
|
||||
- **Poids `action_rate`** : tension snap vs lissage sim2real ; démarrer léger (-0.5).
|
||||
- **Enrichissement optionnel (non retenu pour v1)** : petit reward de « vitesse du
|
||||
pied droit vers l'avant » gaté sur le segment frappe, pour pousser la puissance
|
||||
sans balle simulée. À ajouter seulement si le suivi de pose seul manque de punch.
|
||||
- **Transitions au déploiement** : si `STAND_POSE` ≠ neutre de la policy principale,
|
||||
léger à-coup au déclenchement/retour (comme noté pour crouch).
|
||||
@@ -0,0 +1,83 @@
|
||||
# Swizzle head control (Y button) — design
|
||||
|
||||
**Date:** 2026-07-27
|
||||
**Branch:** `new_pre_alpha_rollers`
|
||||
**Task touched:** `Mjlab-Velocity-Swizzle-MicroDuck` (`microduck_velocity_swizzle_env_cfg.py`)
|
||||
|
||||
## Goal
|
||||
|
||||
Let the operator move the duck's HEAD to different poses (Y button) while it rollers,
|
||||
**without the swizzle falling apart when the head moves**. The head pose is the
|
||||
physical head/neck joints (look up/down/left/right) — unrelated to `heading_tracking`
|
||||
(the body's travel direction, which is untouched).
|
||||
|
||||
The current swizzle policy would tip over if the head moved as an external offset (it
|
||||
doesn't compensate the CoM shift), so the head must be **policy-managed**: the policy
|
||||
produces the head pose AND keeps its balance. This matches how the walking policy does
|
||||
it in `--new-cmd-obs` mode — the head is a COMMAND injected into the observation, and
|
||||
the policy produces the pose (no external "double-add").
|
||||
|
||||
## Approach (chosen: A — policy-managed head via the obs command)
|
||||
|
||||
Port the head-command machinery that already exists in `microduck_velocity_env_cfg.py`
|
||||
into the swizzle env: feed a real head-pose command into the (currently zero-padded)
|
||||
`head_command` obs slot, reward the head tracking that command, and ramp it in LATE
|
||||
via a curriculum so it doesn't disturb the swizzle.
|
||||
|
||||
No external-offset option (rejected earlier: the swizzle won't stay upright if the head
|
||||
moves without the policy compensating).
|
||||
|
||||
## Changes (all in `make_microduck_velocity_swizzle_env_cfg`)
|
||||
|
||||
1. **Head-pose command term.** Add `cfg.commands["head_pose"] = UniformPoseCommandCfg(...)`,
|
||||
copied from the velocity env: 4D `[neck_pitch, head_pitch, head_yaw, head_roll]`
|
||||
deltas-from-default, `resampling_time_range = (2.0, 5.0)`, per-joint ranges (head_roll
|
||||
tighter, matching the small mechanical range).
|
||||
2. **Real `head_command` obs.** Replace the current `zero_command_padding(dim=4)` head
|
||||
slot with the real command obs `func=<head command obs>, params={"command_name":
|
||||
"head_pose"}`, for BOTH actor and critic. (Keeps the 61D layout; body_command stays
|
||||
zero-padded — no body-pose control here.)
|
||||
3. **`head_pose_tracking` reward.** Add `cfg.rewards["head_pose_tracking"]`
|
||||
(`microduck_mdp.head_pose_tracking`, `command_name="head_pose"`, `std=0.5`), initial
|
||||
weight 0 (curriculum-ramped).
|
||||
4. **Late curriculum.** A `reward_weight` curriculum ramps `head_pose_tracking` from 0
|
||||
→ **4.0**, staying 0 until **~1500 iters** (swizzle solid) then climbing over the
|
||||
next ~1000 iters, mirroring velstand's body-pose kick-in. Plus a head-pose command-
|
||||
range curriculum: start with tight ranges (small head deltas) and widen them over
|
||||
the same window, so the head barely moves early and reaches full range once the
|
||||
policy can handle it. This is what makes head control "not hard to manage" — it is
|
||||
added on top of an already-stable swizzle. (Values are starting points, tunable.)
|
||||
5. **Reconcile the neck penalty (required).** The env currently has `neck_joint_pos_l2`
|
||||
which pulls the neck/head joints toward HOME — it would FIGHT `head_pose_tracking`
|
||||
(which pulls them to the command) so the head would never move. Exclude the
|
||||
head-pose joints from `neck_joint_pos_l2` (or drop it), mirroring the velocity env's
|
||||
handling (its comment: keeping them in both "would pull them to HOME while
|
||||
head_pose_tracking pulls them to the command"). Keep `neck_action_rate_l2` (smoothness,
|
||||
no conflict).
|
||||
|
||||
Everything else (swizzle, backward locomotion, heading curriculum, DR, obs layout,
|
||||
command) is unchanged. Requires **retraining** the swizzle task.
|
||||
|
||||
## Runtime
|
||||
|
||||
No runtime code change. The `microduck_runtime` **Y button** already drives the
|
||||
`head_command` obs slot (new-cmd-obs mode injects the head offset as a command, "don't
|
||||
double-add"). Once the swizzle policy is retrained with head control, it responds to Y.
|
||||
Deploy flags unchanged (`--roller --new-cmd-obs ...`).
|
||||
|
||||
## Testing / verification
|
||||
|
||||
- Smoke test: `uv run train Mjlab-Velocity-Swizzle-MicroDuck --env.scene.num-envs 16
|
||||
--agent.max-iterations 2` runs; `head_pose_tracking` appears in the reward log; the
|
||||
`head_pose` command and real `head_command` obs build without error.
|
||||
- Real run: `head_pose_tracking` rises after the curriculum kick-in; the swizzle stays
|
||||
stable (fall rate does not spike when the head curriculum turns on). In the viewer /
|
||||
on the robot: moving the head command moves the head, and the roller keeps skating.
|
||||
|
||||
## Tuning knobs
|
||||
|
||||
- Head disrupts the swizzle when it kicks in → push the curriculum kick-in later, or
|
||||
widen the head range more slowly.
|
||||
- Head doesn't follow well → raise `head_pose_tracking` target weight, or check the neck
|
||||
penalty still isn't fighting it.
|
||||
- Head too twitchy → keep/raise `neck_action_rate_l2`.
|
||||
@@ -0,0 +1,372 @@
|
||||
# Design — `roller_standup` : se relever sur rollers
|
||||
|
||||
**But** : une policy dédiée qui remet le microduck **debout sur ses rollers** après une chute
|
||||
(à plat ventre ou à plat dos), et qui sait ensuite **tenir** la station sur roues.
|
||||
|
||||
Portage de la recette `standup` (canard marcheur) vers le modèle rollers. Aucune modification
|
||||
des envs existants.
|
||||
|
||||
---
|
||||
|
||||
## Décisions actées
|
||||
|
||||
| Décision | Choix | Alternatives écartées |
|
||||
|---|---|---|
|
||||
| Forme | **Policy dédiée** épisodique | Greffer le relevé sur l'env roller (recette `velstand`) → risque réel de casser la foulée acquise |
|
||||
| Poses de départ | **ventre + dos + debout** | `assis` (n'existe que pour le hand-off depuis la policy `sit`, pas d'équivalent roller) ; côtés (couverture max mais convergence bien plus dure) ; sans `debout` (la policy se relèverait puis retomberait) |
|
||||
| Roues libres | **curriculum de friction de roulement inversé** | Vraie friction d'entrée (bootstrap trop dur) ; imposer une technique de patineur par récompenses (historique du repo : les récompenses de style trop directives créent des optima parasites — le swizzle, l'optimum paresseux du crouch) |
|
||||
| Pose cible | **HOME + hauteur mesurée** | `STAND_POSE` du roller-crouch (signalée comme issue ouverte : ≠ du neutre roller → à-coup au retour) ; pose lue sur le vrai robot (bloque le dev) |
|
||||
| Commande | **twist neutralisé** (≈ 0) | Commande de phase / slot bouton (voir « Déploiement ») ; tête pilotable |
|
||||
|
||||
---
|
||||
|
||||
## Architecture
|
||||
|
||||
**Nouveau fichier** : `src/mjlab_microduck/tasks/microduck_roller_standup_env_cfg.py`
|
||||
- `make_microduck_roller_standup_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg`
|
||||
- `MicroduckRollerStandUpRlCfg` (`experiment_name="roller_standup"`)
|
||||
- Task id : `Mjlab-RollerStandUp-Flat-MicroDuck` (flat uniquement, pas de variante rough)
|
||||
|
||||
**Dérivation** : `cfg = make_microduck_velocity_rollers_env_cfg()`.
|
||||
|
||||
C'est le pattern de `roller_slope` (246 lignes) et non celui de `roller_crouch` (479 lignes, qui
|
||||
repart de `make_velocity_env_cfg()` et recopie tous les blocs de DR). On hérite ainsi sans risque
|
||||
de dérive :
|
||||
|
||||
- le robot `MICRODUCK_WALK_ROLLERS_ROBOT_CFG` (14 joints actifs + 4 roues passives, BAM m6, kp_fw 200) ;
|
||||
- les capteurs `feet_ground_contact` (mode subtree sur `ankle_{l,r}_v1`) et `self_collision` ;
|
||||
- toute la DR : CoM tronc + tête, masse/inertie (pseudo_inertia), friction BAM, armature,
|
||||
biais d'encodeur, désalignement IMU au niveau obs, friction des roulements ;
|
||||
- **l'observation unifiée 61D** `[gyro(3), projected_gravity(3), joint_pos(14), joint_vel(14),
|
||||
last_action(14), command(13)]` — condition dure pour l'interchangeabilité au runtime ;
|
||||
- la termination `nan_state` (garde élargi : joints + free-joint + roues).
|
||||
|
||||
Le modèle rollers **permet physiquement** de s'allonger : `robot_allcollisions_rollers.xml` porte des
|
||||
géoms de collision sur le tronc (`np_f970`), les hanches, les jambes, les coques de tête et la mâchoire,
|
||||
en plus des 4 pneus. Vérifié.
|
||||
|
||||
---
|
||||
|
||||
## Constantes mesurées
|
||||
|
||||
Mesurées par cinématique exacte (minimum des sommets de maillage des géoms collidantes, pose
|
||||
`STAND` du keyframe, tronc ramené au contact) sur `scene_rollers.xml` vs `scene.xml` :
|
||||
|
||||
| pose | modèle pieds | modèle rollers |
|
||||
|---|---|---|
|
||||
| debout (`STAND` = HOME) | 0.1172 | **0.1407** |
|
||||
| à plat ventre (repos) | 0.0752 | 0.0752 |
|
||||
| à plat dos (repos) | 0.0476 | 0.0475 |
|
||||
|
||||
Contrôle de cohérence : le `standup` utilise `STAND_Z = 0.115` mesuré **sous charge** contre 0.1172
|
||||
en cinématique → ~2 mm d'affaissement. On applique la même correction, et le résultat tombe pile dans
|
||||
le `reset_base z = 0.1335–0.1435` déjà utilisé par l'env roller.
|
||||
|
||||
```python
|
||||
ROLLER_STAND_Z = 0.138 # tronc debout sur roues, sous charge (+23 mm vs pieds)
|
||||
ROLLER_PRONE_Z = 0.075 # hauteur de repos à plat ventre
|
||||
EPISODE_LENGTH_S = 6.0
|
||||
```
|
||||
|
||||
Les hauteurs de repos au sol sont **identiques** aux deux modèles (c'est la coque du tronc qui touche,
|
||||
pas les pieds). Cela ne veut pas dire que la plage `prone_z` du `standup` se réutilise telle quelle :
|
||||
voir la note sous « Reset » — `prone_z_min` diverge (0.076 ici, pas 0.05) car une seule plage sert
|
||||
deux poses (ventre, dos) dont les hauteurs de contact au reset ne sont pas les mêmes.
|
||||
|
||||
La grandeur mesurée est bien celle que lisent les récompenses : `height_target_gaussian` et
|
||||
`height_l1_penalty` utilisent `root_link_pos_w[:, 2]`, qui vaut exactement `xpos[trunk_base].z`
|
||||
(le free-joint est sur `trunk_base`) — vérifié numériquement.
|
||||
|
||||
## Indices de joints
|
||||
|
||||
Les roues passives sont **intercalées** dans l'ordre des joints. Ordre réel vérifié dans MuJoCo
|
||||
(`m.jnt_qposadr`, modèle rollers, 18 joints après le free-joint) :
|
||||
|
||||
```
|
||||
0-4 left_hip_yaw, left_hip_roll, left_hip_pitch, left_knee, left_ankle
|
||||
5-6 passive_LF_wheel, passive_LR_wheel
|
||||
7-10 neck_pitch, head_pitch, head_yaw, head_roll
|
||||
11-15 right_hip_yaw, right_hip_roll, right_hip_pitch, right_knee, right_ankle
|
||||
16-17 passive_RF_wheel, passive_RR_wheel
|
||||
```
|
||||
|
||||
```python
|
||||
_LEG_JOINTS = [0, 1, 2, 3, 4, 11, 12, 13, 14, 15] # standup : [0-4, 9-13]
|
||||
_NECK_JOINTS = [7, 8, 9, 10] # standup : [5-8]
|
||||
_WHEEL_JOINTS = [5, 6, 16, 17]
|
||||
```
|
||||
|
||||
Seul `_LEG_JOINTS` est réellement consommé (par les récompenses de pose). `_NECK_JOINTS` et
|
||||
`_WHEEL_JOINTS` sont déclarés pour la documentation et pour le test d'indices : le cou est résolu
|
||||
**par nom** (`neck_joint_pos_l2` appelle `find_joints(r".*(neck|head).*")` à chaque pas, précisément
|
||||
pour être robuste au décalage dû aux roues) et les roues par la regex `^passive_.*`.
|
||||
|
||||
Le doc de passation signale explicitement cette fragilité. Elle est verrouillée par un test qui
|
||||
construit l'env et vérifie les noms de joints à ces indices (voir « Tests »).
|
||||
|
||||
---
|
||||
|
||||
## Récompenses
|
||||
|
||||
### Retirées de l'héritage roller
|
||||
|
||||
| Retiré | Pourquoi |
|
||||
|---|---|
|
||||
| `wheel_speed`, `braking`, `skating_air_time`, `glide`, `single_support`, `gait_symmetry`, `forward_lean`, `heading_hold` | récompenses de foulée : aucun sens quand on est par terre |
|
||||
| `feet_flat` | pendant la montée les lames ne sont pas à plat → cette pénalité combattrait le geste |
|
||||
| `hip_roll_neutral` | se relever demande d'écarter les jambes |
|
||||
| `pose`, `com_height_target` | remplacés par les cibles pose/hauteur ci-dessous |
|
||||
| `upright` (gaussienne de base) | remplacée par `upright_linear` + `upright_sharp` |
|
||||
|
||||
### Gardées de l'héritage roller
|
||||
|
||||
| Reward | Poids | Rôle |
|
||||
|---|---|---|
|
||||
| `action_over_limit` | −0.5 | protection sim2real (sur-commande au-delà des butées), indépendante de la tâche |
|
||||
| `self_collisions` | −1.0 | |
|
||||
| `body_ang_vel` | **−0.05** | volontairement **léger** : le `standup` documente qu'à −0.15 il gelait le relevé (bloqueur de mouvement) |
|
||||
| `angular_momentum` | −0.02 | |
|
||||
| `action_rate_l2` | curriculum −0.4 → −0.8 → −1.0 | l'env roller le met à plat à −1.0 ; on reprend la rampe du `standup` (douce au début → aide le bootstrap du grand mouvement de retournement) |
|
||||
| `neck_action_rate_l2` | −0.5 | tête stable |
|
||||
| `neck_joint_pos_l2` | −0.5 | garder la tête droite (le choix de `roller_slope`) — **remplace** la commande `head_pose` du `standup` |
|
||||
| `joint_torques_l2` | −1e-3 | |
|
||||
|
||||
### Ajoutée
|
||||
|
||||
| Reward | Poids | Rôle |
|
||||
|---|---|---|
|
||||
| `joint_torque_rate_l2` | −2e-3 | anti-jitter : le `standup` l'a identifié comme le seul amortisseur qui ne bloque pas le retournement (il pénalise la *variation* de couple, pas son amplitude ni la rotation du tronc) |
|
||||
|
||||
### Récompenses de relevé (transplant du `standup`, remappé)
|
||||
|
||||
Les dix termes sont copiés **avec leurs poids déjà réglés** par les itérations documentées dans
|
||||
`microduck_standup_env_cfg.py`. Seuls changent les indices de joints et les deux hauteurs.
|
||||
Toutes les fonctions mdp existent déjà — **rien à écrire dans `mdp.py`**.
|
||||
|
||||
| Reward | Fonction mdp | Poids | Paramètres roller | Rôle |
|
||||
|---|---|---|---|---|
|
||||
| `pose_stand_legs` | `pose_target_match` | +8.0 | `std=0.5`, `joint_indices=_LEG_JOINTS`, `target_overrides=None` (HOME) | pose articulaire cible |
|
||||
| `pose_stand_l1` | `pose_l1_penalty` | +5.0 | `joint_indices=_LEG_JOINTS`, `target_overrides=None` | bootstrap L1 : gradient constant même loin de HOME |
|
||||
| `height_stand` | `height_target_gaussian` | +4.0 | `std=0.04`, `target_height=0.138` | gaussienne large → tire depuis le sol |
|
||||
| `height_stand_sharp` | `height_target_gaussian` | +4.0 | `std=0.015`, `target_height=0.138` | gaussienne étroite → force les derniers cm |
|
||||
| `height_stand_l1` | `height_l1_penalty` | +30.0 | `target_height=0.138` | rend « rester par terre » net négatif (sinon optimum paresseux) |
|
||||
| `com_upward_velocity` | `com_upward_velocity` | +3.0 | `max_height=0.148` | paye le *mouvement* de montée (+10 mm de marge au-dessus de la cible, comme 0.125 vs 0.115 chez `standup`) |
|
||||
| `gentle_rise` | `trunk_vertical_accel_penalty` | −0.02 | | pénalise `\|a_z\|` → montée lisse à vitesse constante |
|
||||
| `upright_linear` | `body_upright_linear` | +6.0 | | `cos(tilt)` : fort gradient quand couché |
|
||||
| `upright_sharp` | `upright_gaussian_at_height` | +6.0 | `std=0.3`, `height_low=0.075`, `height_high=0.138` | gaussienne serrée gatée en hauteur → tue le penché-arrière |
|
||||
| `standing_composite` | `standing_composite_score` | +15.0 | `height_std=0.04`, `upright_std=0.40`, `pose_std=0.40`, `target_height=0.138`, `joint_indices=_LEG_JOINTS` | score multiplicatif hauteur × droit × pose |
|
||||
|
||||
Tous les termes prennent `asset_cfg=SceneEntityCfg("robot", body_names=("trunk_base",))` là où le
|
||||
`standup` le fait.
|
||||
|
||||
**Pas de pénalités d'impact** (tronc/tête) pour cette v1 : le `standup` n'en a pas, seul `velstand`
|
||||
en a. On garde le jeu minimal.
|
||||
|
||||
---
|
||||
|
||||
## Observation et commande
|
||||
|
||||
**Observation** : héritée intacte de l'env roller (61D). Aucune modification — c'est la raison de
|
||||
dériver de cet env.
|
||||
|
||||
On ajoute `nan_policy = "sanitize"` sur les groupes actor et critic, comme `roller_slope` : un contact
|
||||
rare fait diverger le free-joint en NaN, l'obs est assainie (→ 0) pour ne pas tuer l'entraînement,
|
||||
et l'env fautif se reset au pas suivant.
|
||||
|
||||
**Commande** : le slot `twist` est neutralisé, exactement comme le `standup` :
|
||||
|
||||
```python
|
||||
command = cfg.commands["twist"]
|
||||
command.rel_standing_envs = 0.0
|
||||
command.rel_heading_envs = 0.0
|
||||
command.heading_command = False
|
||||
command.ranges.heading = None
|
||||
command.resampling_time_range = (EPISODE_LENGTH_S, EPISODE_LENGTH_S * 2)
|
||||
command.debug_vis = False
|
||||
command.ranges.lin_vel_x = (-0.01, 0.01)
|
||||
command.ranges.lin_vel_y = (-0.01, 0.01)
|
||||
command.ranges.ang_vel_z = (-0.05, 0.05)
|
||||
cfg.commands["twist"] = microduck_mdp.VelocityCommandCommandOnlyCfg(**vars(command))
|
||||
```
|
||||
|
||||
Les slots `head_pose` (4) et `body_pose` (6) restent **zero-paddés** — convention de la famille
|
||||
roller (`roller`, `roller_crouch`, `roller_slope`). C'est un écart assumé vis-à-vis du `standup` de
|
||||
la marche, qui pilote la tête via une vraie commande `head_pose` 4D (voir « Risques »).
|
||||
|
||||
Justification du twist neutralisé : dans `scripts/infer_policy.py`, la policy `standup` de la marche
|
||||
est chargée en `--standing` à côté de `--walking`, et la bascule est **automatique sur la magnitude
|
||||
de la commande de vitesse** (`infer_policy.py:262`, seuil 0.05) ; quand `standing` est active, le
|
||||
slot twist est laissé à zéro (`infer_policy.py:239`). Les slots à phase (`ground_pick`, `fold`)
|
||||
servent aux tricks one-shot déclenchés au bouton, pas à un relevé.
|
||||
|
||||
---
|
||||
|
||||
## Reset
|
||||
|
||||
Ajout de l'événement `set_ground_state` (mode `reset`), inséré **après** `reset_base` et
|
||||
`reset_robot_joints` de l'héritage (l'ordre des événements suit l'ordre d'insertion dans le dict) :
|
||||
|
||||
```python
|
||||
cfg.events["set_ground_state"] = EventTermCfg(
|
||||
func=microduck_mdp.set_random_ground_state,
|
||||
mode="reset",
|
||||
params={
|
||||
"face_down_prob": 0.50, # ventre — piloté par le curriculum ci-dessous
|
||||
"face_up_prob": 0.00, # dos — introduit tard (le plus dur)
|
||||
"sitting_prob": 0.00, # pas de bucket assis → aucun override de joint à remapper
|
||||
"standing_prob": 0.50,
|
||||
"prone_z_min": 0.076, # cf. note ci-dessous — pas un simple héritage du standup
|
||||
"prone_z_max": 0.09,
|
||||
"standing_z_min": 0.134, # roller (contre 0.11–0.12 pour les pieds)
|
||||
"standing_z_max": 0.144,
|
||||
"sitting_tilt_max": math.radians(10), # ± bruit de pitch/roll ; s'applique AUSSI au bucket debout
|
||||
},
|
||||
)
|
||||
```
|
||||
|
||||
Note : dans `set_random_ground_state`, le bucket `standing` réutilise le quaternion du bucket
|
||||
`sitting` — donc `sitting_tilt_max` bruite aussi les départs debout, ce qui est voulu.
|
||||
|
||||
**Sur `prone_z_min` = 0.076 (et pas 0.05, valeur reprise à tort du `standup`)** : les poses ventre et
|
||||
dos partagent une seule plage de z, mais leurs hauteurs de contact mesurées diffèrent — ventre
|
||||
0.0752, dos 0.0475 — donc une plage unique ne peut pas être idéale pour les deux. Le commentaire du
|
||||
`standup` justifie son plancher `0.05` par un repos mesuré à ~0.044 **après stabilisation sous
|
||||
gravité** ; or ce qui compte à l'instant du reset, c'est la hauteur de contact en pose HOME, pas la
|
||||
hauteur de repos une fois retombé. À 0.05, le ventre spawn avec la coque du tronc **enfoncée de
|
||||
25 mm dans le sol**, un pushout que la policy paie ensuite via `gentle_rise` /
|
||||
`joint_torque_rate_l2`. `prone_z_min = 0.076` élimine cette interpénétration, au prix d'un dos qui
|
||||
démarre 28–42 mm au-dessus de son repos — un artefact bien plus doux qu'un pushout de contact.
|
||||
|
||||
**Aucune modification de `mdp.py`** : `reset_robot_joints` de la base utilise
|
||||
`joint_names=(".*",)` avec `velocity_range=(0.0, 0.0)` et `default_joint_vel` (HOME_FRAME
|
||||
`joint_vel={".*": 0.0}`) → les 4 roues passives sont déjà remises à zéro à chaque reset. Vérifié.
|
||||
|
||||
**Curriculum `ground_state_mix`** (`event_param_curriculum`), même logique easy → hard que le
|
||||
`standup` : le dos est introduit tard et reçoit le plus d'entraînement à la fin.
|
||||
|
||||
| iter | debout | ventre | dos |
|
||||
|---|---|---|---|
|
||||
| 0 | 0.50 | 0.50 | 0.00 |
|
||||
| 600 | 0.35 | 0.45 | 0.20 |
|
||||
| 1500 | 0.25 | 0.40 | 0.35 |
|
||||
| 2500 | 0.20 | 0.40 | 0.40 |
|
||||
|
||||
(Steps en unités de `common_step_counter` = `iter × 24`.)
|
||||
|
||||
**Poussées** : `push_robot` est hérité de l'env roller (±0.2 m/s, intervalle 3–6 s). On ajoute le
|
||||
curriculum montant du `standup` pour ne pas parasiter le bootstrap : 0 → ±0.08 (iter 500) → ±0.2
|
||||
(iter 1000).
|
||||
|
||||
**Terminations** : suppression de `fell_over` (le robot **démarre** tombé — la termination sur
|
||||
inclinaison n'a pas de sens ici). `nan_state` est hérité et conservé.
|
||||
|
||||
**Terrain** : `plane`. Pas de variante rough pour cette v1 — cohérent avec l'env roller, qui n'a pas
|
||||
de paramètre `rough`.
|
||||
|
||||
---
|
||||
|
||||
## Curriculum de friction de roulement, inversé
|
||||
|
||||
C'est la seule pièce réellement nouvelle du design, et le cœur de la question posée par la tâche :
|
||||
**les roues roulent, il n'y a aucune adhérence longitudinale pour pousser sur le sol.**
|
||||
|
||||
Le mécanisme existe déjà et est hérité (`randomize_wheel_friction` via `dr.dof_frictionloss` sur
|
||||
`^passive_.*` + `wheel_friction_curriculum`). Dans l'env roller il **monte** 0 → 0.0015. Ici on le
|
||||
fait **descendre** :
|
||||
|
||||
| iter | frictionloss | effet |
|
||||
|---|---|---|
|
||||
| 0 | 0.05 | roues quasi bloquées → il se relève comme s'il avait des pieds |
|
||||
| 1000 | 0.02 | |
|
||||
| 2000 | 0.008 | |
|
||||
| 3000 | 0.003 | |
|
||||
| 4000 | 0.0015 | la vraie valeur du roulement (celle de l'env roller) |
|
||||
|
||||
`wheel_friction_curriculum` applique simplement le dernier palier franchi
|
||||
(`if env.common_step_counter > stage["step"]`) — il fonctionne aussi bien en descente qu'en montée.
|
||||
**Zéro code à écrire.**
|
||||
|
||||
**Ce que ce curriculum nous dit** : si `Episode_Reward/standing_composite` s'écroule quand la
|
||||
friction baisse, on a la réponse nette que le geste « pieds adhérents » ne transfère pas aux roues
|
||||
libres, et il faudra guider une technique de patineur (appui genou intermédiaire, un patin à la
|
||||
fois). C'est un résultat exploitable, pas un échec.
|
||||
|
||||
---
|
||||
|
||||
## Réseau et PPO
|
||||
|
||||
Identiques au `standup` : actor et critic `(512, 256, 128)` elu, `obs_normalization=True`
|
||||
(normaliseur baké dans l'ONNX par `export.py`), PPO `lr=1e-3` schedule adaptive, `desired_kl=0.01`,
|
||||
`entropy_coef=0.01`, `gamma=0.99`, `lam=0.95`, `num_steps_per_env=24`, `save_interval=250`,
|
||||
`max_iterations=15_000`. **Symétrie OFF** (`SYMMETRY_CFG` est câblé pour l'ancien layout 51D et casse
|
||||
sur le 61D — même situation que tous les envs v1.5+).
|
||||
|
||||
---
|
||||
|
||||
## Tests
|
||||
|
||||
`tests/test_roller_standup_cfg.py` :
|
||||
|
||||
1. l'env se construit (`play=False` et `play=True`) ;
|
||||
2. **les noms de joints aux indices `_LEG_JOINTS` / `_NECK_JOINTS` / `_WHEEL_JOINTS` sont les bons**
|
||||
(le verrou contre la fragilité des roues intercalées) ;
|
||||
3. les récompenses de relevé attendues sont présentes, les récompenses de patinage absentes
|
||||
(`wheel_speed`, `glide`, `single_support`, `feet_flat`, …) ;
|
||||
4. `fell_over` absent, `nan_state` présent ;
|
||||
5. le curriculum `wheel_friction` est bien **décroissant** et finit à 0.0015 ;
|
||||
6. le curriculum `ground_state_mix` : les probabilités du dernier palier somment à 1 et
|
||||
`face_up_prob` croît de façon monotone ;
|
||||
7. **parité d'obs** : les noms et dimensions des termes actor/critic sont identiques à ceux de
|
||||
`make_microduck_velocity_rollers_env_cfg()` (sinon l'ONNX ne se charge pas dans un slot).
|
||||
|
||||
Lancer : `uv run --with pytest pytest tests/ -q`.
|
||||
|
||||
---
|
||||
|
||||
## Entraînement et déploiement
|
||||
|
||||
```bash
|
||||
uv run train Mjlab-RollerStandUp-Flat-MicroDuck --env.scene.num-envs 4096 --agent.max_iterations 15000
|
||||
```
|
||||
|
||||
Surveiller `Episode_Reward/standing_composite` (doit monter), et surtout son comportement **aux
|
||||
paliers de friction de roulement** (iters 1000/2000/3000/4000).
|
||||
|
||||
Play : `uv run scripts/play_latest.py`. Export : `uv run scripts/export_latest.py`.
|
||||
|
||||
Déploiement visé : la policy en `--standing` face à la policy roller en `--walking`, avec la bascule
|
||||
automatique sur la magnitude de la commande. **Réserve** : `infer_policy.py` est le script de
|
||||
sim/clavier local ; le runtime robot est le binaire Rust `microduck_runtime`, absent de ce repo — il
|
||||
n'est pas vérifié ici qu'il expose un équivalent `--standing` avec la même bascule. Le doc de
|
||||
passation ne liste que `--model`, `--ground-pick`, `--fold-policy`. À confirmer. Cela ne change rien
|
||||
à l'entraînement : si le runtime n'a pas ce slot, la policy reste utilisable dans un slot bouton (la
|
||||
commande y serait une phase au lieu de zéro — ce serait alors le seul point à revoir).
|
||||
|
||||
---
|
||||
|
||||
## Risques et points de vigilance
|
||||
|
||||
1. **Le relevé sur roues libres est peut-être infaisable sans technique dédiée.** C'est le risque
|
||||
principal. Le curriculum de friction est conçu pour trancher cette question de façon lisible
|
||||
plutôt que pour la contourner.
|
||||
2. **Le bucket « dos » est le plus dur.** Le `standup` documente qu'il gelait en « ne rien faire »
|
||||
sur cette pose, et que la cause était les *bloqueurs de mouvement* (`body_ang_vel` élevé,
|
||||
`action_rate` trop fort). Les valeurs reprises ici sont celles de la version « se relève de
|
||||
partout » — ne pas les durcir sans raison.
|
||||
3. **Tête zero-paddée vs commande `head_pose`.** Si la policy est déployée en `--standing` et que
|
||||
quelqu'un actionne les touches de tête, `infer_policy` écrit `cmd[3:7] = head_offset` et la policy
|
||||
voit du hors-distribution. Choix assumé pour rester dans la convention roller ; à revoir si le
|
||||
pilotage de tête pendant le relevé s'avère nécessaire.
|
||||
4. **Frictionloss 0.05 est loin du réel.** Les paliers 0 → 2000 iters produisent une policy qui ne
|
||||
transfère pas ; seuls les checkpoints d'après le dernier palier (iter 4000+) sont candidats au
|
||||
déploiement.
|
||||
|
||||
## Hors périmètre
|
||||
|
||||
- Intégrer le relevé dans la policy de roulage (recette `velstand`) — décision reportée après
|
||||
validation de la faisabilité.
|
||||
- Buckets de départ sur le côté.
|
||||
- Variante rough / terrain accidenté.
|
||||
- Pénalités d'impact tronc/tête.
|
||||
- Toute modification des envs `roller`, `roller_crouch`, `roller_slope`, `standup`, `velstand`, ou
|
||||
de `mdp.py`.
|
||||
@@ -0,0 +1,417 @@
|
||||
# Spec — Env « Spin » (rotation rapide sur place, sur rollers)
|
||||
|
||||
Date : 2026-08-04. Branche : `new_pre_alpha_rollers`.
|
||||
|
||||
> **Amendement (après le premier run)** : le premier run de calibrage (500 it.)
|
||||
> a montré que le robot tombe systématiquement vers 1,16 s, bien avant le
|
||||
> freinage. En réponse, la cible a été réduite de moitié — `SPIN_RATE_MAX`
|
||||
> 6.0 → **3.0 rad/s**, soit **1 tour par cycle au lieu de 2** — et
|
||||
> `spin_stay_in_place` renforcé à **−3.0**, **sans curriculum** de vitesse.
|
||||
> Voir « Résultats de la vérification initiale » pour les preuves et la
|
||||
> configuration actuellement en vigueur.
|
||||
|
||||
## But
|
||||
|
||||
Une nouvelle tâche RL qui apprend au microduck sur rollers à faire un **spin** :
|
||||
~2 tours anti-horaire sur place à ~6 rad/s (360°/s) *(cible initiale ; ramenée
|
||||
à 3 rad/s, voir l'amendement)*, puis arrêt propre debout.
|
||||
Geste **cyclique piloté par une phase**, déployé dans un **slot bouton one-shot**
|
||||
du runtime, comme la tâche `roller_crouch` existante.
|
||||
|
||||
## Décisions cadrées
|
||||
|
||||
| Question | Décision |
|
||||
|---|---|
|
||||
| Support | Sur rollers (`MICRODUCK_WALK_ROLLERS_ROBOT_CFG`, 4 roues passives) |
|
||||
| Pilotage | Slot bouton one-shot, commande = phase `[cos(2πφ), sin(2πφ), 0]` |
|
||||
| Cible | ~6 rad/s, 2 tours, puis freinage jusqu'à l'arrêt (cible initiale ; ramenée à 3 rad/s, voir l'amendement) |
|
||||
| État d'entrée | À l'arrêt **ou** en roulement lent (0 → 0.3 m/s) |
|
||||
| Sens | Gauche uniquement (lacet positif, anti-horaire) |
|
||||
| Approche | Objectif « résultat » (suivi de ω_z) + amorce antisymétrique décroissante |
|
||||
|
||||
**Contrainte runtime** : le slot n'envoie que `[cos, sin, 0]` — aucun canal libre
|
||||
pour le sens de rotation. La policy tourne donc **toujours à gauche**. Une policy
|
||||
miroir pourrait plus tard aller dans un autre slot (bouton B, `--fold-policy`).
|
||||
|
||||
## Mécanique physique visée
|
||||
|
||||
Sur 4 roues passives, la rotation sur place « propre » se fait en **roulement
|
||||
différentiel** : le patin gauche part vers l'arrière, le droit vers l'avant (les
|
||||
roues **roulent**, elles ne patinent pas). C'est un *swizzle antisymétrique* : les
|
||||
jambes font l'inverse l'une de l'autre, au lieu du miroir du swizzle classique.
|
||||
|
||||
Vérification des signes pour une rotation anti-horaire (repère : x avant, y gauche,
|
||||
z haut ; ω_z > 0) : un point à gauche (+y) a pour vitesse `ω ẑ × y ŷ = −ω y x̂`,
|
||||
donc **vers l'arrière**. Les 4 roues tournent positif en marche avant (vérifié par
|
||||
`test_wheel_direction.py`), donc pour un spin anti-horaire :
|
||||
`ω_roues_gauche < 0`, `ω_roues_droite > 0`, soit **`ω_D − ω_G > 0`**.
|
||||
|
||||
## Approche retenue (C) et pourquoi
|
||||
|
||||
Trois approches ont été considérées :
|
||||
|
||||
- **A — objectif « résultat » pur** : on récompense la vitesse de lacet et on laisse
|
||||
PPO trouver le geste. Risque documenté dans ce repo : optimum paresseux /
|
||||
patinage-sautillement au lieu du roulement propre.
|
||||
- **B — objectif « directif » par poses** : deux poses de ciseau interpolées par la
|
||||
phase, comme `roller_crouch`. Marche vite *si* les poses sont bonnes ; or pour le
|
||||
crouch elles étaient **lues sur le vrai robot**, alors qu'ici le geste est inconnu.
|
||||
Il faudrait le composer à la main : cher et risqué (des poses sans couple utile
|
||||
ne produisent rien).
|
||||
- **C — A + amorce antisymétrique décroissante** ← **retenue**. Structure de A, plus
|
||||
deux termes de *shaping* faibles qui injectent la seule connaissance physique
|
||||
certaine (le roulement différentiel), et dont le poids décroît par curriculum pour
|
||||
laisser la policy affiner son propre geste. La **fréquence de pompage reste libre**.
|
||||
|
||||
## Architecture
|
||||
|
||||
**Fichier** : `src/mjlab_microduck/tasks/microduck_spin_env_cfg.py`
|
||||
- factory `make_microduck_spin_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg`
|
||||
- config PPO `MicroduckSpinRlCfg`
|
||||
- task id `Mjlab-Spin-Flat-MicroDuck`, enregistré dans `tasks/__init__.py`
|
||||
|
||||
Clone la structure de `microduck_roller_crouch_env_cfg.py` : robot rollers, obs 61D
|
||||
unifié, DR complète, `action.scale = 1.0`, terrain plat.
|
||||
|
||||
**`ENABLE_SYMMETRY = False`** — obligatoire : l'augmentation de symétrie gauche/droite
|
||||
transformerait un spin à gauche en spin à droite et détruirait l'apprentissage.
|
||||
|
||||
**Commande** : `GroundPickPhaseCommandCfg(period=4.0, randomize_phase=False)`.
|
||||
`period=4.0` est le défaut de `--ground-pick-period` → rien à passer au runtime.
|
||||
`randomize_phase=False` → chaque épisode démarre à φ=0 (debout), comme au déploiement.
|
||||
|
||||
## Enveloppe de phase
|
||||
|
||||
La phase pilote une **vitesse de lacet cible** ω\*(φ), en trapèze sur 4 segments
|
||||
(période 4 s, `SPIN_RATE_MAX = 6.0` rad/s — cible initiale ; ramenée à 3 rad/s,
|
||||
voir l'amendement ; les segments et la période n'ont pas changé) :
|
||||
|
||||
```
|
||||
ACCEL_END = 0.125 [0, 0.125) 0.5 s ω* : 0 → 6 rad/s (lancement, rampe linéaire)
|
||||
HOLD_END = 0.525 [0.125, 0.525) 1.6 s ω* = 6 rad/s (régime)
|
||||
BRAKE_END = 0.650 [0.525, 0.650) 0.5 s ω* : 6 → 0 (freinage, rampe linéaire)
|
||||
1.0 [0.650, 1.0) 1.4 s ω* = 0 (repos debout)
|
||||
```
|
||||
|
||||
*(Les valeurs ω\* = 6 rad/s ci-dessus correspondent à `SPIN_RATE_MAX` = 6.0, la
|
||||
cible initiale ; voir l'amendement pour la valeur en vigueur.)*
|
||||
|
||||
Intégrale sur un cycle : `0.5·3 + 1.6·6 + 0.5·3 = 12.6 rad ≈ 2.0 tours`. ✅
|
||||
*(à `SPIN_RATE_MAX = 6.0`, cible initiale.)* Forme générale : l'intégrale vaut
|
||||
`2.1 × SPIN_RATE_MAX` quel que soit `rate_max` (0.25 + 1.6 + 0.25 = 2.1). Avec la
|
||||
cible en vigueur (3.0 rad/s) : `2.1 × 3.0 = 6.3 rad ≈ 1 tour` par cycle — voir
|
||||
l'amendement.
|
||||
|
||||
Épisode = 20 s = **5 cycles** : le robot répète lancement → régime → freinage → repos
|
||||
cinq fois par épisode. Plus de données par épisode, et le segment « repos » entraîne
|
||||
aussi la sortie propre du trick. **Note (post-run)** : ceci reste vrai
|
||||
géométriquement (20 s / 4 s), mais aucun épisode du run de calibrage n'a survécu
|
||||
au-delà de ~1,16 s, soit une fraction du premier cycle seulement — voir
|
||||
« Résultats de la vérification initiale ».
|
||||
|
||||
**Fonction pure** `spin_rate_by_phase(phase, rate_max, accel_end, hold_end, brake_end)`
|
||||
dans `mdp.py`, à côté de `crouch_pose_blend`. Testable sans simulateur.
|
||||
|
||||
**Porte de shaping** : `gate(φ) = spin_rate_by_phase(φ) / rate_max ∈ [0, 1]`. Vaut 0
|
||||
sur le segment repos → aucune amorce ne pousse au ciseau à ce moment-là, donc le robot
|
||||
revient en station neutre. C'est ce qui donne une sortie de trick propre vers la policy
|
||||
roller.
|
||||
|
||||
## Rewards
|
||||
|
||||
### Pièges vérifiés dans mjlab (à traiter explicitement)
|
||||
|
||||
- `body_ang_vel` (`body_angular_velocity_penalty`) ne pénalise que **x/y**
|
||||
(`ang_vel_xy`, commentaire « Don't penalize z-angular velocity ») → **gardée**
|
||||
(poids −0.05) : elle réprime le ballant roulis/tangage sans gêner le spin.
|
||||
- `angular_momentum` (`angular_momentum_penalty`) pénalise la **norme 3D** du moment
|
||||
angulaire → elle combattrait directement le spin. **Supprimée.**
|
||||
|
||||
### Nouvelles rewards (à écrire dans `mdp.py`)
|
||||
|
||||
| Reward | Poids | Définition |
|
||||
|---|---|---|
|
||||
| `spin_rate_track` | 6.0 | `exp(−((ω_z − ω*(φ))/std)²)`, `std = 1.5` rad/s. ω_z = lacet du tronc en repère corps (ce que voit l'IMU). Objectif principal. |
|
||||
| `spin_rate_l1` | 0.5 | `−|ω_z − ω*(φ)|` : bootstrap à gradient constant quand la gaussienne sature loin de la cible (même astuce que `crouch_glide_pose_l1`) |
|
||||
| `spin_stay_in_place` | −3.0 (initialement −1.0, voir l'amendement) | `‖v_xy‖²` du tronc → « sur place », et tue l'élan d'entrée. Pas d'état de référence, donc robuste aux 5 cycles par épisode |
|
||||
| `spin_wheel_differential` | 1.0 | `gate(φ) · tanh(clamp(ω_D − ω_G, min=0) / omega_scale)` avec `ω_G = (LF+LR)/2`, `ω_D = (RF+RR)/2` : récompense les patins qui roulent en sens opposés cohérents avec l'anti-horaire → tourner **en roulement**, pas en patinage. Roues résolues par nom (`passive_LF_?wheel`, …). `omega_scale = 17.0` rad/s en vigueur (voir le paragraphe de calibrage ci-dessous) |
|
||||
| `leg_antisymmetry` | 1.0 → 0.25 | `gate(φ) · (−mean|q_G − q_D|)` sur `hip_pitch` et `knee`. ⚠️ convention miroir : une pose *symétrique* donne `q_G + q_D ≈ 0`, donc le **ciseau** c'est `q_G ≈ q_D`. Décroît par curriculum |
|
||||
| `spin_grounded` | 0.5 | `gate(φ) · 1[n_contact ≥ 2]` : les deux lames au sol, empêche « je saute et je vrille en l'air ». La `grounded_reward` du swizzle n'est pas réutilisable telle quelle (elle se pondère par `cmd_x`, qui vaut ici `cos(2πφ)`) |
|
||||
|
||||
**Calibrage de `omega_scale`** (échelle de saturation du tanh) : au régime visé,
|
||||
chaque patin avance à `v = ω_z · demi_voie`, donc chaque roue tourne à
|
||||
`v / r` avec `r = 0.0175` m, et le différentiel vaut `2 · ω_z · demi_voie / r`.
|
||||
Les racines de jambe sont à `y = ±0.0175` m dans le modèle rollers, mais les patins
|
||||
sont plus écartés (offset de cheville) : la demi-voie réelle est à **mesurer sur les
|
||||
sites `left_foot` / `right_foot` dans le sim** au premier run. Avec une demi-voie
|
||||
estimée à ~0.03 m et `ω_z = 6` rad/s, le différentiel attendu était ~20 rad/s — d'où
|
||||
le défaut initial `omega_scale = 20.0`. **Mesure faite (Task 3) : demi-voie réelle
|
||||
= 0.0499 m, différentiel attendu = 34.2 rad/s, soit 71 % au-dessus de l'estimation
|
||||
— au-delà du seuil de 30 % fixé par le plan.** `SPIN_WHEEL_OMEGA_SCALE` a donc été
|
||||
corrigé à **34.0** (valeur intermédiaire, en vigueur tant que la cible était à
|
||||
6 rad/s ; recalibrée depuis à **17.0**, voir le paragraphe « Mise à jour »
|
||||
juste en dessous). Voir la section « Résultats de la vérification initiale »
|
||||
ci-dessous pour le détail de la mesure de demi-voie.
|
||||
|
||||
**Mise à jour (fix wave post-review)** : `SPIN_RATE_MAX` a été réduit de 6.0 à
|
||||
**3.0 rad/s** (décision humaine, sans curriculum — voir plus bas). Conséquence
|
||||
mécanique directe sur `omega_scale`, pas un choix indépendant : le différentiel
|
||||
attendu au régime redevient `2 · 3.0 · 0.0499 / 0.0175` = **17.1 rad/s**. Laisser
|
||||
`omega_scale = 34.0` plafonnerait le terme à `tanh(17.1/34) = 0.47` de son propre
|
||||
maximum, ce qui affaiblirait exactement le shaping que l'on cherche à renforcer.
|
||||
`SPIN_WHEEL_OMEGA_SCALE` est donc recorrigé à **17.0**, avec la même demi-voie
|
||||
mesurée (0.0499 m) conservée comme référence.
|
||||
|
||||
### Rewards reprises de `roller_crouch` (stabilité / sim2real)
|
||||
|
||||
| Reward | Poids |
|
||||
|---|---|
|
||||
| `upright` (tronc vertical) | 2.0 |
|
||||
| `feet_flat` (lames à plat) | −2.0 |
|
||||
| `self_collisions` | −1.0 |
|
||||
| `body_ang_vel` (xy seulement) | −0.05 |
|
||||
| `action_rate_l2` | −1.0 (curriculum −0.5 → −1.0) |
|
||||
| `neck_action_rate_l2` | −0.5 |
|
||||
| `joint_torques_l2` | −1e-3 |
|
||||
| `neck_joint_pos_l2` **hors `head_yaw`** | −0.2 |
|
||||
|
||||
**La tête** : tangage/roulis de la nuque tenus près du neutre (sim2real), mais
|
||||
`head_yaw` **exclu** du terme → libre de servir de volant d'inertie pour lancer la
|
||||
rotation. Implémentation : `neck_joint_pos_l2` résout ses joints par regex
|
||||
`.*(neck|head).*` en dur ; il faut donc soit ajouter un paramètre de regex à cette
|
||||
fonction, soit écrire une variante `neck_joint_pos_l2_no_yaw`. Choix : **ajouter un
|
||||
paramètre `pattern`** à `neck_joint_pos_l2` (défaut inchangé) pour ne pas dupliquer.
|
||||
|
||||
## Reset / état d'entrée
|
||||
|
||||
```python
|
||||
cfg.events["reset_base"].params["pose_range"]["z"] = (0.1335, 0.1435)
|
||||
cfg.events["reset_base"].params["velocity_range"] = {"x": (0.0, 0.3)}
|
||||
```
|
||||
|
||||
Injection via `reset_root_state_uniform`. **Jamais** `push_by_setting_velocity` en
|
||||
`mode="reset"` : c'est ce qui avait produit les NaN sur le crouch (`root_vel +=` sur
|
||||
une vitesse racine potentiellement divergente → le free-joint de la base explose).
|
||||
|
||||
## Domain randomization
|
||||
|
||||
Identique à `roller_crouch`, sans dévier (recette sim2real validée du repo) : COM
|
||||
tronc + tête, masse/inertie, friction articulaire BAM, armature, friction roues,
|
||||
pushes 0.2 m/s toutes les 3–6 s, désalignement IMU 6°, biais d'encodeurs ±0.015 rad.
|
||||
|
||||
## Observations
|
||||
|
||||
Layout **61D à l'identique** de roller / ground_pick / crouch — condition pour que
|
||||
l'ONNX charge dans le slot :
|
||||
`[gyro(3), projected_gravity(3), joint_pos(14), joint_vel(14), last_action(14), command(13)]`
|
||||
avec `command = [twist(3), head_pose(4), body_pose(6)]`, head/body zero-paddés.
|
||||
|
||||
Donc : retrait de `base_lin_vel` de l'actor (gardé côté critic), retrait des
|
||||
`height_scan` et `foot_height`, `wheel_vel` côté critic, joints passifs exclus des
|
||||
termes `joint_pos`/`joint_vel`, délais et bruits identiques au crouch.
|
||||
|
||||
Le gyro est dans l'obs → la policy **observe** son propre ω_z : la tâche est observable.
|
||||
|
||||
## Terminations
|
||||
|
||||
`time_out`, `fell_over`, `out_of_terrain_bounds` (héritées) + `nan_state`
|
||||
(`microduck_mdp.robot_state_is_nan`), comme le crouch.
|
||||
|
||||
## Curriculum
|
||||
|
||||
| Terme | Étapes |
|
||||
|---|---|
|
||||
| `action_rate_weight` | −0.5 (0) → −0.8 (250 it.) → −1.0 (500 it.) |
|
||||
| `leg_antisym_weight` | 1.0 (0) → 0.5 (1500 it.) → 0.25 (3000 it.) |
|
||||
| `com_range` | 0.003 → 0.005 (500 it.) → 0.01 (1000 it.) |
|
||||
| `head_com_range` | 0.003 → 0.005 (500 it.) → 0.01 (1000 it.) |
|
||||
|
||||
(itérations × 24 pas/env, comme les autres envs)
|
||||
|
||||
**Pas de curriculum sur la vitesse cible** : 6 rad/s d'emblée *(cible initiale ;
|
||||
ramenée à 3 rad/s, toujours sans curriculum, voir l'amendement)*. Voir « Plan B ».
|
||||
|
||||
## PPO
|
||||
|
||||
`MicroduckSpinRlCfg` = copie de `MicroduckRollerCrouchRlCfg` : actor/critic
|
||||
(512, 256, 128) elu, obs normalization, PPO adaptatif lr 1e-3, `desired_kl=0.01`,
|
||||
`num_steps_per_env=24`, `symmetry_cfg=None`, `experiment_name="spin"`,
|
||||
`run_name="spin"`, `max_iterations=8000`.
|
||||
|
||||
## Tests
|
||||
|
||||
`tests/test_spin.py` — fonctions pures, sans simulateur :
|
||||
- `spin_rate_by_phase` : valeurs aux bornes des 4 segments (0, rate_max, rate_max, 0, 0)
|
||||
- monotonie croissante sur la rampe de lancement, décroissante sur le freinage
|
||||
- **intégrale sur un cycle ≈ 4π** à `rate_max = 6.0` (garantit la **forme** du
|
||||
trapèze, `2.1 × rate_max` rad par cycle) — ne protège plus la cible en vigueur
|
||||
depuis l'amendement, cf. bullet suivant. Valeur exacte de l'enveloppe : 12.6 rad
|
||||
contre 4π = 12.566 → tolérance 1 %
|
||||
- **la cible réellement expédiée** (`mdp.SPIN_RATE_MAX`) intègre bien à
|
||||
`2.1 × SPIN_RATE_MAX` rad par cycle, quel que soit `rate_max` — ajouté en
|
||||
7d916aa, c'est ce test qui échoue si la cible change sans qu'on ait réfléchi au
|
||||
nombre de tours. Avec la valeur en vigueur (3.0 rad/s) : 6.3 rad ≈ 1 tour
|
||||
- `gate(φ) = 0` sur tout le segment repos, `∈ [0,1]` partout
|
||||
|
||||
`tests/test_spin_cfg.py` — l'env se construit :
|
||||
- commande = `GroundPickPhaseCommand`, `period == 4.0`, `randomize_phase is False`
|
||||
- `"angular_momentum" not in cfg.rewards` (le piège de la section rewards)
|
||||
- `symmetry_cfg is None`
|
||||
- dimension de l'obs actor == 61
|
||||
- **parité exacte de l'ordre des termes d'observation** (actor + critic) avec
|
||||
`roller_crouch`, groupe par groupe — ajouté en 7d916aa, condition stricte pour
|
||||
que l'ONNX exporté charge dans le slot du runtime
|
||||
|
||||
Lancer : `uv run --with pytest pytest tests/ -q`
|
||||
|
||||
## Entraînement / déploiement
|
||||
|
||||
```bash
|
||||
uv run train Mjlab-Spin-Flat-MicroDuck --env.scene.num-envs 4096 --agent.max_iterations 8000
|
||||
# surveiller Episode_Reward/spin_rate_track (doit monter)
|
||||
uv run scripts/play_latest.py # alias md-play
|
||||
uv run scripts/export_latest.py # ONNX, normaliseur d'obs baké
|
||||
```
|
||||
|
||||
```bash
|
||||
microduck_runtime --variant pre-alpha --new-cmd-obs --roller \
|
||||
--model output.onnx --new-dxl-imu --kp 200 --action-scale 0.8 \
|
||||
--ground-pick spin.onnx \
|
||||
--ground-pick-period 4.0 \ # = SPIN_PERIOD
|
||||
--ground-pick-kp-ratio 1.0 \ # défaut 0.6 -> forcer 1.0 (entraîné kp 200)
|
||||
--ground-pick-action-scale 0.8 # matcher action_scale runtime
|
||||
```
|
||||
|
||||
Bouton **A** → spin, puis retour auto à la policy roller.
|
||||
|
||||
## Critère de succès
|
||||
|
||||
En play : ~2 tours anti-horaire en ~2.6 s, dérive du tronc < ~10 cm, robot debout tout
|
||||
du long, station neutre stable pendant le segment repos avant le cycle suivant.
|
||||
*(Critère formulé pour la cible initiale de 6 rad/s / 2 tours ; à 3 rad/s, voir
|
||||
l'amendement, ce serait ~1 tour sur la durée du régime — critère non révisé, le
|
||||
robot ne tenant pas encore jusque-là.)*
|
||||
|
||||
## Plan B si l'entraînement plafonne
|
||||
|
||||
Dans l'ordre :
|
||||
1. **Curriculum de vitesse** : `SPIN_RATE_MAX` 3 → 6 rad/s (nécessite de rendre
|
||||
`rate_max` pilotable par un `CurriculumTermCfg` sur les params de reward).
|
||||
**Partiellement suivi** : suite au run de calibrage, la cible a bien été
|
||||
abaissée à 3 rad/s (voir l'amendement), mais **sans curriculum** — 3 rad/s
|
||||
est pour l'instant une cible fixe, pas un point de départ ramping vers 6.
|
||||
L'humain a choisi de voir d'abord ce que le robot parvient à faire à cette
|
||||
vitesse avant d'envisager une remontée graduelle.
|
||||
2. Monter `spin_wheel_differential` et retarder la décroissance de `leg_antisymmetry`.
|
||||
3. Élargir `std` de `spin_rate_track` (1.5 → 2.5) pour un gradient utile plus loin.
|
||||
4. En dernier recours, basculer sur l'approche B (poses de ciseau composées à la main
|
||||
dans un pose editor) pour amorcer le geste, puis relâcher.
|
||||
|
||||
## Hors périmètre
|
||||
|
||||
- Spin à droite (policy miroir dans un autre slot) — plus tard.
|
||||
- Variante à pied (sans rollers).
|
||||
- Spin commandé en vitesse continue (nécessiterait un canal de commande runtime).
|
||||
|
||||
## Résultats de la vérification initiale
|
||||
|
||||
### Demi-voie mesurée et `omega_scale`
|
||||
|
||||
La demi-voie a été mesurée sur les sites `left_foot` / `right_foot` du modèle
|
||||
rollers : **0.0499 m**, contre l'estimation de 0.03 m du spec. Différentiel de
|
||||
roues attendu au régime (6 rad/s) : `2 · 6.0 · 0.0499 / 0.0175` = **34.2 rad/s**,
|
||||
soit 71 % au-dessus du défaut 20.0 — au-delà du seuil de 30 % fixé par le plan.
|
||||
`SPIN_WHEEL_OMEGA_SCALE` a donc été changé de 20.0 à **34.0**. Les tests continuent
|
||||
de passer `omega_scale=20.0` explicitement, pour rester indépendants de la
|
||||
constante.
|
||||
|
||||
### Smoke run (Step 2 : 5 itérations, 64 envs, garde NaN)
|
||||
|
||||
Terminé sans exception. `Episode_Termination/nan_state` est resté à 0.0000 sur
|
||||
toute la durée, et `/tmp/mjlab/nan_dumps/` n'a jamais été créé. Les six rewards
|
||||
spin apparaissent bien dans les clés `Episode_Reward/` loggées : `spin_rate_track`,
|
||||
`spin_rate_l1`, `spin_stay_in_place`, `spin_wheel_differential`, `spin_grounded`,
|
||||
`leg_antisymmetry`.
|
||||
|
||||
Parité d'observation (Step 1) : la liste des termes de l'obs actor de l'env spin
|
||||
est **identique** à celle de `roller_crouch` — 8 termes, même ordre :
|
||||
`base_ang_vel, projected_gravity, joint_pos, joint_vel, actions, command,
|
||||
head_command, body_command`. C'est la condition pour que l'ONNX exporté charge
|
||||
dans le slot du runtime.
|
||||
|
||||
**Note d'usage à retenir** : la commande d'exemple du plan avec `--enable-nan-guard`
|
||||
en flag nu est rejetée par le CLI de ce repo — il faut passer
|
||||
`--enable-nan-guard True`.
|
||||
|
||||
### Run de calibrage 500 itérations (Step 3)
|
||||
|
||||
4096 envs, 500 itérations, ~2,32 s/itération, code de sortie 0, logger wandb (donc
|
||||
`scripts/play_latest.py` / `md-play` retrouve le run).
|
||||
|
||||
**Ce qui a réellement été établi** : `Mean episode length` = **57.83 pas** sur un
|
||||
épisode de 1000 pas (20 s à 50 Hz), soit **~1,16 s**. `Episode_Termination/fell_over`
|
||||
≈ **70**, `time_out = 0.0000`, `nan_state = 0`. Le robot **tombe à chaque épisode**,
|
||||
à une phase φ ≈ 0,29 — en plein milieu du segment de régime. Il n'atteint jamais le
|
||||
freinage (φ ≥ 0,525) ni le repos (φ ≥ 0,650) : **71 % du cycle n'est jamais
|
||||
entraîné**.
|
||||
|
||||
La longueur d'épisode est passée de 23,98 à 57,83 pas sur la durée du run : la
|
||||
montée de `Episode_Reward/spin_rate_track` (0,0291 → 0,3168) reflète donc
|
||||
principalement une **survie qui s'allonge**, pas un suivi qui s'améliore. Le
|
||||
critère de succès de cette étape tel qu'énoncé dans le plan (« la courbe doit
|
||||
monter ») **n'est pas un signal valide** pour ce terme : un robot totalement
|
||||
immobile score déjà `6.0 × 0.405 = 2.43` dessus — le segment de repos paie plein
|
||||
tarif pour rester debout sans bouger, donc toute policy qui survit plus longtemps
|
||||
capte mécaniquement plus de ce segment-là, indépendamment de la qualité du suivi.
|
||||
|
||||
### Diagnostic dérivé — estimations, pas des mesures directes
|
||||
|
||||
Les valeurs ci-dessous viennent du rapport entre termes de reward dans le dernier
|
||||
bloc de log, ce qui annule le facteur de normalisation inconnu appliqué par le
|
||||
logger. À prendre comme des estimations, reproductibles à partir de la même
|
||||
méthode :
|
||||
|
||||
**Ce qui tient** : pendant les ~1,2 s où il reste debout, le robot suit la cible
|
||||
d'assez près. Rapport `spin_rate_l1 / spin_rate_track` (−0,0097 / 0,3168, poids 0,5
|
||||
et 6,0, `std = 1.5`), en résolvant `e = 0.3674 · exp(−(e/1.5)²)` : erreur moyenne
|
||||
absolue de suivi de vitesse de lacet ≈ **0,35 rad/s**, confirmée par deux voies
|
||||
indépendantes — ce ratio `spin_rate_l1 / spin_rate_track`, et un calcul inverse à
|
||||
partir de la normalisation du reward manager. Il **peut lancer** le spin ; il **ne
|
||||
peut pas rester debout** en le faisant.
|
||||
|
||||
**Ce qui ne tient pas** : le bloc de shaping (`spin_wheel_differential` 1,0,
|
||||
`spin_grounded` 0,5, `spin_stay_in_place` −1,0) totalise ~1,0 de poids contre 6,0
|
||||
pour l'objectif principal — environ **13 %** de ce qu'une policy en patinage
|
||||
renoncerait à gagner en ignorant ce bloc. Et `spin_wheel_differential` est
|
||||
**invariant au centre instantané de rotation** : un spin centré à 6 rad/s et un
|
||||
pivot sur le patin gauche à 6 rad/s produisent tous les deux un différentiel de
|
||||
34,2 — ce terme n'encode donc **pas** le roulement centré, seul
|
||||
`spin_stay_in_place` le fait. `spin_stay_in_place` ≈ −0,0069 implique
|
||||
`‖v_xy‖ ≈ 0,35 m/s` : le robot est encore en translation, cohérent avec un pivot
|
||||
excentré (patin comme pivot) plutôt qu'une rotation autour du centre du corps.
|
||||
|
||||
### Changement de configuration décidé suite à ce diagnostic
|
||||
|
||||
Cible réduite de moitié — `SPIN_RATE_MAX` 6.0 → **3.0 rad/s** — et
|
||||
`spin_stay_in_place` renforcé −1.0 → **−3.0** (voir le tableau des rewards et
|
||||
`SPIN_WHEEL_OMEGA_SCALE` recalibré à 17.0 plus haut). **Délibérément sans
|
||||
curriculum** sur la vitesse cible : c'est un premier essai pour voir ce que le
|
||||
robot parvient à faire à vitesse moitié, avant d'envisager une remontée graduelle
|
||||
si besoin.
|
||||
|
||||
**Atténuation du coût de dérive pendant le lancement.** Renforcer
|
||||
`spin_stay_in_place` à −3.0 a rendu plus aigu un défaut relevé par la revue : ce
|
||||
terme était le seul du spin à ne pas être modulé par la phase, donc il facturait à
|
||||
plein tarif la translation transitoire pendant la rampe de lancement — précisément
|
||||
le moment où le robot doit pousser au sol pour s'injecter du moment angulaire, et
|
||||
où l'élan d'entrée (jusqu'à 0.3 m/s) doit être **converti** en rotation. Le coût
|
||||
est désormais multiplié par `SPIN_LAUNCH_DRIFT_SCALE = 0.2` sur `[0, ACCEL_END)`
|
||||
et vaut plein tarif ensuite. Il n'est volontairement **pas** éteint pendant le
|
||||
repos, contrairement aux amorces : c'est là que l'immobilité est le vrai critère.
|
||||
|
||||
L'étape 4 (regarder le geste) reste à faire, réservée à l'humain.
|
||||
|
||||
⚠️ Ces quatre tests (trois nouveaux sur l'atténuation, un modifié) n'ont **pas**
|
||||
été exécutés — la machine était réservée à autre chose au moment du commit. À
|
||||
lancer avant tout run long : `uv run --with pytest pytest tests/test_spin.py
|
||||
tests/test_spin_cfg.py -q`.
|
||||
Reference in New Issue
Block a user