diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml new file mode 100644 index 0000000..a402b81 --- /dev/null +++ b/.github/workflows/tests.yml @@ -0,0 +1,20 @@ +name: tests + +on: + push: + branches: [main] + pull_request: + +jobs: + test: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: "3.11" + cache: pip + - name: Install dependencies + run: pip install -e ".[mujoco,pybullet,dev]" + - name: Run tests + run: pytest -v diff --git a/.gitignore b/.gitignore index 748df4c..5140822 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,15 @@ -.claude -./.claude \ No newline at end of file +# Python +__pycache__/ +*.py[cod] +*.egg-info/ +.venv/ +venv/ +.pytest_cache/ + +# Simulator logs +MUJOCO_LOG.TXT + +# Editors / tools +.vscode/ +.idea/ +.claude/ diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..fd857eb --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2026 Pedro H. Peres + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/MUJOCO_LOG.TXT b/MUJOCO_LOG.TXT deleted file mode 100644 index 1ac58d3..0000000 --- a/MUJOCO_LOG.TXT +++ /dev/null @@ -1,255 +0,0 @@ -Fri May 15 08:25:55 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 0.0080. - -Fri May 15 08:26:13 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 1. The simulation is unstable. Time = 0.0060. - -Fri May 15 08:27:24 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 0.0080. - -Fri May 15 08:34:42 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 6. The simulation is unstable. Time = 0.0180. - -Fri May 15 08:36:42 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 16. The simulation is unstable. Time = 0.0100. - -Fri May 15 08:37:58 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 14. The simulation is unstable. Time = 0.0100. - -Fri May 15 08:39:43 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 3.4420. - -Fri May 15 08:42:04 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 14. The simulation is unstable. Time = 0.0280. - -Fri May 15 08:42:54 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 0.0480. - -Fri May 15 09:25:48 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 14. The simulation is unstable. Time = 0.0180. - -Fri May 15 16:23:58 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 16. The simulation is unstable. Time = 0.0320. - -Fri May 15 16:35:21 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 7. The simulation is unstable. Time = 0.0400. - -Fri May 15 16:38:55 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 7. The simulation is unstable. Time = 0.0400. - -Fri May 15 16:41:35 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 0.6000. - -Fri May 15 16:44:42 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 4.9800. - -Fri May 15 16:44:48 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 13. The simulation is unstable. Time = 0.0860. - -Fri May 15 16:44:49 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 7. The simulation is unstable. Time = 0.1680. - -Fri May 15 16:44:51 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 22. The simulation is unstable. Time = 0.1920. - -Fri May 15 16:45:10 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 4.9800. - -Fri May 15 16:54:07 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 23. The simulation is unstable. Time = 2.4580. - -Fri May 15 16:54:17 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 23. The simulation is unstable. Time = 2.4580. - -Fri May 15 16:54:51 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 2.6300. - -Fri May 15 16:55:04 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 2. The simulation is unstable. Time = 2.7300. - -Fri May 15 16:56:12 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 23. The simulation is unstable. Time = 2.4580. - -Fri May 15 16:57:36 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 4.1720. - -Fri May 15 16:57:50 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 4.1720. - -Fri May 15 16:58:01 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 2.5320. - -Fri May 15 17:20:24 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 2.5320. - -Fri May 15 17:49:12 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 2.7920. - -Fri May 15 17:56:44 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 23. The simulation is unstable. Time = 2.0360. - -Fri May 15 17:56:56 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 23. The simulation is unstable. Time = 2.0360. - -Fri May 15 18:41:47 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 23. The simulation is unstable. Time = 2.0360. - -Sat May 16 10:14:26 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 2.8020. - -Sat May 16 10:15:25 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 2.8020. - -Sat May 16 10:15:35 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 2.8020. - -Sat May 16 12:51:49 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 16. The simulation is unstable. Time = 2.7180. - -Sat May 16 12:58:27 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 23. The simulation is unstable. Time = 2.8980. - -Sat May 16 13:04:06 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 3.4080. - -Sat May 16 13:11:12 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 17. The simulation is unstable. Time = 4.4920. - -Sat May 16 13:11:22 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 17. The simulation is unstable. Time = 4.4920. - -Sat May 16 13:12:41 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 3.0880. - -Sat May 16 13:13:38 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 13. The simulation is unstable. Time = 3.1920. - -Sat May 16 13:13:46 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 3.1800. - -Sat May 16 13:14:17 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 3.1800. - -Sat May 16 13:17:27 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 6. The simulation is unstable. Time = 3.4660. - -Sun May 17 04:30:54 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 3.1000. - -Sun May 17 04:31:06 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 3.1000. - -Sun May 17 04:31:33 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 3.1720. - -Sun May 17 04:35:30 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 7. The simulation is unstable. Time = 3.8580. - -Sun May 17 04:35:39 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 7. The simulation is unstable. Time = 3.8580. - -Sun May 17 04:36:08 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 7. The simulation is unstable. Time = 4.0240. - -Sun May 17 04:42:03 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 7. The simulation is unstable. Time = 3.1340. - -Sun May 17 04:44:02 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 3.1180. - -Sun May 17 04:45:55 2026 -WARNING: Nan, Inf or huge value in CTRL at ACTUATOR 1. The simulation is unstable. Time = 3.0000. - -Sun May 17 04:46:05 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 6. The simulation is unstable. Time = 3.1620. - -Sun May 17 04:46:20 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 24. The simulation is unstable. Time = 3.2480. - -Sun May 17 04:47:11 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 3.2400. - -Sun May 17 04:47:22 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 3.1980. - -Sun May 17 05:04:47 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 10. The simulation is unstable. Time = 3.9620. - -Sun May 17 05:08:29 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 6. The simulation is unstable. Time = 8.2400. - -Sun May 17 05:13:49 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 13. The simulation is unstable. Time = 8.0400. - -Sun May 17 05:16:10 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 8.0400. - -Sun May 17 05:26:28 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 8.0460. - -Sun May 17 10:45:58 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 8.0460. - -Sun May 17 10:51:30 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 8.5820. - -Sun May 17 10:54:30 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 11. The simulation is unstable. Time = 9.2420. - -Sun May 17 10:55:06 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 5. The simulation is unstable. Time = 8.2700. - -Sun May 17 10:56:05 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 14. The simulation is unstable. Time = 8.2740. - -Mon May 18 11:52:31 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 14. The simulation is unstable. Time = 8.2740. - -Mon May 18 11:55:08 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 8.3380. - -Mon May 18 11:57:57 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 0.0180. - -Mon May 18 12:11:08 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 0.0200. - -Mon May 18 12:11:32 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 0.0200. - -Mon May 18 12:22:01 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 23. The simulation is unstable. Time = 0.0360. - -Mon May 18 13:41:42 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 6. The simulation is unstable. Time = 5.5980. - -Mon May 18 13:45:38 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 16. The simulation is unstable. Time = 1.0180. - -Mon May 18 13:45:53 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 16. The simulation is unstable. Time = 1.0180. - -Mon May 18 13:47:38 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.5160. - -Mon May 18 13:49:51 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.5160. - -Tue May 19 17:50:39 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 6. The simulation is unstable. Time = 2.9180. - -Tue May 19 17:53:55 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 6. The simulation is unstable. Time = 1.5300. - -Tue May 19 18:00:48 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 11. The simulation is unstable. Time = 1.6500. - -Tue May 19 18:04:10 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 0.5700. - -Tue May 19 19:05:18 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 1.1920. - -Tue May 19 19:07:29 2026 -WARNING: Nan, Inf or huge value in QACC at DOF 6. The simulation is unstable. Time = 1.4740. - diff --git a/README.md b/README.md index 45dc8d0..91e1457 100644 --- a/README.md +++ b/README.md @@ -1,99 +1,143 @@ # humanoidMotion-predictiveControl -Simulações de caminhada bípede para robôs humanoides usando cinemática inversa analítica e controle preditivo (MPC). O projeto tem dois ambientes: um em PyBullet com o robô **Aurea** (URDF próprio), e um mais recente em MuJoCo com o **Robotis OP3**. +[![tests](https://github.com/PedroH-Peres/humanoidMotion-predictiveControl/actions/workflows/tests.yml/badge.svg)](https://github.com/PedroH-Peres/humanoidMotion-predictiveControl/actions/workflows/tests.yml) +[![License: MIT](https://img.shields.io/badge/License-MIT-blue.svg)](LICENSE) ---- +Simulações de caminhada bípede para robôs humanoides, feitas para estudo: **Pêndulo Invertido Linear (LIPM)**, **ZMP**, **controle preditivo (MPC)** e **cinemática inversa analítica**, em código curto e comentado. -## Estrutura +| Simulação | Robô | Gerador de caminhada | Simulador | +|-----------|------|----------------------|-----------| +| [`mujoco_sim/`](mujoco_sim/) | Robotis OP3 (modelo oficial) | Analítico (LIPM em forma fechada) | MuJoCo | +| [`pybullet_sim/`](pybullet_sim/) | Aurea (URDF próprio) | MPC do CoM (QP com restrição de ZMP) | PyBullet | -``` -├── pybullet_sim/ # Simulação do Aurea — MPC + IK analítica -└── mujoco_sim/ # Simulação do OP3 — caminhada analítica - ├── AnalyticalWalking/ - └── Utils/ -``` +![Trajetórias geradas pelo motor analítico](docs/media/analytical_walk_forward.png) + +> 📖 **Teoria:** [`docs/theory.md`](docs/theory.md) explica cada modelo e aponta onde ele está no código. + +
+English summary + +Bipedal walking simulations built for learning: Linear Inverted Pendulum (LIPM), ZMP, model predictive control (MPC) and analytical inverse kinematics, in short, commented Python. + +- `mujoco_sim/`: Robotis OP3 in MuJoCo with an analytical LIPM walking pattern generator and 6-DoF analytical leg IK. Run `python -m mujoco_sim` and drive it with the arrow keys. +- `pybullet_sim/`: custom Aurea robot in PyBullet with a CoM planner based on a ZMP-constrained MPC (cvxpy + OSQP). Run `python pybullet_sim/main.py`. +- `docs/theory.md` (Portuguese) derives every model and links each equation to the code. Code and comments are in English. + +
--- -## pybullet_sim +## Instalação -Caminhada controlada por MPC com geração de trajetória do CoM (Linear Inverted Pendulum Model). A IK é analítica relativa ao CoM — os pés são posicionados no espaço do corpo, não no mundo. +Requer Python 3.10+. As duas simulações são independentes; instale só o que for usar: -### Dependências +```bash +git clone https://github.com/PedroH-Peres/humanoidMotion-predictiveControl.git +cd humanoidMotion-predictiveControl +python -m venv .venv && source .venv/bin/activate # Windows: .venv\Scripts\activate +pip install -e ".[mujoco]" # simulação MuJoCo +pip install -e ".[pybullet]" # simulação PyBullet +pip install -e ".[tools,dev]" # gráficos e testes ``` -pip install pybullet numpy scipy -``` -### Como rodar +--- + +## mujoco_sim: OP3 com caminhada analítica + +Física completa (contato, gravidade, atuadores PD) com o modelo oficial do Robotis OP3. O `robot_descriptions` baixa o MJCF automaticamente na primeira execução. ```bash -cd pybullet_sim -python main.py +python -m mujoco_sim # da raiz do repositório ``` -O robô passa por uma pose inicial de estabilização e em seguida começa a caminhar com `cmd_vx = 0.1 m/s`. Para mudar a velocidade, edite essa variável em `main.py`. +> No **macOS**, o viewer passivo do MuJoCo precisa ser iniciado com `mjpython -m mujoco_sim`. ---- +Com a **janela do viewer em foco**: + +| Tecla | Comando | +|-------|---------| +| `↑` / `↓` | Andar para frente / para trás | +| `←` / `→` | Passo lateral para a esquerda / direita | +| `PgUp` / `PgDn` | Girar para a esquerda / direita | +| `Home` | Marchar no lugar | +| `Delete` | Parar (termina o passo atual) | -## mujoco_sim +Todas as letras já são atalhos do viewer do MuJoCo (wireframe, sombras, contatos…), por isso os comandos ficam nas setas e nas teclas de navegação. Para sair, feche a janela. -Simulação com física completa (contato, torques, amortecimento). Usa o modelo oficial do Robotis OP3 via `robot_descriptions`. Suporta dois modos de caminhada, configurados em `mujoco_sim/AnalyticalWalking/config.py`: +### Configuração -| Flag | Efeito | -|------|--------| -| `PHYSICS = True` | Roda `mj_step` com atuadores PD — física real | -| `PHYSICS = False` | Roda `mj_forward` — só cinemática, sem contato | -| `USE_MPC = True` | Motor de caminhada baseado em MPC | -| `USE_MPC = False` | Motor de caminhada analítico (puro) | +Tudo em [`mujoco_sim/config.py`](mujoco_sim/config.py): -### Dependências +| Parâmetro | Efeito | +|-----------|--------| +| `PHYSICS` | `True`: `mj_step` com física real. `False`: só cinemática (útil para depurar o gerador e o IK) | +| `USE_TORSO_CTRL`, `TORSO_CTRL_PARAMS` | Liga e ajusta o controlador PD que corrige o quadril a partir da inclinação do torso (IMU simulada) | +| `WALK_PARAMS` | Período do passo `T`, altura do torso `z_com`, altura do pé `z_step`, fração de apoio duplo `ds_ratio`, separação dos pés `y_sep` | +| `KEY_COMMANDS` | Velocidades associadas às teclas | +| `ACTUATOR_KP/KV/FORCE_RANGE` | Ganhos e limite de torque dos atuadores | + +### Código ``` -pip install mujoco robot_descriptions numpy +mujoco_sim/ +├── sim.py # modelo MuJoCo, laço de simulação, teclado +├── config.py # todos os parâmetros +├── analytical_walking/ +│ ├── trajectory_generator.py # LIPM em forma fechada, pé em balanço, planejamento de passos +│ └── walking_engine.py # máquina de estados da caminhada +├── kinematics/ +│ └── ik_solver.py # IK analítica 6-DoF da perna do OP3 +└── control/ + └── torso_controller.py # IMU simulada + PD de orientação do torso ``` -> O `robot_descriptions` baixa o MJCF do OP3 automaticamente na primeira execução. +--- + +## pybullet_sim: Aurea com MPC -### Como rodar +O CoM é planejado por um MPC que resolve dois QPs (eixos x e y) a cada 20 ms (LIPM discretizado + ZMP dentro do pé). A IK é relativa ao torso: os pés são posicionados no referencial do corpo, não do mundo. ```bash -# da raiz do projeto -python -m mujoco_sim +python pybullet_sim/main.py # GUI, 0,1 m/s +python pybullet_sim/main.py --vx 0.05 # outra velocidade +python pybullet_sim/main.py --headless --duration 10 # sem GUI ``` -Uma janela do viewer MuJoCo abre. Com o **terminal em foco** (não a janela do viewer), use as teclas: +O robô passa por uma pose inicial de estabilização e depois começa a andar. -| Tecla | Comando | -|-------|---------| -| `W` | Andar para frente | -| `S` | Passo lateral | -| `R` | Rotação no lugar | -| `M` | Marcha estacionária | -| `X` | Parar | -| `Q` | Sair | - -> **Atenção:** o input de teclado usa `msvcrt` (Windows). No Linux/macOS o polling de tecla não funciona — o robô executa a pose padrão (marcha no lugar) sem comandos interativos. - -### Parâmetros de caminhada - -Tudo em `mujoco_sim/AnalyticalWalking/config.py`: - -```python -WALK_PARAMS = { - "T": 0.4, # período de um passo [s] - "z_com": 0.22, # altura do torso [m] - "z_step": 0.04, # altura máxima do pé no ar [m] - "ds_ratio": 0.12, # fração em apoio duplo - "y_sep": 0.054, # separação lateral dos pés [m] -} +``` +pybullet_sim/ +├── main.py # laço de simulação, MPC → IK → motores +├── mpc_planner.py # QP do MPC (cvxpy/OSQP) e plano de passos +├── kinematics.py # IK analítica relativa ao torso +└── assets/aurea/ # URDF, meshes e export do SolidWorks +``` + +> ⚠️ O MPC roda em **malha aberta** e há um descompasso conhecido entre o relógio do MPC e o da física. Veja [Limitações conhecidas](docs/theory.md#limitações-conhecidas). + +--- + +## Ferramentas e testes + +```bash +python tools/plot_trajectories.py --vx 0.05 # CoM, ZMP e pés do gerador analítico +python tools/plot_trajectories.py --vy 0.03 --vx 0 --save lateral.png +python tools/inspect_urdf.py # juntas e offsets do URDF do Aurea + +pytest # todos os testes +pytest -m "not slow" # pula as simulações físicas completas ``` -Os ganhos dos atuadores (`ACTUATOR_KP`, `ACTUATOR_KV`, `ACTUATOR_FORCE_RANGE`) também ficam no mesmo arquivo. +Os testes conferem propriedades da trajetória (continuidade, pé nunca abaixo do chão, condições de contorno do LIPM), o IK contra a cinemática direta do MuJoCo, as restrições do MPC e uma caminhada completa sem queda. Eles rodam no GitHub Actions a cada push. --- -## Requisitos gerais +## Créditos + +- O gerador de trajetória analítico é um port em Python do `trajectory_generator.cpp` do pacote ROS2 **aurea_walk**, com duas correções no perfil de pouso do pé (veja os comentários no código). +- Modelo do OP3: [MuJoCo Menagerie](https://github.com/google-deepmind/mujoco_menagerie/tree/main/robotis_op3), via [robot_descriptions](https://github.com/robot-descriptions/robot_descriptions.py). + +## Licença -- Python 3.10+ -- As duas sims são independentes — instale só o que for usar +[MIT](LICENSE). Use, modifique e redistribua à vontade, mantendo o aviso de copyright. diff --git a/docs/media/analytical_walk_forward.png b/docs/media/analytical_walk_forward.png new file mode 100644 index 0000000..4e7c40d Binary files /dev/null and b/docs/media/analytical_walk_forward.png differ diff --git a/docs/theory.md b/docs/theory.md new file mode 100644 index 0000000..38fdf5c --- /dev/null +++ b/docs/theory.md @@ -0,0 +1,163 @@ +# Teoria + +Este documento explica os modelos usados no código e aponta onde cada equação está implementada. A ideia é poder ler a teoria e o código lado a lado. + +- [1. Pêndulo Invertido Linear (LIPM)](#1-pêndulo-invertido-linear-lipm) +- [2. Zero Moment Point (ZMP)](#2-zero-moment-point-zmp) +- [3. Gerador analítico de caminhada (MuJoCo)](#3-gerador-analítico-de-caminhada-mujoco) +- [4. MPC do CoM (PyBullet)](#4-mpc-do-com-pybullet) +- [5. Cinemática inversa analítica da perna](#5-cinemática-inversa-analítica-da-perna) +- [6. Do alvo cinemático ao torque](#6-do-alvo-cinemático-ao-torque) +- [Limitações conhecidas](#limitações-conhecidas) +- [Referências](#referências) + +--- + +## 1. Pêndulo Invertido Linear (LIPM) + +Um humanoide tem dezenas de graus de liberdade, mas para planejar o equilíbrio basta um modelo bem mais simples: toda a massa concentrada no centro de massa (CoM), apoiada num ponto do chão por uma "perna" sem massa e de comprimento variável. Se o CoM é mantido a uma **altura constante** $z_c$, a dinâmica horizontal fica **linear** e desacoplada entre x e y: + +$$ +\ddot{x} = \omega^2 \,(x - p_x), \qquad \ddot{y} = \omega^2 \,(y - p_y), \qquad \omega = \sqrt{g / z_c} +$$ + +onde $(p_x, p_y)$ é o ZMP (próxima seção). Intuição: o CoM é "empurrado para longe" do ponto de apoio, e com mais força quanto mais longe estiver. Por isso andar é uma sequência de quedas controladas: a cada passo o ponto de apoio é colocado de forma a frear e redirecionar essa queda. + +| Onde | O quê | +|------|-------| +| [`trajectory_generator.py`](../mujoco_sim/analytical_walking/trajectory_generator.py) | `lam = np.sqrt(g / z_com)`, solução fechada | +| [`mpc_planner.py`](../pybullet_sim/mpc_planner.py) | `self.omega`, dinâmica discretizada nas restrições do QP | + +## 2. Zero Moment Point (ZMP) + +O ZMP é o ponto do chão onde o momento horizontal resultante das forças de contato é nulo, ou seja, o "centro de pressão" sob os pés. A condição física de equilíbrio é: + +> **O ZMP precisa estar dentro do polígono de suporte** (a envoltória convexa dos pés em contato). + +Se o planejador pede um ZMP fora do pé, o pé real não consegue gerar essa força e o robô tomba em torno da borda do pé. Na prática: + +- **Apoio simples:** o ZMP fica sob o pé de apoio. +- **Apoio duplo:** o ZMP pode estar em qualquer lugar entre os dois pés, e é nessa fase que ele "viaja" de um pé para o outro. + +Invertendo a equação do LIPM, dá para recuperar o ZMP a partir de uma trajetória do CoM: $p = x - \ddot{x}/\omega^2$. É isso que [`tools/plot_trajectories.py`](../tools/plot_trajectories.py) faz para desenhar o ZMP. + +## 3. Gerador analítico de caminhada (MuJoCo) + +Arquivo: [`mujoco_sim/analytical_walking/trajectory_generator.py`](../mujoco_sim/analytical_walking/trajectory_generator.py) + +Aqui o problema é invertido: **primeiro** escolhe-se a trajetória do ZMP e **depois** calcula-se o CoM que a realiza, resolvendo a EDO do LIPM em forma fechada. + +### Fases de um passo + +Cada passo dura $T$ e tem três fases, com $t_b = T \cdot \text{ds\_ratio}/2$ e $t_e = T - t_b$: + +``` +|-- apoio duplo --|---------- apoio simples ----------|-- apoio duplo --| +0 tb te T +ZMP: início → pé de apoio ZMP parado no pé de apoio pé de apoio → fim +``` + +O ZMP de referência é linear por partes: sai da posição inicial do torso, vai até o pé de apoio, fica lá durante o apoio simples e segue para a posição final do torso. + +### Solução fechada + +Para um ZMP linear por partes, a solução de $\ddot{x} = \omega^2(x - p)$ é + +$$ +x(t) = p(t) + c_1 e^{\omega t} + c_2 e^{-\omega t} - \text{(termo de correção da rampa do ZMP)} +$$ + +As constantes $c_1, c_2$ são escolhidas para que $x(0)$ seja a pose inicial do torso e $x(T)$, a pose final (`c1`, `c2` no código). O resultado é um CoM que balança suavemente na direção do pé de apoio. Veja o gráfico gerado por `tools/plot_trajectories.py`. + +> **Detalhe didático:** a solução casa a *posição* do CoM entre passos, mas não a *velocidade*. Na troca de passo a aceleração dá um salto, e o ZMP recuperado tem picos nesses instantes. Um gerador "de verdade" impõe também continuidade de velocidade, que é uma das coisas que o MPC resolve naturalmente. + +### Pé em balanço + +- **Horizontal:** blend cosseno $h(\phi)$ de 1 → 0 entre a posição inicial e a final do pé. +- **Vertical:** bump cosseno $v(\phi)$ com altura máxima `z_step`. Perto do fim do apoio simples ele vira uma descida linear, para o pé pousar com velocidade vertical controlada. + +### Planejamento de passos + +`select_next_poses` converte o comando de velocidade $(v_x, v_y, \omega_z)$ num deslocamento do torso de $(v_x, v_y, \omega_z)\cdot T$, no referencial do próprio torso. O pé em balanço pousa a `y_sep` para o lado do novo torso e meia passada à frente. + +### Máquina de estados + +[`walking_engine.py`](../mujoco_sim/analytical_walking/walking_engine.py) alterna pé de apoio e pé de balanço a cada passo e controla as transições `IDLE → WALKING ⇄ IDLE_MARCH → STOPPING → IDLE`. + +## 4. MPC do CoM (PyBullet) + +Arquivo: [`pybullet_sim/mpc_planner.py`](../pybullet_sim/mpc_planner.py) + +Em vez de fixar o ZMP e resolver o CoM analiticamente, o **Model Predictive Control** deixa o otimizador escolher o ZMP a cada instante, dentro de limites. Para cada eixo (x e y separadamente), num horizonte de $N$ passos de $\Delta t$: + +$$ +\begin{aligned} +\min_{x,\,v,\,p}\quad & \sum_{k=0}^{N-1} W_{com}\,(x_k - x^{ref}_k)^2 + W_{zmp}\,(p_k - p^{ref}_k)^2 + W_{vel}\,v_k^2 \\ +\text{s.a.}\quad & x_{k+1} = x_k + v_k\,\Delta t \\ +& v_{k+1} = v_k + \omega^2 (x_k - p_k)\,\Delta t \\ +& p^{min}_k \le p_k \le p^{max}_k \\ +& x_0 = x_{atual},\; v_0 = v_{atual} +\end{aligned} +$$ + +- As duas primeiras restrições são o LIPM discretizado por Euler. +- A terceira garante que **o ZMP fica dentro do pé** (o polígono de suporte vira uma caixa de ±`FOOT_MARGIN` em torno do pé). É ela que dá garantia de equilíbrio *no modelo*. +- É um QP convexo, resolvido pelo OSQP via `cvxpy`. O problema é montado uma vez com `cp.Parameter` e só os valores mudam a cada ciclo. +- Só o primeiro estado previsto é usado e o problema é resolvido de novo no ciclo seguinte (*receding horizon*). + +As referências (`get_references_x/y`) e os limites do ZMP vêm de um plano de passos fixo: período `T_STEP`, pés alternados e passada `cmd_vx * T_STEP`. + +> **Importante:** neste código o MPC roda em **malha aberta**. O estado $(x_0, v_0)$ passado ao MPC é a própria previsão do ciclo anterior, não uma medição do simulador. O MPC funciona, portanto, como um **gerador de trajetória**, e quem estabiliza o robô são os controladores PD das juntas. Fechar a malha (estimar o CoM real e realimentar) é o passo natural seguinte. + +## 5. Cinemática inversa analítica da perna + +Arquivos: [`mujoco_sim/kinematics/ik_solver.py`](../mujoco_sim/kinematics/ik_solver.py) (OP3, 6 DoF completos) e [`pybullet_sim/kinematics.py`](../pybullet_sim/kinematics.py) (Aurea, versão simplificada com pé paralelo ao chão e `hip_yaw = 0`). + +A perna tem a cadeia `hip_yaw (z) → hip_roll (x) → hip_pitch (y) → knee (y) → ank_pitch (y) → ank_roll (x)`. Como os três eixos do quadril se cruzam (aproximadamente) num ponto e os dois do tornozelo também, o problema tem solução fechada. É o método do livro de Kajita et al.: + +1. **Ponto do tornozelo:** a partir do alvo da sola, sobe-se `D_ANKLE_SOLE` no referencial do pé. +2. **Vetor tornozelo → quadril** $r$, expresso no referencial do pé. Sua norma $C$ é a distância entre quadril e tornozelo. +3. **Joelho** pela lei dos cossenos: $\cos q_{knee} = \dfrac{C^2 - L_1^2 - L_2^2}{2 L_1 L_2}$. Se $C > L_1 + L_2$, o alvo está fora de alcance. +4. **Tornozelo roll:** $q_{ar} = \operatorname{atan2}(r_y, r_z)$. +5. **Tornozelo pitch:** ângulo de $r$ no plano sagital, menos o ângulo $\alpha$ que a coxa faz com a linha quadril–tornozelo ($\sin\alpha = \frac{L_1}{C}\sin q_{knee}$). +6. **Quadril:** a rotação que sobra, $R = R_{body}^\top R_{foot} R_x(-q_{ar}) R_y(-(q_{knee}+q_{ap}))$, é decomposta em $R_z(q_{hy}) R_x(q_{hr}) R_y(q_{hp})$. + +O IK devolve ângulos numa convenção geométrica. A tabela `JOINT_SIGNS` em [`config.py`](../mujoco_sim/config.py) converte para o sentido real de cada eixo no MJCF do OP3. + +O teste [`tests/test_mujoco_sim.py`](../tests/test_mujoco_sim.py) confere o IK contra a cinemática direta do MuJoCo: aplica os ângulos e mede onde o tornozelo foi parar. + +## 6. Do alvo cinemático ao torque + +Nenhum dos dois simuladores recebe torques calculados pelo planejador. O laço é sempre: + +``` +gerador/MPC → poses desejadas (torso + pés) → IK → ângulos → controlador PD de posição → torque → física +``` + +No MuJoCo os atuadores são `position` com $\tau = K_p (q_{des} - q) - K_v \dot{q}$, saturado em `ACTUATOR_FORCE_RANGE` ([`config.py`](../mujoco_sim/config.py)). No PyBullet é o `POSITION_CONTROL` do motor de cada junta. + +Com `PHYSICS = False`, o MuJoCo só faz cinemática: o torso é teleportado para a pose planejada e as juntas são escritas diretamente. Serve para depurar o gerador e o IK isolados da dinâmica. + +--- + +## Limitações conhecidas + +Estes pontos são intencionais (simplificações de estudo) ou problemas já identificados. São bons exercícios. + +| Onde | Limitação | +|------|-----------| +| PyBullet | **Relógio do MPC ≠ relógio da física.** `int(DT_MPC / DT_SIM) = int(4,8) = 4`, então cada ciclo avança a trajetória 0,020 s, mas a física só 0,0167 s: a caminhada roda 1,2× mais rápido que o planejado. Os ganhos atuais foram ajustados assim; com relógios consistentes o robô cai até o planejador ser reajustado. Detalhes em [`main.py`](../pybullet_sim/main.py). | +| PyBullet | MPC em malha aberta (seção 4). | +| MuJoCo | O IK ignora o deslocamento dos eixos de roll do quadril e do tornozelo (−24, ∓19 mm no OP3). Com o balanço lateral da marcha padrão, o tornozelo erra o alvo em ~4–5 mm. | +| MuJoCo | Velocidade do CoM descontínua entre passos (seção 3). | +| Ambos | O CoM é aproximado pela posição do torso/pelve; a massa real das pernas e dos braços não entra no modelo. | + +## Referências + +- S. Kajita, H. Hirukawa, K. Harada, K. Yokoi. *Introduction to Humanoid Robotics*. Springer, 2014. LIPM, ZMP, preview control e o IK analítico da seção 5. +- S. Kajita et al. "Biped walking pattern generation by using preview control of zero-moment point". *ICRA*, 2003. +- S. Kajita et al. "The 3D Linear Inverted Pendulum Mode: a simple modeling for a biped walking pattern generation". *IROS*, 2001. +- P.-B. Wieber. "Trajectory free linear model predictive control for stable walking in the presence of strong perturbations". *Humanoids*, 2006. A formulação de MPC com restrição de ZMP. +- M. Vukobratović, B. Borovac. "Zero-Moment Point — thirty five years of its life". *International Journal of Humanoid Robotics*, 2004. +- [MuJoCo Menagerie: Robotis OP3](https://github.com/google-deepmind/mujoco_menagerie/tree/main/robotis_op3), o modelo usado na simulação MuJoCo. +- [robot_descriptions.py](https://github.com/robot-descriptions/robot_descriptions.py), que baixa o modelo automaticamente. diff --git a/mujoco_sim/AnalyticalWalking/__init__.py b/mujoco_sim/AnalyticalWalking/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/mujoco_sim/AnalyticalWalking/__pycache__/__init__.cpython-310.pyc b/mujoco_sim/AnalyticalWalking/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index 6b13200..0000000 Binary files a/mujoco_sim/AnalyticalWalking/__pycache__/__init__.cpython-310.pyc and /dev/null differ diff --git a/mujoco_sim/AnalyticalWalking/__pycache__/__init__.cpython-311.pyc b/mujoco_sim/AnalyticalWalking/__pycache__/__init__.cpython-311.pyc deleted file mode 100644 index 61da5f6..0000000 Binary files a/mujoco_sim/AnalyticalWalking/__pycache__/__init__.cpython-311.pyc and /dev/null differ diff --git a/mujoco_sim/AnalyticalWalking/__pycache__/config.cpython-310.pyc b/mujoco_sim/AnalyticalWalking/__pycache__/config.cpython-310.pyc deleted file mode 100644 index 3da3f8b..0000000 Binary files a/mujoco_sim/AnalyticalWalking/__pycache__/config.cpython-310.pyc and /dev/null differ diff --git a/mujoco_sim/AnalyticalWalking/__pycache__/config.cpython-311.pyc b/mujoco_sim/AnalyticalWalking/__pycache__/config.cpython-311.pyc deleted file mode 100644 index 2a3d83e..0000000 Binary files a/mujoco_sim/AnalyticalWalking/__pycache__/config.cpython-311.pyc and /dev/null differ diff --git a/mujoco_sim/AnalyticalWalking/__pycache__/trajectory_generator.cpython-310.pyc b/mujoco_sim/AnalyticalWalking/__pycache__/trajectory_generator.cpython-310.pyc deleted file mode 100644 index 3b6ac0a..0000000 Binary files a/mujoco_sim/AnalyticalWalking/__pycache__/trajectory_generator.cpython-310.pyc and /dev/null differ diff --git a/mujoco_sim/AnalyticalWalking/__pycache__/trajectory_generator.cpython-311.pyc b/mujoco_sim/AnalyticalWalking/__pycache__/trajectory_generator.cpython-311.pyc deleted file mode 100644 index 1cae5de..0000000 Binary files a/mujoco_sim/AnalyticalWalking/__pycache__/trajectory_generator.cpython-311.pyc and /dev/null differ diff --git a/mujoco_sim/AnalyticalWalking/__pycache__/walking_engine.cpython-310.pyc b/mujoco_sim/AnalyticalWalking/__pycache__/walking_engine.cpython-310.pyc deleted file mode 100644 index 6f0bd98..0000000 Binary files a/mujoco_sim/AnalyticalWalking/__pycache__/walking_engine.cpython-310.pyc and /dev/null differ diff --git a/mujoco_sim/AnalyticalWalking/__pycache__/walking_engine.cpython-311.pyc b/mujoco_sim/AnalyticalWalking/__pycache__/walking_engine.cpython-311.pyc deleted file mode 100644 index f19261f..0000000 Binary files a/mujoco_sim/AnalyticalWalking/__pycache__/walking_engine.cpython-311.pyc and /dev/null differ diff --git a/mujoco_sim/AnalyticalWalking/config.py b/mujoco_sim/AnalyticalWalking/config.py deleted file mode 100644 index 8522be7..0000000 --- a/mujoco_sim/AnalyticalWalking/config.py +++ /dev/null @@ -1,58 +0,0 @@ -JOINT_SIGNS = { - # Junta Esquerda Direita - "hip_yaw": {"l": -1.0, "r": -1.0}, - "hip_roll": {"l": -1.0, "r": -1.0}, - "hip_pitch": {"l": 1.0, "r": -1.0}, - "knee": {"l": 1.0, "r": -1.0}, - "ank_pitch": {"l": -1.0, "r": 1.0}, - "ank_roll": {"l": 1.0, "r": 1.0}, -} - -PHYSICS = True -USE_MPC = False # True → usa MPCWalking; False → usa AnalyticalWalking -USE_TORSO_CTRL = True # True → ativa controlador de orientacao do torso via IMU - -TORSO_CTRL_PARAMS = { - "kp_pitch": 1.5, - "kd_pitch": 0.05, - "kp_roll": 1.5, - "kd_roll": 0.05, - "max_delta": 0.25, - "target_pitch": 0.0, - "target_roll": 0.0, -} - -# ----------------------------------------------------------------------------- -# GANHOS DOS ATUADORES DE POSIÇÃO -# ACTUATOR_KP — ganho proporcional [N·m/rad] -# ACTUATOR_KV — amortecimento derivativo [N·m·s/rad] -# ACTUATOR_FORCE_RANGE — limite de torque por junta [N·m] -# ----------------------------------------------------------------------------- - -ACTUATOR_KP = 120.0 -ACTUATOR_KV = 12.0 -ACTUATOR_FORCE_RANGE = 80.0 - -# ----------------------------------------------------------------------------- -# PARÂMETROS DO MOTOR DE CAMINHADA -# ----------------------------------------------------------------------------- - -WALK_PARAMS = { - "T": 0.4, # Período de um passo [s] — menor = mais rápido - "z_com": 0.22, # Altura do torso [m] - "z_step": 0.04, # Altura máxima do pé no ar [m] - "ds_ratio": 0.12, # Fração do passo em apoio duplo (0 a 0.5) - "y_sep": 0.054, # Separação lateral dos pés [m] -} - -ARM_POSE = { - "l_sho_pitch": -0.0, # ombro esquerdo pitch - "r_sho_pitch": 0.0, # ombro direito pitch - "l_sho_roll": 1.4, # ombro esquerdo roll - "r_sho_roll": -1.4, # ombro direito roll - "l_el": 0.0, # cotovelo esquerdo - "r_el": -0.0, # cotovelo direito -} - -def joint_name(side: str, key: str) -> str: - return f"{side}_{key}" diff --git a/mujoco_sim/AnalyticalWalking/trajectory_generator.py b/mujoco_sim/AnalyticalWalking/trajectory_generator.py deleted file mode 100644 index 25b04ee..0000000 --- a/mujoco_sim/AnalyticalWalking/trajectory_generator.py +++ /dev/null @@ -1,155 +0,0 @@ -""" -Trajectory generator for analytical bipedal walking. -Port of trajectory_generator.cpp (aurea_walk ROS2 package). - -All poses are dicts: {'position': np.ndarray(2,), 'yaw': float, 'is_left': bool (optional)} -""" - -import numpy as np - - -def _h_phi(phi: float, phi_b: float, phi_e: float) -> float: - """Cosine interpolation from 1→0 during single support phase.""" - if phi < phi_b: - return 1.0 - if phi >= phi_e: - return 0.0 - return 0.5 * (1.0 + np.cos(np.pi * (phi - phi_b) / (phi_e - phi_b))) - - -def _v_phi(phi: float, phi_b: float, phi_e: float) -> float: - """Smooth vertical profile for foot lift/land.""" - if phi < phi_b or phi >= phi_e: - return 0.0 - return 0.5 * (1.0 - np.cos(2.0 * np.pi * (phi - phi_b) / (phi_e - phi_b))) - - -def get_com_pose_at_time( - t: float, - T: float, - ds_ratio: float, - z_com: float, - g: float, - p_start: dict, - p_end: dict, - p_support: dict, -) -> tuple[np.ndarray, float]: - """ - Compute COM (x, y) position and yaw at time t within a step of period T. - Uses LIPM dynamics for lateral/forward motion, cosine blend for yaw. - """ - tb = (T * ds_ratio) / 2.0 - te = T - tb - phi = t / T - - h = _h_phi(phi, tb / T, te / T) - com_yaw = h * p_start["yaw"] + (1.0 - h) * p_end["yaw"] - - if z_com <= 0.0: - return np.array(p_start["position"], dtype=float), com_yaw - - lam = np.sqrt(g / z_com) - - m_d1 = (p_support["position"] - p_start["position"]) / tb if tb > 1e-6 else np.zeros(2) - m_d2 = (p_end["position"] - p_support["position"]) / (T - te) if (T - te) > 1e-6 else np.zeros(2) - - k_d1 = (1.0 / lam) * m_d1 * np.sinh(-lam * tb) - k_d2 = (1.0 / lam) * m_d2 * np.sinh(lam * (T - te)) - - exp_pos = np.exp(lam * T) - exp_neg = np.exp(-lam * T) - denom = exp_pos - exp_neg - if abs(denom) < 1e-6: - return np.array(p_start["position"], dtype=float), com_yaw - - c1 = (k_d2 - k_d1 * exp_neg) / denom - c2 = (k_d1 * exp_pos - k_d2) / denom - - if t < tb: - zmp = p_start["position"] + m_d1 * t - sinh_t = (1.0 / lam) * m_d1 * np.sinh(lam * (t - tb)) - com_pos = zmp + c1 * np.exp(lam * t) + c2 * np.exp(-lam * t) - sinh_t - elif t >= te: - zmp = p_support["position"] + m_d2 * (t - te) - sinh_t = (1.0 / lam) * m_d2 * np.sinh(lam * (t - te)) - com_pos = zmp + c1 * np.exp(lam * t) + c2 * np.exp(-lam * t) - sinh_t - else: - com_pos = p_support["position"] + c1 * np.exp(lam * t) + c2 * np.exp(-lam * t) - - return com_pos, com_yaw - - -def get_swing_foot_pose_at_time( - t: float, - T: float, - z_step: float, - ds_ratio: float, - p_start: dict, - p_end: dict, -) -> tuple[np.ndarray, float]: - - # Compute swing foot position (x, y, z) and yaw in world frame at time t. - - tb = (T * ds_ratio) / 2.0 - te = T - tb - phi = t / T - phi_b = tb / T - phi_e = te / T - - h = _h_phi(phi, phi_b, phi_e) - v = _v_phi(phi, phi_b, phi_e) - - pos_2d = h * p_start["position"] + (1.0 - h) * p_end["position"] - yaw = h * p_start["yaw"] + (1.0 - h) * p_end["yaw"] - - landing_phase_start = phi_e * 0.85 - if phi > landing_phase_start: - phase_phi = (phi - landing_phase_start) / (phi_e - landing_phase_start) - # Height at start of landing phase (faithful port of C++ code) - z_land_start = z_step * _v_phi(landing_phase_start * T, phi_b, phi_e) - z = (1.0 - phase_phi) * z_land_start - else: - z = z_step * v - - return np.array([pos_2d[0], pos_2d[1], z], dtype=float), yaw - - -def select_next_poses( - current_torso: dict, - swing_foot: dict, - v_cmd: dict, - T: float, - y_sep: float, -) -> tuple[dict, dict]: - - #Plan the next torso and swing-foot target poses given a velocity command. - - dx = v_cmd.get("vx", 0.0) * T - dy = v_cmd.get("vy", 0.0) * T - d_yaw = v_cmd.get("wz", 0.0) * T - - yaw = current_torso["yaw"] - c, s = np.cos(yaw), np.sin(yaw) - world_disp = np.array([c * dx - s * dy, s * dx + c * dy]) - - next_torso = { - "position": current_torso["position"] + world_disp, - "yaw": yaw + d_yaw, - } - - is_left = swing_foot.get("is_left", True) - foot_offset_y = y_sep if is_left else -y_sep - - ny = next_torso["yaw"] - cn, sn = np.cos(ny), np.sin(ny) - local_offset = np.array([v_cmd.get("vx", 0.0) * T / 2.0, foot_offset_y]) - world_offset = np.array([cn * local_offset[0] - sn * local_offset[1], - sn * local_offset[0] + cn * local_offset[1]]) - - next_swing = { - "position": next_torso["position"] + world_offset, - "yaw": ny, - "is_left": is_left, - } - - return next_torso, next_swing diff --git a/mujoco_sim/AnalyticalWalking/walking_engine.py b/mujoco_sim/AnalyticalWalking/walking_engine.py deleted file mode 100644 index c491ed9..0000000 --- a/mujoco_sim/AnalyticalWalking/walking_engine.py +++ /dev/null @@ -1,184 +0,0 @@ -""" -Analytical walking state machine for bipedal robots. - -Usage: - engine = WalkingEngine() - engine.set_command(vx=0.1) # start walking forward - body_pos, body_yaw, lp, ly, rp, ry = engine.step(dt) - # body_pos: np.ndarray(3,) – desired pelvis position in world frame - # body_yaw: float – desired pelvis yaw - # lp / rp: np.ndarray(3,) – left/right sole target in world frame - # ly / ry: float – left/right foot yaw -""" - -import numpy as np -from .trajectory_generator import ( - get_com_pose_at_time, - get_swing_foot_pose_at_time, - select_next_poses, -) - - -class WalkingEngine: - IDLE = 0 - WALKING = 1 - IDLE_MARCH = 2 - STOPPING = 3 - - def __init__( - self, - T: float = 0.5, - z_com: float = 0.22, - z_step: float = 0.04, - ds_ratio: float = 0.13, - y_sep: float = 0.054, - g: float = 9.81, - ): - self.T = T - self.z_com = z_com - self.z_step = z_step - self.ds_ratio = ds_ratio - self.y_sep = y_sep - self.g = g - - self.state = self.IDLE - self.t_step = 0.0 - - # Foot states (world frame 2-D positions) - self.left_foot = {"position": np.array([0.0, y_sep]), "yaw": 0.0, "is_left": True} - self.right_foot = {"position": np.array([0.0, -y_sep]), "yaw": 0.0, "is_left": False} - - self.support_foot = self.right_foot - self.swing_foot_ref = self.left_foot - - # Torso state (world frame) - self.torso = {"position": np.array([0.0, 0.0]), "yaw": 0.0} - - # Step plan (set by start_new_step) - self.torso_start = None - self.torso_target = None - self.swing_start = None - self.swing_target = None - - # Velocity command - self._cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} - - def set_command(self, vx: float = 0.0, vy: float = 0.0, wz: float = 0.0) -> None: - self._cmd = {"vx": vx, "vy": vy, "wz": wz} - - def request_stop(self) -> None: - if self.state in (self.WALKING, self.IDLE_MARCH): - self.state = self.STOPPING - - def step(self, dt: float) -> tuple: - """ - Advance the engine by dt seconds and return the desired kinematic state. - - Returns: - (body_pos, body_yaw, left_sole, left_yaw, right_sole, right_yaw) - body_pos – np.ndarray(3,): desired pelvis world position [x, y, z_com] - body_yaw – float: desired pelvis yaw - left_sole – np.ndarray(3,): desired left foot sole world position - left_yaw – float: desired left foot yaw - right_sole – np.ndarray(3,): desired right foot sole world position - right_yaw – float: desired right foot yaw - """ - self._update_state() - - if self.state == self.IDLE: - return self._standing_pose() - - self.t_step += dt - - com_2d, com_yaw = get_com_pose_at_time( - self.t_step, self.T, self.ds_ratio, self.z_com, self.g, - self.torso_start, self.torso_target, self.support_foot, - ) - - swing_3d, swing_yaw = get_swing_foot_pose_at_time( - self.t_step, self.T, self.z_step, self.ds_ratio, - self.swing_start, self.swing_target, - ) - - body_pos = np.array([com_2d[0], com_2d[1], self.z_com]) - sup_pos = np.array([self.support_foot["position"][0], - self.support_foot["position"][1], 0.0]) - - if self.swing_foot_ref["is_left"]: - left_pos, left_yaw = swing_3d, swing_yaw - right_pos, right_yaw = sup_pos, self.support_foot["yaw"] - else: - right_pos, right_yaw = swing_3d, swing_yaw - left_pos, left_yaw = sup_pos, self.support_foot["yaw"] - - if self.t_step >= self.T: - self._end_step() - - return body_pos, com_yaw, left_pos, left_yaw, right_pos, right_yaw - - def _update_state(self) -> None: - should_walk = (abs(self._cmd["vx"]) > 0.01 or - abs(self._cmd["vy"]) > 0.01 or - abs(self._cmd["wz"]) > 0.01) - - prev = self.state - - if self.state == self.STOPPING: - pass # state changes happen in _end_step - elif self.state == self.IDLE: - if should_walk: - self.state = self.WALKING - elif self.state in (self.WALKING, self.IDLE_MARCH): - self.state = self.WALKING if should_walk else self.IDLE_MARCH - - if prev == self.IDLE and self.state != self.IDLE: - self._start_new_step() - - def _start_new_step(self) -> None: - self.t_step = 0.0 - - # Swap support / swing - if self.support_foot["is_left"]: - self.support_foot = self.right_foot - self.swing_foot_ref = self.left_foot - else: - self.support_foot = self.left_foot - self.swing_foot_ref = self.right_foot - - cmd = self._cmd if self.state == self.WALKING else {"vx": 0.0, "vy": 0.0, "wz": 0.0} - - self.torso_target, self.swing_target = select_next_poses( - self.torso, self.swing_foot_ref, cmd, self.T, self.y_sep - ) - self.torso_start = { - "position": self.torso["position"].copy(), - "yaw": self.torso["yaw"], - } - self.swing_start = { - "position": self.swing_foot_ref["position"].copy(), - "yaw": self.swing_foot_ref["yaw"], - "is_left": self.swing_foot_ref["is_left"], - } - - def _end_step(self) -> None: - self.torso["position"] = self.torso_target["position"].copy() - self.torso["yaw"] = self.torso_target["yaw"] - self.swing_foot_ref["position"] = self.swing_target["position"].copy() - self.swing_foot_ref["yaw"] = self.swing_target["yaw"] - - if self.state == self.STOPPING: - self.state = self.IDLE - else: - self._start_new_step() - - def _standing_pose(self) -> tuple: - body_pos = np.array([self.torso["position"][0], - self.torso["position"][1], self.z_com]) - body_yaw = self.torso["yaw"] - left_pos = np.array([self.left_foot["position"][0], - self.left_foot["position"][1], 0.0]) - right_pos = np.array([self.right_foot["position"][0], - self.right_foot["position"][1], 0.0]) - return (body_pos, body_yaw, - left_pos, self.left_foot["yaw"], - right_pos, self.right_foot["yaw"]) diff --git a/mujoco_sim/Utils/__pycache__/ik_solver.cpython-310.pyc b/mujoco_sim/Utils/__pycache__/ik_solver.cpython-310.pyc deleted file mode 100644 index dd859d9..0000000 Binary files a/mujoco_sim/Utils/__pycache__/ik_solver.cpython-310.pyc and /dev/null differ diff --git a/mujoco_sim/Utils/__pycache__/ik_solver.cpython-311.pyc b/mujoco_sim/Utils/__pycache__/ik_solver.cpython-311.pyc deleted file mode 100644 index 62cc4ac..0000000 Binary files a/mujoco_sim/Utils/__pycache__/ik_solver.cpython-311.pyc and /dev/null differ diff --git a/mujoco_sim/Utils/__pycache__/torso_controller.cpython-310.pyc b/mujoco_sim/Utils/__pycache__/torso_controller.cpython-310.pyc deleted file mode 100644 index eeacac3..0000000 Binary files a/mujoco_sim/Utils/__pycache__/torso_controller.cpython-310.pyc and /dev/null differ diff --git a/mujoco_sim/__init__.py b/mujoco_sim/__init__.py index e69de29..ea8ec66 100644 --- a/mujoco_sim/__init__.py +++ b/mujoco_sim/__init__.py @@ -0,0 +1 @@ +"""Robotis OP3 walking simulation in MuJoCo. Run with: python -m mujoco_sim""" diff --git a/mujoco_sim/__main__.py b/mujoco_sim/__main__.py index cd03d8a..2ccf3cf 100644 --- a/mujoco_sim/__main__.py +++ b/mujoco_sim/__main__.py @@ -1,2 +1,4 @@ from .sim import main -main() + +if __name__ == "__main__": + main() diff --git a/mujoco_sim/__pycache__/__init__.cpython-310.pyc b/mujoco_sim/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index d691e1e..0000000 Binary files a/mujoco_sim/__pycache__/__init__.cpython-310.pyc and /dev/null differ diff --git a/mujoco_sim/__pycache__/__init__.cpython-311.pyc b/mujoco_sim/__pycache__/__init__.cpython-311.pyc deleted file mode 100644 index 9080895..0000000 Binary files a/mujoco_sim/__pycache__/__init__.cpython-311.pyc and /dev/null differ diff --git a/mujoco_sim/__pycache__/__init__.cpython-312.pyc b/mujoco_sim/__pycache__/__init__.cpython-312.pyc deleted file mode 100644 index 8431853..0000000 Binary files a/mujoco_sim/__pycache__/__init__.cpython-312.pyc and /dev/null differ diff --git a/mujoco_sim/__pycache__/__main__.cpython-310.pyc b/mujoco_sim/__pycache__/__main__.cpython-310.pyc deleted file mode 100644 index 6c359b6..0000000 Binary files a/mujoco_sim/__pycache__/__main__.cpython-310.pyc and /dev/null differ diff --git a/mujoco_sim/__pycache__/__main__.cpython-311.pyc b/mujoco_sim/__pycache__/__main__.cpython-311.pyc deleted file mode 100644 index caa6212..0000000 Binary files a/mujoco_sim/__pycache__/__main__.cpython-311.pyc and /dev/null differ diff --git a/mujoco_sim/__pycache__/__main__.cpython-312.pyc b/mujoco_sim/__pycache__/__main__.cpython-312.pyc deleted file mode 100644 index 032cd68..0000000 Binary files a/mujoco_sim/__pycache__/__main__.cpython-312.pyc and /dev/null differ diff --git a/mujoco_sim/__pycache__/sim.cpython-310.pyc b/mujoco_sim/__pycache__/sim.cpython-310.pyc deleted file mode 100644 index e856a1c..0000000 Binary files a/mujoco_sim/__pycache__/sim.cpython-310.pyc and /dev/null differ diff --git a/mujoco_sim/__pycache__/sim.cpython-311.pyc b/mujoco_sim/__pycache__/sim.cpython-311.pyc deleted file mode 100644 index 4e560ce..0000000 Binary files a/mujoco_sim/__pycache__/sim.cpython-311.pyc and /dev/null differ diff --git a/mujoco_sim/__pycache__/sim.cpython-312.pyc b/mujoco_sim/__pycache__/sim.cpython-312.pyc deleted file mode 100644 index e3784fd..0000000 Binary files a/mujoco_sim/__pycache__/sim.cpython-312.pyc and /dev/null differ diff --git a/mujoco_sim/analytical_walking/__init__.py b/mujoco_sim/analytical_walking/__init__.py new file mode 100644 index 0000000..b8e2f06 --- /dev/null +++ b/mujoco_sim/analytical_walking/__init__.py @@ -0,0 +1,5 @@ +"""Analytical (LIPM-based) walking pattern generator.""" + +from .walking_engine import WalkingEngine, WalkState, WalkTarget + +__all__ = ["WalkingEngine", "WalkState", "WalkTarget"] diff --git a/mujoco_sim/analytical_walking/trajectory_generator.py b/mujoco_sim/analytical_walking/trajectory_generator.py new file mode 100644 index 0000000..7151dce --- /dev/null +++ b/mujoco_sim/analytical_walking/trajectory_generator.py @@ -0,0 +1,203 @@ +""" +Trajectory generator for analytical bipedal walking. + +Port of trajectory_generator.cpp from the aurea_walk ROS2 package, with two +fixes to the swing-foot landing profile (see get_swing_foot_pose_at_time). + +Conventions +----------- +- All positions are in the world frame, in metres. Yaw is in radians. +- A step lasts T seconds and is split into three phases: + + |-- double support --|------ single support ------|-- double support --| + 0 tb te T + + with tb = T * ds_ratio / 2 and te = T - tb. +- phi = t / T is the normalised phase of the step (0 → 1). +""" + +from dataclasses import dataclass, field + +import numpy as np + + +@dataclass +class Pose2D: + """Planar pose on the ground: (x, y) position and yaw.""" + + position: np.ndarray = field(default_factory=lambda: np.zeros(2)) + yaw: float = 0.0 + + def copy(self) -> "Pose2D": + return Pose2D(self.position.copy(), self.yaw) + + +@dataclass +class FootPose(Pose2D): + """Planar pose of a foot, tagged with which leg it belongs to.""" + + is_left: bool = True + + def copy(self) -> "FootPose": + return FootPose(self.position.copy(), self.yaw, self.is_left) + + +def _h_phi(phi: float, phi_b: float, phi_e: float) -> float: + """Horizontal blend: 1 before phi_b, 0 after phi_e, cosine in between.""" + if phi < phi_b: + return 1.0 + if phi >= phi_e: + return 0.0 + return 0.5 * (1.0 + np.cos(np.pi * (phi - phi_b) / (phi_e - phi_b))) + + +def _v_phi(phi: float, phi_b: float, phi_e: float) -> float: + """Vertical bump: 0 outside [phi_b, phi_e], peaks at 1 in the middle.""" + if phi < phi_b or phi >= phi_e: + return 0.0 + return 0.5 * (1.0 - np.cos(2.0 * np.pi * (phi - phi_b) / (phi_e - phi_b))) + + +def get_com_pose_at_time( + t: float, + T: float, + ds_ratio: float, + z_com: float, + g: float, + p_start: Pose2D, + p_end: Pose2D, + p_support: Pose2D, +) -> tuple[np.ndarray, float]: + """ + CoM (x, y) position and yaw at time t within a step. + + The ZMP reference is piecewise linear: it moves from p_start to the support + foot during the first double support, stays on the support foot during + single support, and moves to p_end during the last double support. The CoM + is the closed-form solution of the Linear Inverted Pendulum Model (LIPM) + + x_ddot = lambda^2 * (x - zmp), lambda = sqrt(g / z_com) + + with boundary conditions x(0) = p_start and x(T) = p_end. + """ + tb = (T * ds_ratio) / 2.0 + te = T - tb + phi = t / T + + h = _h_phi(phi, tb / T, te / T) + com_yaw = h * p_start.yaw + (1.0 - h) * p_end.yaw + + if z_com <= 0.0: + return np.array(p_start.position, dtype=float), com_yaw + + lam = np.sqrt(g / z_com) + + # ZMP slopes during the two double-support phases + m_d1 = (p_support.position - p_start.position) / tb if tb > 1e-6 else np.zeros(2) + m_d2 = (p_end.position - p_support.position) / (T - te) if (T - te) > 1e-6 else np.zeros(2) + + k_d1 = (1.0 / lam) * m_d1 * np.sinh(-lam * tb) + k_d2 = (1.0 / lam) * m_d2 * np.sinh(lam * (T - te)) + + exp_pos = np.exp(lam * T) + exp_neg = np.exp(-lam * T) + denom = exp_pos - exp_neg + if abs(denom) < 1e-6: + return np.array(p_start.position, dtype=float), com_yaw + + # Homogeneous-solution coefficients that satisfy the boundary conditions + c1 = (k_d2 - k_d1 * exp_neg) / denom + c2 = (k_d1 * exp_pos - k_d2) / denom + + if t < tb: + zmp = p_start.position + m_d1 * t + sinh_t = (1.0 / lam) * m_d1 * np.sinh(lam * (t - tb)) + com_pos = zmp + c1 * np.exp(lam * t) + c2 * np.exp(-lam * t) - sinh_t + elif t >= te: + zmp = p_support.position + m_d2 * (t - te) + sinh_t = (1.0 / lam) * m_d2 * np.sinh(lam * (t - te)) + com_pos = zmp + c1 * np.exp(lam * t) + c2 * np.exp(-lam * t) - sinh_t + else: + com_pos = p_support.position + c1 * np.exp(lam * t) + c2 * np.exp(-lam * t) + + return com_pos, com_yaw + + +def get_swing_foot_pose_at_time( + t: float, + T: float, + z_step: float, + ds_ratio: float, + p_start: Pose2D, + p_end: Pose2D, +) -> tuple[np.ndarray, float]: + """ + Swing foot (x, y, z) position and yaw at time t within a step. + + The foot follows a cosine blend horizontally and a cosine bump vertically. + Near the end of single support the vertical bump is replaced by a linear + descent so the foot lands with a constant, controlled vertical speed. + """ + tb = (T * ds_ratio) / 2.0 + te = T - tb + phi = t / T + phi_b = tb / T + phi_e = te / T + + h = _h_phi(phi, phi_b, phi_e) + v = _v_phi(phi, phi_b, phi_e) + + pos_2d = h * p_start.position + (1.0 - h) * p_end.position + yaw = h * p_start.yaw + (1.0 - h) * p_end.yaw + + landing_phase_start = phi_e * 0.85 + if phi > landing_phase_start: + # Fix 1: the C++ original passed a time (landing_phase_start * T) where + # _v_phi expects a phase, so the foot jumped up when landing started. + z_land_start = z_step * _v_phi(landing_phase_start, phi_b, phi_e) + # Fix 2: clamp so the foot stays on the ground (z = 0) during the final + # double support instead of being commanded below the floor. + landing_progress = min( + (phi - landing_phase_start) / (phi_e - landing_phase_start), 1.0 + ) + z = (1.0 - landing_progress) * z_land_start + else: + z = z_step * v + + return np.array([pos_2d[0], pos_2d[1], z], dtype=float), yaw + + +def select_next_poses( + current_torso: Pose2D, + swing_foot: FootPose, + v_cmd: dict, + T: float, + y_sep: float, +) -> tuple[Pose2D, FootPose]: + """ + Plan the next torso pose and swing-foot landing pose. + + The torso moves by (vx, vy, wz) * T, expressed in its own frame. The swing + foot lands y_sep to the side of the new torso pose and half a stride ahead. + """ + dx = v_cmd.get("vx", 0.0) * T + dy = v_cmd.get("vy", 0.0) * T + d_yaw = v_cmd.get("wz", 0.0) * T + + yaw = current_torso.yaw + c, s = np.cos(yaw), np.sin(yaw) + world_disp = np.array([c * dx - s * dy, s * dx + c * dy]) + + next_torso = Pose2D(current_torso.position + world_disp, yaw + d_yaw) + + foot_offset_y = y_sep if swing_foot.is_left else -y_sep + + ny = next_torso.yaw + cn, sn = np.cos(ny), np.sin(ny) + local_offset = np.array([v_cmd.get("vx", 0.0) * T / 2.0, foot_offset_y]) + world_offset = np.array([cn * local_offset[0] - sn * local_offset[1], + sn * local_offset[0] + cn * local_offset[1]]) + + next_swing = FootPose(next_torso.position + world_offset, ny, swing_foot.is_left) + + return next_torso, next_swing diff --git a/mujoco_sim/analytical_walking/walking_engine.py b/mujoco_sim/analytical_walking/walking_engine.py new file mode 100644 index 0000000..ccef1a8 --- /dev/null +++ b/mujoco_sim/analytical_walking/walking_engine.py @@ -0,0 +1,199 @@ +""" +Analytical walking state machine for bipedal robots. + +Usage: + engine = WalkingEngine() + engine.set_command(vx=0.1) # start walking forward + target = engine.step(dt) + target.body_pos # np.ndarray(3,) – desired pelvis position in world frame + target.body_yaw # float – desired pelvis yaw + target.left_sole # np.ndarray(3,) – left sole target in world frame + target.left_yaw # float – left foot yaw + target.right_sole / target.right_yaw + +State machine: + + IDLE ──set_command(≠0)──▶ WALKING ◀──set_command(≠0 / 0)──▶ IDLE_MARCH + │ │ ▲ + └──────────────────────────┼────── march_in_place() ──────────┘ + ▲ ▼ │ + └──(step ends)──── STOPPING ◀──────── request_stop() ─────────┘ +""" + +from enum import Enum, auto +from typing import NamedTuple + +import numpy as np + +from .trajectory_generator import ( + FootPose, + Pose2D, + get_com_pose_at_time, + get_swing_foot_pose_at_time, + select_next_poses, +) + + +class WalkState(Enum): + IDLE = auto() # standing still, both feet on the ground + WALKING = auto() # stepping according to the velocity command + IDLE_MARCH = auto() # stepping in place (command is zero) + STOPPING = auto() # finishing the current step, then IDLE + + +class WalkTarget(NamedTuple): + """Desired kinematic state for one control tick (all in world frame).""" + + body_pos: np.ndarray + body_yaw: float + left_sole: np.ndarray + left_yaw: float + right_sole: np.ndarray + right_yaw: float + + +class WalkingEngine: + def __init__( + self, + T: float = 0.5, + z_com: float = 0.22, + z_step: float = 0.04, + ds_ratio: float = 0.13, + y_sep: float = 0.054, + g: float = 9.81, + ): + self.T = T + self.z_com = z_com + self.z_step = z_step + self.ds_ratio = ds_ratio + self.y_sep = y_sep + self.g = g + + self.state = WalkState.IDLE + self.t_step = 0.0 + + self.left_foot = FootPose(np.array([0.0, y_sep]), 0.0, is_left=True) + self.right_foot = FootPose(np.array([0.0, -y_sep]), 0.0, is_left=False) + + # References to left_foot / right_foot; swapped at every step + self.support_foot = self.right_foot + self.swing_foot = self.left_foot + + self.torso = Pose2D(np.array([0.0, 0.0]), 0.0) + + # Current step plan (set by _start_new_step) + self.torso_start: Pose2D | None = None + self.torso_target: Pose2D | None = None + self.swing_start: FootPose | None = None + self.swing_target: FootPose | None = None + + self._cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} + + def set_command(self, vx: float = 0.0, vy: float = 0.0, wz: float = 0.0) -> None: + """Set the velocity command [m/s, m/s, rad/s] in the torso frame.""" + self._cmd = {"vx": vx, "vy": vy, "wz": wz} + + def march_in_place(self) -> None: + """Keep stepping without moving (also starts stepping from IDLE).""" + self._cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} + if self.state == WalkState.IDLE: + self.state = WalkState.IDLE_MARCH + self._start_new_step() + elif self.state == WalkState.STOPPING: + self.state = WalkState.IDLE_MARCH + + def request_stop(self) -> None: + """Finish the current step and go back to IDLE.""" + # Clear the command, otherwise IDLE would switch straight back to WALKING + self._cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} + if self.state in (WalkState.WALKING, WalkState.IDLE_MARCH): + self.state = WalkState.STOPPING + + def step(self, dt: float) -> WalkTarget: + """Advance the engine by dt seconds and return the desired kinematic state.""" + self._update_state() + + if self.state == WalkState.IDLE: + return self.standing_pose() + + self.t_step += dt + + com_2d, com_yaw = get_com_pose_at_time( + self.t_step, self.T, self.ds_ratio, self.z_com, self.g, + self.torso_start, self.torso_target, self.support_foot, + ) + + swing_3d, swing_yaw = get_swing_foot_pose_at_time( + self.t_step, self.T, self.z_step, self.ds_ratio, + self.swing_start, self.swing_target, + ) + + body_pos = np.array([com_2d[0], com_2d[1], self.z_com]) + sup_pos = np.array([*self.support_foot.position, 0.0]) + + if self.swing_foot.is_left: + left_pos, left_yaw = swing_3d, swing_yaw + right_pos, right_yaw = sup_pos, self.support_foot.yaw + else: + right_pos, right_yaw = swing_3d, swing_yaw + left_pos, left_yaw = sup_pos, self.support_foot.yaw + + if self.t_step >= self.T: + self._end_step() + + return WalkTarget(body_pos, com_yaw, left_pos, left_yaw, right_pos, right_yaw) + + def standing_pose(self) -> WalkTarget: + """Kinematic state with both feet on the ground under the current torso.""" + return WalkTarget( + body_pos=np.array([*self.torso.position, self.z_com]), + body_yaw=self.torso.yaw, + left_sole=np.array([*self.left_foot.position, 0.0]), + left_yaw=self.left_foot.yaw, + right_sole=np.array([*self.right_foot.position, 0.0]), + right_yaw=self.right_foot.yaw, + ) + + def _update_state(self) -> None: + should_walk = any(abs(v) > 0.01 for v in self._cmd.values()) + + prev = self.state + + if self.state == WalkState.STOPPING: + pass # the transition to IDLE happens in _end_step + elif self.state == WalkState.IDLE: + if should_walk: + self.state = WalkState.WALKING + elif self.state in (WalkState.WALKING, WalkState.IDLE_MARCH): + self.state = WalkState.WALKING if should_walk else WalkState.IDLE_MARCH + + if prev == WalkState.IDLE and self.state != WalkState.IDLE: + self._start_new_step() + + def _start_new_step(self) -> None: + self.t_step = 0.0 + + # Swap support / swing + if self.support_foot.is_left: + self.support_foot, self.swing_foot = self.right_foot, self.left_foot + else: + self.support_foot, self.swing_foot = self.left_foot, self.right_foot + + cmd = self._cmd if self.state == WalkState.WALKING else {"vx": 0.0, "vy": 0.0, "wz": 0.0} + + self.torso_target, self.swing_target = select_next_poses( + self.torso, self.swing_foot, cmd, self.T, self.y_sep + ) + self.torso_start = self.torso.copy() + self.swing_start = self.swing_foot.copy() + + def _end_step(self) -> None: + self.torso.position = self.torso_target.position.copy() + self.torso.yaw = self.torso_target.yaw + self.swing_foot.position = self.swing_target.position.copy() + self.swing_foot.yaw = self.swing_target.yaw + + if self.state == WalkState.STOPPING: + self.state = WalkState.IDLE + else: + self._start_new_step() diff --git a/mujoco_sim/config.py b/mujoco_sim/config.py new file mode 100644 index 0000000..04e4cb8 --- /dev/null +++ b/mujoco_sim/config.py @@ -0,0 +1,92 @@ +""" +Configuration for the MuJoCo OP3 simulation. + +Edit the values here to change how the robot is simulated and how it walks. +""" + +# ----------------------------------------------------------------------------- +# SIMULATION MODE +# True → mj_step: full physics (contacts, gravity, PD actuators) +# False → mj_forward: kinematics only; joints are set directly, no contacts +# ----------------------------------------------------------------------------- + +PHYSICS = True + +# ----------------------------------------------------------------------------- +# TORSO ORIENTATION CONTROLLER (simulated IMU → hip corrections) +# A PD on torso pitch/roll that adds corrections to the hip joint targets. +# Gains are in [rad of hip correction / rad of tilt] and [... / (rad/s)]. +# ----------------------------------------------------------------------------- + +USE_TORSO_CTRL = True + +TORSO_CTRL_PARAMS = { + "kp_pitch": 1.5, + "kd_pitch": 0.05, + "kp_roll": 1.5, + "kd_roll": 0.05, + "max_delta": 0.25, # clamp on each correction [rad] + "target_pitch": 0.0, # desired torso pitch [rad] + "target_roll": 0.0, # desired torso roll [rad] +} + +# ----------------------------------------------------------------------------- +# POSITION ACTUATOR GAINS (applied to every joint) +# ACTUATOR_KP — proportional gain [N·m/rad] +# ACTUATOR_KV — derivative (damping) gain [N·m·s/rad] +# ACTUATOR_FORCE_RANGE — torque limit per joint [N·m] +# ----------------------------------------------------------------------------- + +ACTUATOR_KP = 120.0 +ACTUATOR_KV = 12.0 +ACTUATOR_FORCE_RANGE = 80.0 + +# ----------------------------------------------------------------------------- +# WALKING ENGINE PARAMETERS +# ----------------------------------------------------------------------------- + +WALK_PARAMS = { + "T": 0.4, # Step period [s] — smaller = faster steps + "z_com": 0.22, # Torso height [m] + "z_step": 0.04, # Maximum swing-foot height [m] + "ds_ratio": 0.12, # Fraction of the step in double support (0 to 0.5) + "y_sep": 0.054, # Lateral distance from torso centre to each foot [m] +} + +# Velocity commands bound to the keyboard keys (see sim.py) +KEY_COMMANDS = { + "forward": {"vx": 0.05}, # [m/s] + "lateral": {"vy": 0.03}, # [m/s] + "turn": {"wz": 0.5}, # [rad/s] +} + +# ----------------------------------------------------------------------------- +# ROBOT-SPECIFIC MAPPING +# ----------------------------------------------------------------------------- + +# Sign that converts the IK's geometric angle convention into the joint axes +# defined in the OP3 MJCF (some axes point in the opposite direction). +JOINT_SIGNS = { + # Joint Left Right + "hip_yaw": {"l": -1.0, "r": -1.0}, + "hip_roll": {"l": -1.0, "r": -1.0}, + "hip_pitch": {"l": 1.0, "r": -1.0}, + "knee": {"l": 1.0, "r": -1.0}, + "ank_pitch": {"l": -1.0, "r": 1.0}, + "ank_roll": {"l": 1.0, "r": 1.0}, +} + +# Fixed arm pose [rad] held during walking +ARM_POSE = { + "l_sho_pitch": 0.0, + "r_sho_pitch": 0.0, + "l_sho_roll": 1.4, + "r_sho_roll": -1.4, + "l_el": 0.0, + "r_el": 0.0, +} + + +def joint_name(side: str, key: str) -> str: + """MJCF joint name, e.g. joint_name("l", "knee") → "l_knee".""" + return f"{side}_{key}" diff --git a/mujoco_sim/control/__init__.py b/mujoco_sim/control/__init__.py new file mode 100644 index 0000000..a2b26b0 --- /dev/null +++ b/mujoco_sim/control/__init__.py @@ -0,0 +1,5 @@ +"""Feedback controllers that correct the walking engine's joint targets.""" + +from .torso_controller import SimulatedIMU, TorsoOrientationController + +__all__ = ["SimulatedIMU", "TorsoOrientationController"] diff --git a/mujoco_sim/Utils/torso_controller.py b/mujoco_sim/control/torso_controller.py similarity index 100% rename from mujoco_sim/Utils/torso_controller.py rename to mujoco_sim/control/torso_controller.py diff --git a/mujoco_sim/kinematics/__init__.py b/mujoco_sim/kinematics/__init__.py new file mode 100644 index 0000000..c3d0b5b --- /dev/null +++ b/mujoco_sim/kinematics/__init__.py @@ -0,0 +1,5 @@ +"""Leg kinematics for the OP3.""" + +from .ik_solver import IKSolver + +__all__ = ["IKSolver"] diff --git a/mujoco_sim/Utils/ik_solver.py b/mujoco_sim/kinematics/ik_solver.py similarity index 51% rename from mujoco_sim/Utils/ik_solver.py rename to mujoco_sim/kinematics/ik_solver.py index 67b2ab3..baab0b6 100644 --- a/mujoco_sim/Utils/ik_solver.py +++ b/mujoco_sim/kinematics/ik_solver.py @@ -6,6 +6,13 @@ Constants measured from the robot_descriptions OP3 MJCF (mujoco_menagerie / op3_mj_description). + +Approximation: the solver treats the hip_roll and ank_roll axes as passing +through the hip_pitch / ank_pitch points. On the OP3 they are offset by +[-0.024, ∓0.019] m, so the foot lands off target whenever the roll angles +are non-zero. With the lateral CoM sway of the default gait the ankle error +reaches ~4-5 mm (measured against MuJoCo forward kinematics). Modelling the +offsets exactly would remove it; it is a good exercise. """ import numpy as np @@ -21,8 +28,10 @@ L_COXA = 0.1102 # hip_pitch → knee (vertical) L_TIBIA = 0.1100 # knee → ank_pitch (vertical) -# Distance from ank_roll joint to foot sole (from contact box geoms, z-half = 0.004, -# centre at z = -0.0265 → bottom = -0.0265 - 0.004 = -0.0305) +# Distance from the ankle point to the foot sole. The "ankle point" is the +# ank_pitch joint (the ank_roll axis sits at the same height, offset by +# [-0.0241, ∓0.019], mirroring the hip). Foot contact boxes: z-half = 0.004, +# centre 0.0266 below ank_pitch → sole 0.0305 below it. D_ANKLE_SOLE = 0.0305 # ───────────────────────────────────────────────────────────────────────────── @@ -68,51 +77,46 @@ def solve( R1 = R_body R7 = R_foot_target - # Ank_roll joint position from sole target (sole is D_ANKLE_SOLE below ank_roll) + # Ankle point (ank_pitch joint) from the sole target p7_sole = p_foot_target p7 = p7_sole - R7 @ np.array([0.0, 0.0, -D_ANKLE_SOLE]) - try: - sign = 1.0 if leg == "left" else -1.0 - hip_offset = np.array([0.0, HIP_Y * sign, HIP_Z]) - p2 = p_body + R1 @ hip_offset # hip_pitch joint in world frame - - # Vector from ankle to hip expressed in foot frame - r_vec = R7.T @ (p2 - p7) - rx, ry, rz = r_vec + sign = 1.0 if leg == "left" else -1.0 + hip_offset = np.array([0.0, HIP_Y * sign, HIP_Z]) + p2 = p_body + R1 @ hip_offset # hip_pitch joint in world frame - C = float(np.linalg.norm(r_vec)) - if C > L_COXA + L_TIBIA: - return False, [] + # Vector from ankle to hip expressed in foot frame + r_vec = R7.T @ (p2 - p7) + rx, ry, rz = r_vec - # ── Knee angle (q5) ─────────────────────────────────────────────── - cos_q5 = (C**2 - L_COXA**2 - L_TIBIA**2) / (2.0 * L_COXA * L_TIBIA) - cos_q5 = float(np.clip(cos_q5, -1.0, 1.0)) - q5 = np.arccos(cos_q5) + C = float(np.linalg.norm(r_vec)) + if C > L_COXA + L_TIBIA or C < 1e-9: # out of reach / degenerate + return False, [] - # ── Ankle roll (q7) ─────────────────────────────────────────────── - q7 = np.arctan2(ry, rz) + # ── Knee angle (q5) ─────────────────────────────────────────────── + cos_q5 = (C**2 - L_COXA**2 - L_TIBIA**2) / (2.0 * L_COXA * L_TIBIA) + cos_q5 = float(np.clip(cos_q5, -1.0, 1.0)) + q5 = np.arccos(cos_q5) - # ── Ankle pitch (q6) ────────────────────────────────────────────── - sin_alpha = float(np.clip((L_COXA / C) * np.sin(q5), -1.0, 1.0)) - alpha = np.arcsin(sin_alpha) - q6 = -np.arctan2(rx, np.copysign(1.0, rz) * np.sqrt(ry**2 + rz**2)) - alpha + # ── Ankle roll (q7) ─────────────────────────────────────────────── + q7 = np.arctan2(ry, rz) - # ── Hip angles from remaining rotation ──────────────────────────── - R_q7_inv = _rot_x(-q7) - R_q56_inv = _rot_y(-q5 - q6) - R_target = R1.T @ R7 @ R_q7_inv @ R_q56_inv + # ── Ankle pitch (q6) ────────────────────────────────────────────── + sin_alpha = float(np.clip((L_COXA / C) * np.sin(q5), -1.0, 1.0)) + alpha = np.arcsin(sin_alpha) + q6 = -np.arctan2(rx, np.copysign(1.0, rz) * np.sqrt(ry**2 + rz**2)) - alpha - R12, R22 = R_target[0, 1], R_target[1, 1] - R31, R32, R33 = R_target[2, 0], R_target[2, 1], R_target[2, 2] + # ── Hip angles from remaining rotation ──────────────────────────── + R_q7_inv = _rot_x(-q7) + R_q56_inv = _rot_y(-q5 - q6) + R_target = R1.T @ R7 @ R_q7_inv @ R_q56_inv - q2 = np.arctan2(-R12, R22) # hip_yaw - s2, c2 = np.sin(q2), np.cos(q2) - q3 = np.arctan2(R32, -R12 * s2 + R22 * c2) # hip_roll - q4 = np.arctan2(-R31, R33) # hip_pitch + R12, R22 = R_target[0, 1], R_target[1, 1] + R31, R32, R33 = R_target[2, 0], R_target[2, 1], R_target[2, 2] - return True, [q2, q3, q4, q5, q6, q7] + q2 = np.arctan2(-R12, R22) # hip_yaw + s2, c2 = np.sin(q2), np.cos(q2) + q3 = np.arctan2(R32, -R12 * s2 + R22 * c2) # hip_roll + q4 = np.arctan2(-R31, R33) # hip_pitch - except Exception as exc: - print(f"[IKSolver] error ({leg}): {exc}") - return False, [] + return True, [q2, q3, q4, q5, q6, q7] diff --git a/mujoco_sim/sim.py b/mujoco_sim/sim.py index 94cc184..1736f27 100644 --- a/mujoco_sim/sim.py +++ b/mujoco_sim/sim.py @@ -1,30 +1,40 @@ -# Simulação do robô Robotis OP3 via MuJoCo -# Pedro H. Peres +""" +Robotis OP3 walking simulation in MuJoCo. + +Pipeline per control tick: + + WalkingEngine.step() → desired pelvis + sole poses (world frame) + IKSolver.solve() → 6 joint angles per leg + TorsoOrientationController (optional) → hip corrections from a simulated IMU + data.ctrl → PD position actuators → mj_step + +Run from the repository root: + + python -m mujoco_sim + +Author: Pedro H. Peres +""" -import os -import sys import time -import select -import termios -import tty -import numpy as np +from collections import deque + import mujoco import mujoco.viewer +import numpy as np -sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) -os.chdir(os.path.join(os.path.dirname(__file__), "..")) - -from mujoco_sim.Utils.ik_solver import IKSolver -from mujoco_sim.Utils.torso_controller import SimulatedIMU, TorsoOrientationController -from mujoco_sim.AnalyticalWalking.walking_engine import WalkingEngine -from mujoco_sim.AnalyticalWalking.config import ( - JOINT_SIGNS, WALK_PARAMS, ARM_POSE, PHYSICS, USE_MPC, - USE_TORSO_CTRL, TORSO_CTRL_PARAMS, - ACTUATOR_KP, ACTUATOR_KV, ACTUATOR_FORCE_RANGE, - joint_name as _joint_name, +from .analytical_walking.walking_engine import WalkingEngine, WalkTarget +from .config import ( + ACTUATOR_FORCE_RANGE, ACTUATOR_KP, ACTUATOR_KV, ARM_POSE, JOINT_SIGNS, + KEY_COMMANDS, PHYSICS, TORSO_CTRL_PARAMS, USE_TORSO_CTRL, WALK_PARAMS, + joint_name, ) +from .control.torso_controller import SimulatedIMU, TorsoOrientationController +from .kinematics.ik_solver import IKSolver +# Order of the angles returned by IKSolver.solve _IK_KEYS = ["hip_yaw", "hip_roll", "hip_pitch", "knee", "ank_pitch", "ank_roll"] +_LEGS = (("left", "l"), ("right", "r")) + def _rot_z(yaw: float) -> np.ndarray: c, s = np.cos(yaw), np.sin(yaw) @@ -32,68 +42,75 @@ def _rot_z(yaw: float) -> np.ndarray: def _yaw_to_quat(yaw: float) -> np.ndarray: + """Quaternion (w, x, y, z) for a rotation of `yaw` about the world z axis.""" return np.array([np.cos(yaw / 2.0), 0.0, 0.0, np.sin(yaw / 2.0)]) + def build_model() -> mujoco.MjModel: + """Load the OP3 MJCF, add a floor and tune solver, contacts and actuators.""" from robot_descriptions import op3_mj_description spec = mujoco.MjSpec.from_file(op3_mj_description.MJCF_PATH) - spec.option.timestep = 0.008 - spec.option.gravity = [0.0, 0.0, -9.81] - spec.option.integrator = mujoco.mjtIntegrator.mjINT_IMPLICIT - spec.option.solver = mujoco.mjtSolver.mjSOL_NEWTON - spec.option.impratio = 50.0 - spec.option.iterations = 40 + spec.option.timestep = 0.008 + spec.option.gravity = [0.0, 0.0, -9.81] + spec.option.integrator = mujoco.mjtIntegrator.mjINT_IMPLICIT + spec.option.solver = mujoco.mjtSolver.mjSOL_NEWTON + spec.option.impratio = 50.0 # stiffer friction cone → less foot slip + spec.option.iterations = 40 spec.option.noslip_iterations = 10 - spec.option.tolerance = 1e-4 + spec.option.tolerance = 1e-4 - _SOLREF = [0.010, 2.0] - _SOLIMP = [0.99, 0.9999, 0.0001, 0.5, 3] + # Stiff, nearly hard contacts between the feet and the floor + solref = [0.010, 2.0] + solimp = [0.99, 0.9999, 0.0001, 0.5, 3] floor = spec.worldbody.add_geom() - floor.name = "floor" - floor.type = mujoco.mjtGeom.mjGEOM_PLANE - floor.size = [0, 0, 0.1] - floor.pos = [0, 0, 0] - floor.rgba = [0.5, 0.5, 0.5, 1.0] - floor.contype = 1 + floor.name = "floor" + floor.type = mujoco.mjtGeom.mjGEOM_PLANE + floor.size = [0, 0, 0.1] + floor.pos = [0, 0, 0] + floor.rgba = [0.5, 0.5, 0.5, 1.0] + floor.contype = 1 floor.conaffinity = 1 - floor.solref = _SOLREF - floor.solimp = _SOLIMP + floor.solref = solref + floor.solimp = solimp + # Robot geoms collide with the floor only (no self-collision) for geom in spec.geoms: if geom.name == "floor": continue if geom.contype > 0 or geom.conaffinity > 0: geom.conaffinity = 0 - geom.solref = _SOLREF - geom.solimp = _SOLIMP + geom.solref = solref + geom.solimp = solimp model = spec.compile() + # Damping on the floating base (3 translational + 3 rotational DoFs) dof_start = model.jnt_dofadr[0] - model.dof_damping[dof_start:dof_start+3] = 3.0 - model.dof_damping[dof_start+3:dof_start+6] = 1.0 + model.dof_damping[dof_start:dof_start + 3] = 3.0 + model.dof_damping[dof_start + 3:dof_start + 6] = 1.0 - model.actuator_gainprm[:, 0] = ACTUATOR_KP - model.actuator_biasprm[:, 1] = -ACTUATOR_KP - model.actuator_biasprm[:, 2] = -ACTUATOR_KV + # Position actuators: torque = KP * (ctrl - q) - KV * q_dot + model.actuator_gainprm[:, 0] = ACTUATOR_KP + model.actuator_biasprm[:, 1] = -ACTUATOR_KP + model.actuator_biasprm[:, 2] = -ACTUATOR_KV model.actuator_forcerange[:, 0] = -ACTUATOR_FORCE_RANGE - model.actuator_forcerange[:, 1] = ACTUATOR_FORCE_RANGE + model.actuator_forcerange[:, 1] = ACTUATOR_FORCE_RANGE return model - def _build_maps(model: mujoco.MjModel) -> tuple[dict, dict, dict]: + """Joint name → qpos address, dof address and actuator index.""" qpos_addr: dict[str, int] = {} - dof_addr: dict[str, int] = {} + dof_addr: dict[str, int] = {} ctrl_addr: dict[str, int] = {} for i in range(model.njnt): name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) if name: qpos_addr[name] = model.jnt_qposadr[i] - dof_addr[name] = model.jnt_dofadr[i] + dof_addr[name] = model.jnt_dofadr[i] for i in range(model.nu): if model.actuator_trntype[i] == mujoco.mjtTrn.mjTRN_JOINT: jnt_id = model.actuator_trnid[i, 0] @@ -102,176 +119,196 @@ def _build_maps(model: mujoco.MjModel) -> tuple[dict, dict, dict]: ctrl_addr[name] = i return qpos_addr, dof_addr, ctrl_addr -def _set_free_joint(data: mujoco.MjData, model: mujoco.MjModel, - pos: np.ndarray, quat: np.ndarray) -> None: - adr = model.jnt_qposadr[0] - data.qpos[adr:adr+3] = pos - data.qpos[adr+3:adr+7] = quat - - -def _apply_ctrl(data: mujoco.MjData, - ctrl_addr: dict, - joint_angles: dict[str, float]) -> None: - for jname, angle in joint_angles.items(): - if jname in ctrl_addr: - data.ctrl[ctrl_addr[jname]] = angle - - -def _apply_joints(data: mujoco.MjData, - qpos_addr: dict, dof_addr: dict, - joint_angles: dict[str, float], - zero_vel: bool) -> None: - for jname, angle in joint_angles.items(): - if jname in qpos_addr: - data.qpos[qpos_addr[jname]] = angle - if zero_vel: - data.qvel[dof_addr[jname]] = 0.0 - - -def _solve_leg(ik: IKSolver, - body_pos: np.ndarray, body_yaw: float, - sole_pos: np.ndarray, sole_yaw: float, - leg: str, side: str) -> dict[str, float] | None: - ok, angles = ik.solve(body_pos, _rot_z(body_yaw), - sole_pos, _rot_z(sole_yaw), leg) - if not ok: - return None - return { - _joint_name(side, key): JOINT_SIGNS[key][side] * angles[idx] - for idx, key in enumerate(_IK_KEYS) - } + +class Simulation: + """OP3 model + walking engine + IK, advanced one control tick at a time.""" + + def __init__(self, physics: bool = PHYSICS, use_torso_ctrl: bool = USE_TORSO_CTRL): + self.physics = physics + self.use_torso_ctrl = use_torso_ctrl + self.imu = SimulatedIMU() + self.torso_ctrl = TorsoOrientationController(**TORSO_CTRL_PARAMS) + self.model = build_model() + self.data = mujoco.MjData(self.model) + self.qpos_addr, self.dof_addr, self.ctrl_addr = _build_maps(self.model) + self.ik = IKSolver() + self.engine = WalkingEngine(**WALK_PARAMS) + self.dt = self.model.opt.timestep + self.time = 0.0 + self.ik_failures = {"left": 0, "right": 0} + self.reset() + + @property + def body_pos(self) -> np.ndarray: + adr = self.model.jnt_qposadr[0] + return self.data.qpos[adr:adr + 3].copy() + + @property + def body_yaw(self) -> float: + adr = self.model.jnt_qposadr[0] + w, _, _, z = self.data.qpos[adr + 3:adr + 7] + return float(2.0 * np.arctan2(z, w)) + + def reset(self) -> None: + """Place the robot in the engine's standing pose.""" + mujoco.mj_resetData(self.model, self.data) + target = self.engine.standing_pose() + self._set_free_joint(target.body_pos, target.body_yaw) + for joint_angles in (*self._solve_legs(target), ARM_POSE): + self._set_joints(joint_angles) + self._set_ctrl(joint_angles) + mujoco.mj_forward(self.model, self.data) + + def step(self) -> None: + """Advance the engine and the simulation by one timestep.""" + target = self.engine.step(self.dt) + leg_angles = self._solve_legs(target) + + if self.use_torso_ctrl: + corrections = self.torso_ctrl.compute(self.imu.read(self.model, self.data)) + for joint_angles in leg_angles: + for name, delta in corrections.items(): + if name in joint_angles: + joint_angles[name] += delta + + if self.physics: + for joint_angles in (*leg_angles, ARM_POSE): + self._set_ctrl(joint_angles) + mujoco.mj_step(self.model, self.data) + else: + # Kinematic playback: teleport the pelvis and set joints directly + self._set_free_joint(target.body_pos, target.body_yaw) + for joint_angles in (*leg_angles, ARM_POSE): + self._set_joints(joint_angles) + self.data.qvel[:] = 0.0 + mujoco.mj_forward(self.model, self.data) + + self.time += self.dt + + def _solve_legs(self, target: WalkTarget) -> list[dict[str, float]]: + """IK for both legs. A leg whose IK fails is left out (keeps its last command).""" + R_body = _rot_z(target.body_yaw) + soles = {"left": (target.left_sole, target.left_yaw), + "right": (target.right_sole, target.right_yaw)} + result = [] + for leg, side in _LEGS: + sole_pos, sole_yaw = soles[leg] + ok, angles = self.ik.solve(target.body_pos, R_body, + sole_pos, _rot_z(sole_yaw), leg) + if not ok: + if self.ik_failures[leg] == 0: + print(f"[IK] {leg} leg target out of reach at t={self.time:.2f}s " + f"(further failures are only counted)") + self.ik_failures[leg] += 1 + continue + result.append({ + joint_name(side, key): JOINT_SIGNS[key][side] * angles[idx] + for idx, key in enumerate(_IK_KEYS) + }) + return result + + def _set_free_joint(self, pos: np.ndarray, yaw: float) -> None: + adr = self.model.jnt_qposadr[0] + self.data.qpos[adr:adr + 3] = pos + self.data.qpos[adr + 3:adr + 7] = _yaw_to_quat(yaw) + + def _set_ctrl(self, joint_angles: dict[str, float]) -> None: + for name, angle in joint_angles.items(): + if name in self.ctrl_addr: + self.data.ctrl[self.ctrl_addr[name]] = angle + + def _set_joints(self, joint_angles: dict[str, float]) -> None: + for name, angle in joint_angles.items(): + if name in self.qpos_addr: + self.data.qpos[self.qpos_addr[name]] = angle + + +# ----------------------------------------------------------------------------- +# Keyboard control (keys pressed with the viewer window focused) +# Every letter is already a viewer shortcut, so we use arrows and nav keys. +# The arrows only step the simulation in the viewer while it is paused. +# ----------------------------------------------------------------------------- + +_GLFW_KEYS = { + 265: "up", 264: "down", 263: "left", 262: "right", + 266: "page_up", 267: "page_down", 268: "home", 261: "delete", +} _KEY_HELP = ( - "Teclas (terminal em foco):\n" - " W=frente S=lateral R=rotação M=marcha X=parar Q=sair" + "Keys (viewer window focused):\n" + " ↑ / ↓ forward / backward\n" + " ← / → sidestep left / right\n" + " PgUp / PgDn turn left / right\n" + " Home march in place\n" + " Delete stop\n" + " Close the window to quit." ) -_term_fd = sys.stdin.fileno() -_term_old = None - -def _setup_terminal(): - global _term_old - _term_old = termios.tcgetattr(_term_fd) - tty.setcbreak(_term_fd) - -def _restore_terminal(): - if _term_old is not None: - termios.tcsetattr(_term_fd, termios.TCSADRAIN, _term_old) - -def _poll_key(engine: WalkingEngine) -> None: - if not select.select([sys.stdin], [], [], 0)[0]: - return - key = sys.stdin.read(1).lower() - if key == "w": engine.set_command(vx=0.05); print("[CMD] Frente") - elif key == "s": engine.set_command(vy=0.03); print("[CMD] Lateral") - elif key == "r": engine.set_command(wz=0.5); print("[CMD] Rotacao") - elif key == "m": engine.set_command(); print("[CMD] Marcha no lugar") - elif key == "x": engine.request_stop(); print("[CMD] Parando...") - elif key == "q": _restore_terminal(); os._exit(0) - -def main(): - model = build_model() - data = mujoco.MjData(model) - mujoco.mj_resetData(model, data) - - qpos_addr, dof_addr, ctrl_addr = _build_maps(model) - ik = IKSolver() - - if USE_MPC: - from mujoco_sim.MPCWalking.walking_engine import MPCWalkingEngine - from mujoco_sim.MPCWalking.config import MPC_PARAMS - engine = MPCWalkingEngine(dt=model.opt.timestep, **MPC_PARAMS) - else: - engine = WalkingEngine(**WALK_PARAMS) - - imu = SimulatedIMU() - torso_ctrl = TorsoOrientationController(**TORSO_CTRL_PARAMS) - - mode_label = "FÍSICA (mj_step)" if PHYSICS else "CINEMÁTICA (mj_forward)" - ctrl_label = "ON" if USE_TORSO_CTRL else "OFF" - print(f"\n{'='*50}") - print(f" MODO: {mode_label}") - print(f" Torso controller: {ctrl_label}") - print(f" Joints disponíveis ({len(qpos_addr)}): {sorted(qpos_addr)}") - print(f"{'='*50}\n") - - body_pos, body_yaw, l_sole, l_yaw, r_sole, r_yaw = engine._standing_pose() - _set_free_joint(data, model, body_pos, _yaw_to_quat(body_yaw)) - - l_angles = _solve_leg(ik, body_pos, body_yaw, l_sole, l_yaw, "left", "l") - r_angles = _solve_leg(ik, body_pos, body_yaw, r_sole, r_yaw, "right", "r") - if l_angles: - _apply_joints(data, qpos_addr, dof_addr, l_angles, zero_vel=False) - _apply_ctrl(data, ctrl_addr, l_angles) - if r_angles: - _apply_joints(data, qpos_addr, dof_addr, r_angles, zero_vel=False) - _apply_ctrl(data, ctrl_addr, r_angles) - _apply_joints(data, qpos_addr, dof_addr, ARM_POSE, zero_vel=False) - _apply_ctrl(data, ctrl_addr, ARM_POSE) - mujoco.mj_forward(model, data) - - dt = model.opt.timestep - sim_time = 0.0 - real_start = time.perf_counter() - _setup_terminal() - with mujoco.viewer.launch_passive(model, data) as viewer: +def _apply_key(engine: WalkingEngine, key: str) -> None: + vx = KEY_COMMANDS["forward"]["vx"] + vy = KEY_COMMANDS["lateral"]["vy"] + wz = KEY_COMMANDS["turn"]["wz"] + actions = { + "up": ("Forward", lambda: engine.set_command(vx=vx)), + "down": ("Backward", lambda: engine.set_command(vx=-vx)), + "left": ("Sidestep left", lambda: engine.set_command(vy=vy)), + "right": ("Sidestep right", lambda: engine.set_command(vy=-vy)), + "page_up": ("Turn left", lambda: engine.set_command(wz=wz)), + "page_down": ("Turn right", lambda: engine.set_command(wz=-wz)), + "home": ("March in place", engine.march_in_place), + "delete": ("Stopping...", engine.request_stop), + } + label, action = actions[key] + action() + print(f"[CMD] {label}") + + +def main() -> None: + sim = Simulation() + + mode_label = "PHYSICS (mj_step)" if sim.physics else "KINEMATICS (mj_forward)" + print(f"\n{'=' * 50}") + print(f" MODE: {mode_label}") + print(f" Torso controller: {'ON' if sim.use_torso_ctrl else 'OFF'}") + print(f" Joints ({len(sim.qpos_addr)}): {sorted(sim.qpos_addr)}") + print(f"{'=' * 50}\n") + print(_KEY_HELP, "\n") + + # The viewer calls key_callback from its own thread; queue the keys and + # apply them in the simulation loop. + pressed: deque[str] = deque() + + def key_callback(keycode: int) -> None: + if keycode in _GLFW_KEYS: + pressed.append(_GLFW_KEYS[keycode]) + + real_start = time.perf_counter() + with mujoco.viewer.launch_passive(sim.model, sim.data, key_callback=key_callback) as viewer: viewer.cam.lookat[:] = [0.0, 0.0, 0.2] - print(f"[MODO: {mode_label}]") - print(_KEY_HELP, "\n") while viewer.is_running(): - _poll_key(engine) - - body_pos, body_yaw, l_sole, l_yaw, r_sole, r_yaw = engine.step(dt) - - l_angles = _solve_leg(ik, body_pos, body_yaw, l_sole, l_yaw, "left", "l") - r_angles = _solve_leg(ik, body_pos, body_yaw, r_sole, r_yaw, "right", "r") - - if USE_TORSO_CTRL: - imu_reading = imu.read(model, data) - corrections = torso_ctrl.compute(imu_reading) - if l_angles: - for joint, delta in corrections.items(): - if joint in l_angles: - l_angles[joint] += delta - if r_angles: - for joint, delta in corrections.items(): - if joint in r_angles: - r_angles[joint] += delta - - if PHYSICS: - if USE_MPC: - # Open-loop: impõe a posição do torso diretamente (trajetória do LIPM). - # As pernas usam física para contato; o corpo segue o MPC sem lag. - _set_free_joint(data, model, body_pos, _yaw_to_quat(body_yaw)) - if l_angles: _apply_ctrl(data, ctrl_addr, l_angles) - if r_angles: _apply_ctrl(data, ctrl_addr, r_angles) - _apply_ctrl(data, ctrl_addr, ARM_POSE) - mujoco.mj_step(model, data) - else: - _set_free_joint(data, model, body_pos, _yaw_to_quat(body_yaw)) - if l_angles: _apply_joints(data, qpos_addr, dof_addr, l_angles, zero_vel=False) - if r_angles: _apply_joints(data, qpos_addr, dof_addr, r_angles, zero_vel=False) - _apply_joints(data, qpos_addr, dof_addr, ARM_POSE, zero_vel=False) - data.qvel[:] = 0.0 - mujoco.mj_forward(model, data) + while pressed: + _apply_key(sim.engine, pressed.popleft()) + with viewer.lock(): + sim.step() viewer.sync() - if int(sim_time / 0.5) != int((sim_time + dt) / 0.5): - adr = model.jnt_qposadr[0] - px, py, pz = data.qpos[adr], data.qpos[adr+1], data.qpos[adr+2] - if USE_TORSO_CTRL: - r = imu.read(model, data) + if int(sim.time / 0.5) != int((sim.time - sim.dt) / 0.5): + px, py, pz = sim.body_pos + imu_str = "" + if sim.use_torso_ctrl: + r = sim.imu.read(sim.model, sim.data) imu_str = f" imu=({np.degrees(r['pitch']):.1f}°p, {np.degrees(r['roll']):.1f}°r)" - else: - imu_str = "" - print(f"[t={sim_time:.1f}s] pos=({px:.3f},{py:.3f},{pz:.3f}) state={engine.state}{imu_str}") + print(f"[t={sim.time:.1f}s] pos=({px:.3f},{py:.3f},{pz:.3f}) " + f"state={sim.engine.state.name}{imu_str}") - sim_time += dt - elapsed = time.perf_counter() - real_start - sleep_time = sim_time - elapsed + # Keep the simulation in sync with wall-clock time + sleep_time = sim.time - (time.perf_counter() - real_start) if sleep_time > 0: time.sleep(sleep_time) - _restore_terminal() + + failures = sum(sim.ik_failures.values()) + if failures: + print(f"[IK] {failures} failed solves during the run: {sim.ik_failures}") diff --git a/pybullet_sim/__pycache__/caminhada_relativa.cpython-310.pyc b/pybullet_sim/__pycache__/caminhada_relativa.cpython-310.pyc deleted file mode 100644 index 36af00d..0000000 Binary files a/pybullet_sim/__pycache__/caminhada_relativa.cpython-310.pyc and /dev/null differ diff --git a/pybullet_sim/__pycache__/ik_solver.cpython-310.pyc b/pybullet_sim/__pycache__/ik_solver.cpython-310.pyc deleted file mode 100644 index 4181512..0000000 Binary files a/pybullet_sim/__pycache__/ik_solver.cpython-310.pyc and /dev/null differ diff --git a/pybullet_sim/__pycache__/ik_solver.cpython-311.pyc b/pybullet_sim/__pycache__/ik_solver.cpython-311.pyc deleted file mode 100644 index afa0af2..0000000 Binary files a/pybullet_sim/__pycache__/ik_solver.cpython-311.pyc and /dev/null differ diff --git a/pybullet_sim/__pycache__/lipm_model.cpython-310.pyc b/pybullet_sim/__pycache__/lipm_model.cpython-310.pyc deleted file mode 100644 index bc2bae6..0000000 Binary files a/pybullet_sim/__pycache__/lipm_model.cpython-310.pyc and /dev/null differ diff --git a/pybullet_sim/__pycache__/mpc_walker.cpython-310.pyc b/pybullet_sim/__pycache__/mpc_walker.cpython-310.pyc deleted file mode 100644 index 00acf79..0000000 Binary files a/pybullet_sim/__pycache__/mpc_walker.cpython-310.pyc and /dev/null differ diff --git a/pybullet_sim/__pycache__/state_estimator.cpython-310.pyc b/pybullet_sim/__pycache__/state_estimator.cpython-310.pyc deleted file mode 100644 index 4eae4fd..0000000 Binary files a/pybullet_sim/__pycache__/state_estimator.cpython-310.pyc and /dev/null differ diff --git a/pybullet_sim/__pycache__/trajectory_generator.cpython-310.pyc b/pybullet_sim/__pycache__/trajectory_generator.cpython-310.pyc deleted file mode 100644 index 20f221e..0000000 Binary files a/pybullet_sim/__pycache__/trajectory_generator.cpython-310.pyc and /dev/null differ diff --git a/pybullet_sim/__pycache__/trajectory_generator.cpython-311.pyc b/pybullet_sim/__pycache__/trajectory_generator.cpython-311.pyc deleted file mode 100644 index 284e4c7..0000000 Binary files a/pybullet_sim/__pycache__/trajectory_generator.cpython-311.pyc and /dev/null differ diff --git a/pybullet_sim/__pycache__/walk_engine.cpython-310.pyc b/pybullet_sim/__pycache__/walk_engine.cpython-310.pyc deleted file mode 100644 index 6b2b2ff..0000000 Binary files a/pybullet_sim/__pycache__/walk_engine.cpython-310.pyc and /dev/null differ diff --git a/pybullet_sim/urdf/aurea_urdf_pkg.urdf b/pybullet_sim/assets/aurea/aurea.urdf similarity index 95% rename from pybullet_sim/urdf/aurea_urdf_pkg.urdf rename to pybullet_sim/assets/aurea/aurea.urdf index 5a84928..bd2dcff 100644 --- a/pybullet_sim/urdf/aurea_urdf_pkg.urdf +++ b/pybullet_sim/assets/aurea/aurea.urdf @@ -1,1218 +1,1218 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - edrom_lowlevel/AureaHardwareInterface - /dev/ttyUSB0 - 1000000 - - - - - - 1 - 1.0 - false - - - - - 2 - 1.0 - false - - - - - - 10 - 2.0 - false - - - - - 12 - 2.0 - true - - - - - 14 - 2.0 - true - - - - - 16 - 2.0 - true - - - - - 18 - 2.0 - true - - - - - 20 - 2.0 - false - - - - - - 9 - 2.0 - false - - - - - 11 - 2.0 - true - - - - - 13 - 2.0 - true - - - - - 15 - 2.0 - false - - - - - 17 - 2.0 - true - - - - - 19 - 2.0 - false - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + edrom_lowlevel/AureaHardwareInterface + /dev/ttyUSB0 + 1000000 + + + + + + 1 + 1.0 + false + + + + + 2 + 1.0 + false + + + + + + 10 + 2.0 + false + + + + + 12 + 2.0 + true + + + + + 14 + 2.0 + true + + + + + 16 + 2.0 + true + + + + + 18 + 2.0 + true + + + + + 20 + 2.0 + false + + + + + + 9 + 2.0 + false + + + + + 11 + 2.0 + true + + + + + 13 + 2.0 + true + + + + + 15 + 2.0 + false + + + + + 17 + 2.0 + true + + + + + 19 + 2.0 + false + + + diff --git a/pybullet_sim/urdf/aurea_urdf_pkg.csv b/pybullet_sim/assets/aurea/aurea_solidworks_export.csv similarity index 99% rename from pybullet_sim/urdf/aurea_urdf_pkg.csv rename to pybullet_sim/assets/aurea/aurea_solidworks_export.csv index ff81c7f..093a3d0 100644 --- a/pybullet_sim/urdf/aurea_urdf_pkg.csv +++ b/pybullet_sim/assets/aurea/aurea_solidworks_export.csv @@ -1,22 +1,22 @@ -Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity -base_link,"-0,0054386","-0,001346","0,071435",0,0,0,"1,4383","0,002822","1,8394E-05","7,3373E-06","0,0011477","1,1908E-06","0,0035028",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/base_link.STL,"0,75294","0,75294","0,75294",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/base_link.STL,,BODY-1,com_origin,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, -head_pan_link,"6,8372E-10","0,0084419","0,042088",0,0,0,"0,0042427","3,8746E-07","-1,3861E-16","3,3793E-14","2,1011E-07","3,2642E-12","4,6187E-07",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/head_pan_link.STL,"0,75294","0,75294","0,75294",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/head_pan_link.STL,,NECK-1,head_pan_origin,head_pan_axis,head_pan,continuous,"0,011996","0,00071768","0,1444",0,0,0,base_link,0,0,1,,,,,,,,,,,, -head_tilt_link,"0,018438","0,00044728","0,041065",0,0,0,"0,06657","5,0882E-05","-2,503E-07","-1,8977E-06","1,4263E-05","1,2974E-08","4,3876E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/head_tilt_link.STL,"0,75294","0,75294","0,75294",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/head_tilt_link.STL,,HEAD-1,head_tilt_origin,head_tilt_axis,head_tilt,continuous,0,"0,0002","0,06104",0,0,0,head_pan_link,0,1,0,,,,,,,,,,,, -l_sho_pitch_link,"0,00352137182401335","0,0407154850483834","-0,00945859812868083",0,0,0,"0,0265849110122631","7,26212411316309E-06","6,5171693357601E-08","1,06977457104858E-08","1,68111887165657E-05","1,56116573724164E-06","1,65455750032948E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_sho_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_sho_pitch_link.STL,,LARM_1(2)-1,l_sho_p_origin,l_sho_pitch_axis,l_sho_pitch,continuous,"0,012683","0,068258","0,1366",0,0,0,base_link,0,1,0,,,,,,,,,,,, -l_sho_roll_link,"0,000189659734588288","0,0505998821833899","-0,00015618572152401",0,0,0,"0,197011517994657","7,72328719397364E-05","-3,30651837574508E-07","5,34864466021505E-09","5,39299927610623E-05","1,71324199171907E-07","7,72363058962331E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_sho_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_sho_roll_link.STL,,LARM_2(2)-1,l_sho_r_origin,l_sho_roll_axis,l_sho_roll,continuous,"0,00084166","0,049985","-0,03201",0,0,0,l_sho_pitch_link,1,0,0,,,,,,,,,,,, -l_el_link,"3,40772950824242E-06","0,0812441997050536","0,0163078157472427",0,0,0,"0,0742846959386868","0,000257242445168631","2,40125472668084E-09","7,63997467824725E-09","2,53349920387169E-05","-2,81862911803432E-05","0,000251570298661733",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_el_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_el_link.STL,,LARM_3(2)-1,l_el_origin,l_el_axis,l_el,continuous,"0,00069168","0,1012",0,0,0,0,l_sho_roll_link,0,0,1,,,,,,,,,,,, -r_sho_pitch_link,"-0,00324300761837326","-0,0407386018885676","-0,00945859812868109",0,0,0,"0,0265849110122628","7,26167934268369E-06","-9,87611207484539E-11","-2,72495337082694E-11","1,68116334870447E-05","-1,56120238923654E-06","1,65455750032945E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_sho_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_sho_pitch_link.STL,,RARM_1(2)-1,r_sho_p_origin,r_sho_pitch_axis,r_sho_pitch,continuous,"0,011753","-0,067739","0,1366",0,0,0,base_link,0,1,0,,,,,,,,,,,, -r_sho_roll_link,"-0,00018965852905839","-0,0505998890321953","-0,000156184466288206",0,0,0,"0,197011518002432","7,7232874661571E-05","-3,30654304600183E-07","-5,34863006420127E-09","5,39299928078269E-05","-1,71321741828796E-07","7,72363031557339E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_sho_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_sho_roll_link.STL,,RARM_2(2)-1,r_sho_r_origin,r_sho_roll_axis,r_sho_roll,continuous,0,"-0,04999","-0,03201",0,0,0,r_sho_pitch_link,1,0,0,,,,,,,,,,,, -r_el_link,"3,69373478137369E-07","-0,0812441997050545","0,0163078098038721",0,0,0,"0,0742846959386874","0,000257242445168637","1,40129351675067E-08","-7,6399746782357E-09","2,53349920387171E-05","2,81863008773858E-05","0,000251570298661738",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_el_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_el_link.STL,,RARM_3(2)-1,r_el_origin,r_el_axis,r_el,continuous,"-0,00069168","-0,1012",0,0,0,0,r_sho_roll_link,0,0,1,,,,,,,,,,,, -r_hip_yaw_link,0,0,"-0,0359900474652093",0,0,0,"0,00694060291701572","1,66514174382027E-06","7,61187511128963E-14","1,13106493815015E-21","4,14964511041944E-06","4,47500627637193E-22","4,00421747655342E-06",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_yaw_link.STL,"0,792156862745098","0,819607843137255","0,933333333333333",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_yaw_link.STL,,RLEG_1(2)-1,r_hip_y_origin,r_hip_yaw_axis,r_hip_yaw,continuous,0,"-0,0425",0,0,0,0,base_link,0,0,1,,,,,,,,,,,, -r_hip_roll_link,"0,0298644539096895","9,54006661061657E-05","-0,015199101611021",0,0,0,"0,183944855809613","7,35153703278313E-05","-9,12174690443411E-13","-1,99657590840599E-07","7,2145316762463E-05","1,71352044892206E-07","4,46900690426448E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_roll_link.STL,,RLEG_2(2)-1,r_hip_r_origin,r_hip_roll_axis,r_hip_roll,revolute,"-0,054","-0,0005","-0,062",0,0,0,r_hip_yaw_link,1,0,0,0,0,0,0,,,,,,,, -r_hip_pitch_link,"0,000158089791263194","0,000105595270809319","-0,0854824898523879",0,0,0,"0,126175620271201","6,48140911230731E-05","2,10740537539349E-09","1,1644914411195E-06","6,27191148165412E-05","-1,73025564156333E-07","2,69153394295054E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_pitch_link.STL,,RLEG_3(2)-1,r_hip_p_origin,r_hip_pitch_axis,r_hip_pitch,continuous,"0,054","0,0005",0,0,0,0,r_hip_roll_link,0,1,0,,,,,,,,,,,, -r_knee_link,0,"-0,000249999999999945","-0,0419003925928639",0,0,0,"0,0371694656443498","3,02487395406759E-05","2,83795802988007E-21","2,69721239634144E-19","3,30337384651361E-05","-2,46337337062787E-21","2,81492271510805E-06",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_knee_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_knee_link.STL,,RLEG_4(2)-1,r_knee_origin,r_knee_axis,r_knee,continuous,0,"-0,00043","-0,12",0,0,0,r_hip_pitch_link,0,1,0,,,,,,,,,,,, -r_ank_pitch_link,"-0,0241355511158929","0,00040459916222195","0,0151991016063729",0,0,0,"0,183944856002735","7,35153675760217E-05","-9,68086292419633E-13","1,99652664804746E-07","7,21453195202993E-05","1,71354502622176E-07","4,46900690405164E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_ank_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_ank_pitch_link.STL,,RLEG_5(2)-1,r_ank_p_origin,r_ank_pitch_axis,r_ank_pitch,continuous,0,"-0,0005","-0,085",0,0,0,r_knee_link,0,1,0,,,,,,,,,,,, -r_ank_roll_link,"0,0539999996375236","-0,0118311818058534","-0,035114625516821",0,0,0,"0,0877025419110131","4,99364719914314E-05","2,18241108911093E-12","-8,1038721944463E-20","0,000121170006996388","-2,73345204332721E-20","0,000166148487178617",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_ank_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_ank_roll_link.STL,,RLEG_6(2)-1,r_ank_r_origin,r_ank_roll_axis,r_ank_roll,continuous,"-0,054",0,0,0,0,0,r_ank_pitch_link,1,0,0,,,,,,,,,,,, -l_hip_yaw_link,"6,66133814775094E-16","-1,04083408558608E-16","-0,0359900474652092",0,0,0,"0,0187396278759425","4,49588270831473E-06","1,64070596376897E-21","2,90408135599367E-21","1,12040417981325E-05","2,72994201449648E-21","1,08113871866942E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_yaw_link.STL,"0,898039215686275","0,917647058823529","0,929411764705882",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_yaw_link.STL,,LLEG_1(2)-1,l_hip_y_origin,l_hip_yaw_axis,l_hip_yaw,continuous,0,"0,0425",0,0,0,0,base_link,0,0,1,,,,,,,,,,,, -l_hip_roll_link,"0,029864450820877","9,54018942858591E-05","-0,0151991003922303",0,0,0,"0,183944868508389","7,35153751264597E-05","4,82256065759444E-14","-1,99653903762223E-07","7,21453243143375E-05","1,71350802680741E-07","4,46900721055964E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_roll_link.STL,,LLEG_2(2)-1,l_hip_r_origin,l_hip_roll_axis,l_hip_roll,continuous,"-0,054","-0,0005","-0,062",0,0,0,l_hip_yaw_link,1,0,0,,,,,,,,,,,, -l_hip_pitch_link,"0,000158090270155786","0,000105593836474918","-0,0854824861313004",0,0,0,"0,126175607572412","6,4814083571068E-05","2,10655113465296E-09","1,16449268251577E-06","6,27191100180411E-05","-1,73029220807411E-07","2,69153363665543E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_pitch_link.STL,,LLEG_3(2)-1,l_hip_p_origin,l_hip_pitch_axis,l_hip_pitch,continuous,"0,054","0,0005",0,0,0,0,l_hip_roll_link,0,1,0,,,,,,,,,,,, -l_knee_link,"-3,33066907387547E-16","-0,000250000000000136","-0,0419003925928641",0,0,0,"0,0371694656443498","3,02487395406759E-05","-4,12887867253622E-21","1,06646590271131E-21","3,30337384651361E-05","-8,46596202007678E-21","2,81492271510806E-06",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_knee_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_knee_link.STL,,LLEG_4(2)-1,l_knee_origin,l_knee_axis,l_knee,continuous,0,"-0,00043","-0,12",0,0,0,l_hip_pitch_link,0,1,0,,,,,,,,,,,, -l_ank_pitch_link,"-0,0241355491298787","0,000404598110121463","0,0151991003875814",0,0,0,"0,183944868701523","7,35153751278978E-05","-4,96608920988143E-14","1,99653900899916E-07","7,21453243189286E-05","1,71350810294181E-07","4,46900721034675E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_ank_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_ank_pitch_link.STL,,LLEG_5(2)-1,l_ank_p_origin,l_ank_pitch_axis,l_ank_pitch,continuous,0,"-0,0005","-0,085",0,0,0,l_knee_link,0,1,0,,,,,,,,,,,, -l_ank_roll_link,"0,0539999999999995","0,0113311801514332","-0,0351146255168209",0,0,0,"0,0877025419110131","4,99364719914313E-05","-1,8100008925471E-20","-2,31402748877954E-20","0,000121170006996388","-8,68904764115959E-23","0,000166148487178617",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_ank_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_ank_roll_link.STL,,LLEG_6(2)-1,l_ank_r_origin,l_ank_roll_axis,l_ank_roll,continuous,"-0,054","0,0005",0,0,0,0,l_ank_pitch_link,1,0,0,,,,,,,,,,,, +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +base_link,"-0,0054386","-0,001346","0,071435",0,0,0,"1,4383","0,002822","1,8394E-05","7,3373E-06","0,0011477","1,1908E-06","0,0035028",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/base_link.STL,"0,75294","0,75294","0,75294",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/base_link.STL,,BODY-1,com_origin,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, +head_pan_link,"6,8372E-10","0,0084419","0,042088",0,0,0,"0,0042427","3,8746E-07","-1,3861E-16","3,3793E-14","2,1011E-07","3,2642E-12","4,6187E-07",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/head_pan_link.STL,"0,75294","0,75294","0,75294",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/head_pan_link.STL,,NECK-1,head_pan_origin,head_pan_axis,head_pan,continuous,"0,011996","0,00071768","0,1444",0,0,0,base_link,0,0,1,,,,,,,,,,,, +head_tilt_link,"0,018438","0,00044728","0,041065",0,0,0,"0,06657","5,0882E-05","-2,503E-07","-1,8977E-06","1,4263E-05","1,2974E-08","4,3876E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/head_tilt_link.STL,"0,75294","0,75294","0,75294",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/head_tilt_link.STL,,HEAD-1,head_tilt_origin,head_tilt_axis,head_tilt,continuous,0,"0,0002","0,06104",0,0,0,head_pan_link,0,1,0,,,,,,,,,,,, +l_sho_pitch_link,"0,00352137182401335","0,0407154850483834","-0,00945859812868083",0,0,0,"0,0265849110122631","7,26212411316309E-06","6,5171693357601E-08","1,06977457104858E-08","1,68111887165657E-05","1,56116573724164E-06","1,65455750032948E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_sho_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_sho_pitch_link.STL,,LARM_1(2)-1,l_sho_p_origin,l_sho_pitch_axis,l_sho_pitch,continuous,"0,012683","0,068258","0,1366",0,0,0,base_link,0,1,0,,,,,,,,,,,, +l_sho_roll_link,"0,000189659734588288","0,0505998821833899","-0,00015618572152401",0,0,0,"0,197011517994657","7,72328719397364E-05","-3,30651837574508E-07","5,34864466021505E-09","5,39299927610623E-05","1,71324199171907E-07","7,72363058962331E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_sho_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_sho_roll_link.STL,,LARM_2(2)-1,l_sho_r_origin,l_sho_roll_axis,l_sho_roll,continuous,"0,00084166","0,049985","-0,03201",0,0,0,l_sho_pitch_link,1,0,0,,,,,,,,,,,, +l_el_link,"3,40772950824242E-06","0,0812441997050536","0,0163078157472427",0,0,0,"0,0742846959386868","0,000257242445168631","2,40125472668084E-09","7,63997467824725E-09","2,53349920387169E-05","-2,81862911803432E-05","0,000251570298661733",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_el_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_el_link.STL,,LARM_3(2)-1,l_el_origin,l_el_axis,l_el,continuous,"0,00069168","0,1012",0,0,0,0,l_sho_roll_link,0,0,1,,,,,,,,,,,, +r_sho_pitch_link,"-0,00324300761837326","-0,0407386018885676","-0,00945859812868109",0,0,0,"0,0265849110122628","7,26167934268369E-06","-9,87611207484539E-11","-2,72495337082694E-11","1,68116334870447E-05","-1,56120238923654E-06","1,65455750032945E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_sho_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_sho_pitch_link.STL,,RARM_1(2)-1,r_sho_p_origin,r_sho_pitch_axis,r_sho_pitch,continuous,"0,011753","-0,067739","0,1366",0,0,0,base_link,0,1,0,,,,,,,,,,,, +r_sho_roll_link,"-0,00018965852905839","-0,0505998890321953","-0,000156184466288206",0,0,0,"0,197011518002432","7,7232874661571E-05","-3,30654304600183E-07","-5,34863006420127E-09","5,39299928078269E-05","-1,71321741828796E-07","7,72363031557339E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_sho_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_sho_roll_link.STL,,RARM_2(2)-1,r_sho_r_origin,r_sho_roll_axis,r_sho_roll,continuous,0,"-0,04999","-0,03201",0,0,0,r_sho_pitch_link,1,0,0,,,,,,,,,,,, +r_el_link,"3,69373478137369E-07","-0,0812441997050545","0,0163078098038721",0,0,0,"0,0742846959386874","0,000257242445168637","1,40129351675067E-08","-7,6399746782357E-09","2,53349920387171E-05","2,81863008773858E-05","0,000251570298661738",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_el_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_el_link.STL,,RARM_3(2)-1,r_el_origin,r_el_axis,r_el,continuous,"-0,00069168","-0,1012",0,0,0,0,r_sho_roll_link,0,0,1,,,,,,,,,,,, +r_hip_yaw_link,0,0,"-0,0359900474652093",0,0,0,"0,00694060291701572","1,66514174382027E-06","7,61187511128963E-14","1,13106493815015E-21","4,14964511041944E-06","4,47500627637193E-22","4,00421747655342E-06",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_yaw_link.STL,"0,792156862745098","0,819607843137255","0,933333333333333",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_yaw_link.STL,,RLEG_1(2)-1,r_hip_y_origin,r_hip_yaw_axis,r_hip_yaw,continuous,0,"-0,0425",0,0,0,0,base_link,0,0,1,,,,,,,,,,,, +r_hip_roll_link,"0,0298644539096895","9,54006661061657E-05","-0,015199101611021",0,0,0,"0,183944855809613","7,35153703278313E-05","-9,12174690443411E-13","-1,99657590840599E-07","7,2145316762463E-05","1,71352044892206E-07","4,46900690426448E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_roll_link.STL,,RLEG_2(2)-1,r_hip_r_origin,r_hip_roll_axis,r_hip_roll,revolute,"-0,054","-0,0005","-0,062",0,0,0,r_hip_yaw_link,1,0,0,0,0,0,0,,,,,,,, +r_hip_pitch_link,"0,000158089791263194","0,000105595270809319","-0,0854824898523879",0,0,0,"0,126175620271201","6,48140911230731E-05","2,10740537539349E-09","1,1644914411195E-06","6,27191148165412E-05","-1,73025564156333E-07","2,69153394295054E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_hip_pitch_link.STL,,RLEG_3(2)-1,r_hip_p_origin,r_hip_pitch_axis,r_hip_pitch,continuous,"0,054","0,0005",0,0,0,0,r_hip_roll_link,0,1,0,,,,,,,,,,,, +r_knee_link,0,"-0,000249999999999945","-0,0419003925928639",0,0,0,"0,0371694656443498","3,02487395406759E-05","2,83795802988007E-21","2,69721239634144E-19","3,30337384651361E-05","-2,46337337062787E-21","2,81492271510805E-06",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_knee_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_knee_link.STL,,RLEG_4(2)-1,r_knee_origin,r_knee_axis,r_knee,continuous,0,"-0,00043","-0,12",0,0,0,r_hip_pitch_link,0,1,0,,,,,,,,,,,, +r_ank_pitch_link,"-0,0241355511158929","0,00040459916222195","0,0151991016063729",0,0,0,"0,183944856002735","7,35153675760217E-05","-9,68086292419633E-13","1,99652664804746E-07","7,21453195202993E-05","1,71354502622176E-07","4,46900690405164E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_ank_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_ank_pitch_link.STL,,RLEG_5(2)-1,r_ank_p_origin,r_ank_pitch_axis,r_ank_pitch,continuous,0,"-0,0005","-0,085",0,0,0,r_knee_link,0,1,0,,,,,,,,,,,, +r_ank_roll_link,"0,0539999996375236","-0,0118311818058534","-0,035114625516821",0,0,0,"0,0877025419110131","4,99364719914314E-05","2,18241108911093E-12","-8,1038721944463E-20","0,000121170006996388","-2,73345204332721E-20","0,000166148487178617",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_ank_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/r_ank_roll_link.STL,,RLEG_6(2)-1,r_ank_r_origin,r_ank_roll_axis,r_ank_roll,continuous,"-0,054",0,0,0,0,0,r_ank_pitch_link,1,0,0,,,,,,,,,,,, +l_hip_yaw_link,"6,66133814775094E-16","-1,04083408558608E-16","-0,0359900474652092",0,0,0,"0,0187396278759425","4,49588270831473E-06","1,64070596376897E-21","2,90408135599367E-21","1,12040417981325E-05","2,72994201449648E-21","1,08113871866942E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_yaw_link.STL,"0,898039215686275","0,917647058823529","0,929411764705882",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_yaw_link.STL,,LLEG_1(2)-1,l_hip_y_origin,l_hip_yaw_axis,l_hip_yaw,continuous,0,"0,0425",0,0,0,0,base_link,0,0,1,,,,,,,,,,,, +l_hip_roll_link,"0,029864450820877","9,54018942858591E-05","-0,0151991003922303",0,0,0,"0,183944868508389","7,35153751264597E-05","4,82256065759444E-14","-1,99653903762223E-07","7,21453243143375E-05","1,71350802680741E-07","4,46900721055964E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_roll_link.STL,,LLEG_2(2)-1,l_hip_r_origin,l_hip_roll_axis,l_hip_roll,continuous,"-0,054","-0,0005","-0,062",0,0,0,l_hip_yaw_link,1,0,0,,,,,,,,,,,, +l_hip_pitch_link,"0,000158090270155786","0,000105593836474918","-0,0854824861313004",0,0,0,"0,126175607572412","6,4814083571068E-05","2,10655113465296E-09","1,16449268251577E-06","6,27191100180411E-05","-1,73029220807411E-07","2,69153363665543E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_hip_pitch_link.STL,,LLEG_3(2)-1,l_hip_p_origin,l_hip_pitch_axis,l_hip_pitch,continuous,"0,054","0,0005",0,0,0,0,l_hip_roll_link,0,1,0,,,,,,,,,,,, +l_knee_link,"-3,33066907387547E-16","-0,000250000000000136","-0,0419003925928641",0,0,0,"0,0371694656443498","3,02487395406759E-05","-4,12887867253622E-21","1,06646590271131E-21","3,30337384651361E-05","-8,46596202007678E-21","2,81492271510806E-06",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_knee_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_knee_link.STL,,LLEG_4(2)-1,l_knee_origin,l_knee_axis,l_knee,continuous,0,"-0,00043","-0,12",0,0,0,l_hip_pitch_link,0,1,0,,,,,,,,,,,, +l_ank_pitch_link,"-0,0241355491298787","0,000404598110121463","0,0151991003875814",0,0,0,"0,183944868701523","7,35153751278978E-05","-4,96608920988143E-14","1,99653900899916E-07","7,21453243189286E-05","1,71350810294181E-07","4,46900721034675E-05",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_ank_pitch_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_ank_pitch_link.STL,,LLEG_5(2)-1,l_ank_p_origin,l_ank_pitch_axis,l_ank_pitch,continuous,0,"-0,0005","-0,085",0,0,0,l_knee_link,0,1,0,,,,,,,,,,,, +l_ank_roll_link,"0,0539999999999995","0,0113311801514332","-0,0351146255168209",0,0,0,"0,0877025419110131","4,99364719914313E-05","-1,8100008925471E-20","-2,31402748877954E-20","0,000121170006996388","-8,68904764115959E-23","0,000166148487178617",0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_ank_roll_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://aurea_urdf_pkg/meshes/l_ank_roll_link.STL,,LLEG_6(2)-1,l_ank_r_origin,l_ank_roll_axis,l_ank_roll,continuous,"-0,054","0,0005",0,0,0,0,l_ank_pitch_link,1,0,0,,,,,,,,,,,, diff --git a/pybullet_sim/meshes/base_link.STL b/pybullet_sim/assets/aurea/meshes/base_link.STL similarity index 100% rename from pybullet_sim/meshes/base_link.STL rename to pybullet_sim/assets/aurea/meshes/base_link.STL diff --git a/pybullet_sim/meshes/head_pan_link.STL b/pybullet_sim/assets/aurea/meshes/head_pan_link.STL similarity index 100% rename from pybullet_sim/meshes/head_pan_link.STL rename to pybullet_sim/assets/aurea/meshes/head_pan_link.STL diff --git a/pybullet_sim/meshes/head_tilt_link.STL b/pybullet_sim/assets/aurea/meshes/head_tilt_link.STL similarity index 100% rename from pybullet_sim/meshes/head_tilt_link.STL rename to pybullet_sim/assets/aurea/meshes/head_tilt_link.STL diff --git a/pybullet_sim/meshes/l_ank_pitch_link.STL b/pybullet_sim/assets/aurea/meshes/l_ank_pitch_link.STL similarity index 100% rename from pybullet_sim/meshes/l_ank_pitch_link.STL rename to pybullet_sim/assets/aurea/meshes/l_ank_pitch_link.STL diff --git a/pybullet_sim/meshes/l_ank_roll_link.STL b/pybullet_sim/assets/aurea/meshes/l_ank_roll_link.STL similarity index 100% rename from pybullet_sim/meshes/l_ank_roll_link.STL rename to pybullet_sim/assets/aurea/meshes/l_ank_roll_link.STL diff --git a/pybullet_sim/meshes/l_el_link.STL b/pybullet_sim/assets/aurea/meshes/l_el_link.STL similarity index 100% rename from pybullet_sim/meshes/l_el_link.STL rename to pybullet_sim/assets/aurea/meshes/l_el_link.STL diff --git a/pybullet_sim/meshes/l_hip_pitch_link.STL b/pybullet_sim/assets/aurea/meshes/l_hip_pitch_link.STL similarity index 100% rename from pybullet_sim/meshes/l_hip_pitch_link.STL rename to pybullet_sim/assets/aurea/meshes/l_hip_pitch_link.STL diff --git a/pybullet_sim/meshes/l_hip_roll_link.STL b/pybullet_sim/assets/aurea/meshes/l_hip_roll_link.STL similarity index 100% rename from pybullet_sim/meshes/l_hip_roll_link.STL rename to pybullet_sim/assets/aurea/meshes/l_hip_roll_link.STL diff --git a/pybullet_sim/meshes/l_hip_yaw_link.STL b/pybullet_sim/assets/aurea/meshes/l_hip_yaw_link.STL similarity index 100% rename from pybullet_sim/meshes/l_hip_yaw_link.STL rename to pybullet_sim/assets/aurea/meshes/l_hip_yaw_link.STL diff --git a/pybullet_sim/meshes/l_knee_link.STL b/pybullet_sim/assets/aurea/meshes/l_knee_link.STL similarity index 100% rename from pybullet_sim/meshes/l_knee_link.STL rename to pybullet_sim/assets/aurea/meshes/l_knee_link.STL diff --git a/pybullet_sim/meshes/l_sho_pitch_link.STL b/pybullet_sim/assets/aurea/meshes/l_sho_pitch_link.STL similarity index 100% rename from pybullet_sim/meshes/l_sho_pitch_link.STL rename to pybullet_sim/assets/aurea/meshes/l_sho_pitch_link.STL diff --git a/pybullet_sim/meshes/l_sho_roll_link.STL b/pybullet_sim/assets/aurea/meshes/l_sho_roll_link.STL similarity index 100% rename from pybullet_sim/meshes/l_sho_roll_link.STL rename to pybullet_sim/assets/aurea/meshes/l_sho_roll_link.STL diff --git a/pybullet_sim/meshes/r_ank_pitch_link.STL b/pybullet_sim/assets/aurea/meshes/r_ank_pitch_link.STL similarity index 100% rename from pybullet_sim/meshes/r_ank_pitch_link.STL rename to pybullet_sim/assets/aurea/meshes/r_ank_pitch_link.STL diff --git a/pybullet_sim/meshes/r_ank_roll_link.STL b/pybullet_sim/assets/aurea/meshes/r_ank_roll_link.STL similarity index 100% rename from pybullet_sim/meshes/r_ank_roll_link.STL rename to pybullet_sim/assets/aurea/meshes/r_ank_roll_link.STL diff --git a/pybullet_sim/meshes/r_el_link.STL b/pybullet_sim/assets/aurea/meshes/r_el_link.STL similarity index 100% rename from pybullet_sim/meshes/r_el_link.STL rename to pybullet_sim/assets/aurea/meshes/r_el_link.STL diff --git a/pybullet_sim/meshes/r_hip_pitch_link.STL b/pybullet_sim/assets/aurea/meshes/r_hip_pitch_link.STL similarity index 100% rename from pybullet_sim/meshes/r_hip_pitch_link.STL rename to pybullet_sim/assets/aurea/meshes/r_hip_pitch_link.STL diff --git a/pybullet_sim/meshes/r_hip_roll_link.STL b/pybullet_sim/assets/aurea/meshes/r_hip_roll_link.STL similarity index 100% rename from pybullet_sim/meshes/r_hip_roll_link.STL rename to pybullet_sim/assets/aurea/meshes/r_hip_roll_link.STL diff --git a/pybullet_sim/meshes/r_hip_yaw_link.STL b/pybullet_sim/assets/aurea/meshes/r_hip_yaw_link.STL similarity index 100% rename from pybullet_sim/meshes/r_hip_yaw_link.STL rename to pybullet_sim/assets/aurea/meshes/r_hip_yaw_link.STL diff --git a/pybullet_sim/meshes/r_knee_link.STL b/pybullet_sim/assets/aurea/meshes/r_knee_link.STL similarity index 100% rename from pybullet_sim/meshes/r_knee_link.STL rename to pybullet_sim/assets/aurea/meshes/r_knee_link.STL diff --git a/pybullet_sim/meshes/r_sho_pitch_link.STL b/pybullet_sim/assets/aurea/meshes/r_sho_pitch_link.STL similarity index 100% rename from pybullet_sim/meshes/r_sho_pitch_link.STL rename to pybullet_sim/assets/aurea/meshes/r_sho_pitch_link.STL diff --git a/pybullet_sim/meshes/r_sho_roll_link.STL b/pybullet_sim/assets/aurea/meshes/r_sho_roll_link.STL similarity index 100% rename from pybullet_sim/meshes/r_sho_roll_link.STL rename to pybullet_sim/assets/aurea/meshes/r_sho_roll_link.STL diff --git a/pybullet_sim/caminhada_relativa.py b/pybullet_sim/caminhada_relativa.py deleted file mode 100644 index 1fcdc50..0000000 --- a/pybullet_sim/caminhada_relativa.py +++ /dev/null @@ -1,203 +0,0 @@ -import pybullet as p -import pybullet_data -import time -import math -import numpy as np -import cvxpy as cp - -class RelativeAnalyticalIK: - def __init__(self, robot_id): - self.robot_id = robot_id - - # --- PARÂMETROS DO ROBÔ --- - self.L_COXA = 0.12 - self.L_TIBIA = 0.085 - self.D_QUADRIL_OFFSET = 0.0425 - self.D_TORNOZELO_SOLA = 0.048 - - self.JUNTAS_DIR = [8, 9, 10, 11, 12, 13] - self.JUNTAS_ESQ = [14, 15, 16, 17, 18, 19] - self.fixed_joints = {} - - def set_fixed_joint(self, joint_id, target_angle): - - self.fixed_joints[joint_id] = target_angle - - def solve_leg(self, dx, dy, dz, is_left): - """ - dx, dy, dz: Distância do pé em relação ao centro do torso (0,0,0) - """ - - sinal_y = 1.0 if is_left else -1.0 - offset_y = self.D_QUADRIL_OFFSET * sinal_y - - rx = -dx - ry = -(dy - offset_y) - rz = -(dz + self.D_TORNOZELO_SOLA) - C = math.sqrt(rx**2 + ry**2 + rz**2) - - - C = min(C, self.L_COXA + self.L_TIBIA - 0.001) - - cos_q5 = (C**2 - self.L_COXA**2 - self.L_TIBIA**2) / (2 * self.L_COXA * self.L_TIBIA) - cos_q5 = max(-1.0, min(1.0, cos_q5)) - q5_knee = math.acos(cos_q5) - - - q7_ankle_roll = math.atan2(ry, rz) - - alpha = math.asin(max(-1.0, min(1.0, (self.L_COXA / C) * math.sin(q5_knee)))) - q6_ankle_pitch = -math.atan2(rx, math.copysign(1.0, rz) * math.sqrt(ry**2 + rz**2)) - alpha - - q1_hip_yaw = 0.0 - q2_hip_roll = -q7_ankle_roll - q3_hip_pitch = -q6_ankle_pitch - q5_knee - - return [q1_hip_yaw, q2_hip_roll, q3_hip_pitch, q5_knee, q6_ankle_pitch, q7_ankle_roll] - - def apply(self, angulos_dir, angulos_esq): - for i, angle in enumerate(angulos_dir): - p.setJointMotorControl2(self.robot_id, self.JUNTAS_DIR[i], p.POSITION_CONTROL, - targetPosition=angle, force=5000, positionGain=1.0) - for i, angle in enumerate(angulos_esq): - p.setJointMotorControl2(self.robot_id, self.JUNTAS_ESQ[i], p.POSITION_CONTROL, - targetPosition=angle, force=5000, positionGain=1.0) - - for j in range(p.getNumJoints(self.robot_id)): - if j not in self.JUNTAS_DIR and j not in self.JUNTAS_ESQ: - target = self.fixed_joints.get(j, 0.0) # Busca o ângulo desejado (padrão é 0) - p.setJointMotorControl2(self.robot_id, j, p.POSITION_CONTROL, targetPosition=target, force=5000, positionGain=1.0) - - -class WalkingMath: - def __init__(self, h_target=0.2, t_step=0.6, y_sep=0.09): - self.H_TARGET = h_target - self.T_STEP = t_step - self.Y_SEP = y_sep - - def get_desired_state(self, t, cmd_vx): - """ - Gera as coordenadas matemáticas puras do CoM e dos pés ao longo do tempo. - """ - step_idx = int(t / self.T_STEP) - t_in_step = t % self.T_STEP - progresso = t_in_step / self.T_STEP - - is_left_support = (step_idx % 2 == 0) - - avanco_total = cmd_vx * t - - if is_left_support: - math_foot_l_x = avanco_total - math_foot_l_y = self.Y_SEP / 2.0 - math_foot_l_z = 0.0 - - passo_anterior = cmd_vx * (step_idx * self.T_STEP) - passo_futuro = cmd_vx * ((step_idx + 1) * self.T_STEP) - math_foot_r_x = passo_anterior + (passo_futuro - passo_anterior) * self.suavizar(progresso) - math_foot_r_y = -self.Y_SEP / 2.0 - math_foot_r_z = 0.05 * math.sin(math.pi * progresso) # Parábola - else: - math_foot_r_x = avanco_total - math_foot_r_y = -self.Y_SEP / 2.0 - math_foot_r_z = 0.0 - - passo_anterior = cmd_vx * (step_idx * self.T_STEP) - passo_futuro = cmd_vx * ((step_idx + 1) * self.T_STEP) - math_foot_l_x = passo_anterior + (passo_futuro - passo_anterior) * self.suavizar(progresso) - math_foot_l_y = self.Y_SEP / 2.0 - math_foot_l_z = 0.05 * math.sin(math.pi * progresso) - - math_com_x = avanco_total - ginga_amplitude = self.Y_SEP / 2.0 - 0.01 - math_com_y = ginga_amplitude * math.sin(math.pi * (t / self.T_STEP)) - - return (math_com_x, math_com_y), (math_foot_r_x, math_foot_r_y, math_foot_r_z), (math_foot_l_x, math_foot_l_y, math_foot_l_z) - - def suavizar(self, x): - return 0.5 * (1.0 - math.cos(math.pi * x)) - - -def main(): - p.connect(p.GUI) - p.setAdditionalSearchPath(pybullet_data.getDataPath()) - p.setGravity(0, 0, -9.81) - p.loadURDF("plane.urdf") - - robotId = p.loadURDF("urdf/aurea_urdf_pkg.urdf", [0, 0, 0.45], useFixedBase=True) - - ik = RelativeAnalyticalIK(robotId) - brain = WalkingMath(h_target=0.2) - - ID_R_SHO_PITCH = 5 - ID_L_SHO_PITCH = 2 - ID_R_SHO_ROLL = 6 - ID_L_SHO_ROLL = 3 - ID_R_ELBOW = 7 - ID_L_ELBOW = 4 - - ik.set_fixed_joint(ID_R_SHO_ROLL, 1.4) - ik.set_fixed_joint(ID_L_SHO_ROLL, -1.4) - - ik.set_fixed_joint(ID_R_SHO_PITCH, 0.4) - ik.set_fixed_joint(ID_L_SHO_PITCH, 0.4) - - - ik.set_fixed_joint(ID_R_ELBOW, 1.9) - ik.set_fixed_joint(ID_L_ELBOW, -1.9) - - print("Iniciando a First Pose...") - - ang_d_init = ik.solve_leg(0.0, -brain.Y_SEP/2, -brain.H_TARGET, is_left=False) - ang_e_init = ik.solve_leg(0.0, brain.Y_SEP/2, -brain.H_TARGET, is_left=True) - - for i, j in enumerate(ik.JUNTAS_DIR): - p.resetJointState(robotId, j, ang_d_init[i]) - for i, j in enumerate(ik.JUNTAS_ESQ): - p.resetJointState(robotId, j, ang_e_init[i]) - - for j_arm, ang_arm in ik.fixed_joints.items(): - p.resetJointState(robotId, j_arm, ang_arm) - - for _ in range(360): - ik.apply(ang_d_init, ang_e_init) - p.stepSimulation() - time.sleep(1/240.) - - print("First Pose estabilizada! Iniciando a Caminhada...") - # ======================================== - - for _ in range(100): - ang_d = ik.solve_leg(0.0, -brain.Y_SEP/2, -brain.H_TARGET, is_left=False) - ang_e = ik.solve_leg(0.0, brain.Y_SEP/2, -brain.H_TARGET, is_left=True) - ik.apply(ang_d, ang_e) - p.stepSimulation() - - print("Iniciando Caminhada Relativa!") - t = 0.0 - dt_sim = 1/240. - - cmd_vx = 0.07 - - while True: - com_math, foot_r_math, foot_l_math = brain.get_desired_state(t, cmd_vx) - - dx_r = foot_r_math[0] - com_math[0] - dy_r = foot_r_math[1] - com_math[1] - dz_r = foot_r_math[2] - brain.H_TARGET - - dx_l = foot_l_math[0] - com_math[0] - dy_l = foot_l_math[1] - com_math[1] - dz_l = foot_l_math[2] - brain.H_TARGET - - angulos_d = ik.solve_leg(dx_r, dy_r, dz_r, is_left=False) - angulos_e = ik.solve_leg(dx_l, dy_l, dz_l, is_left=True) - - ik.apply(angulos_d, angulos_e) - p.stepSimulation() - - t += dt_sim - time.sleep(dt_sim) - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/pybullet_sim/ik_solver.py b/pybullet_sim/ik_solver.py deleted file mode 100644 index e9b24f9..0000000 --- a/pybullet_sim/ik_solver.py +++ /dev/null @@ -1,120 +0,0 @@ -import math -import pybullet as p - -class RelativeAnalyticalIK: - def __init__(self, robot_id): - self.robot_id = robot_id - - # --- PARÂMETROS DO ROBÔ --- - self.L_COXA = 0.12 - self.L_TIBIA = 0.085 - self.D_QUADRIL_OFFSET = 0.0425 - self.D_TORNOZELO_SOLA = 0.045 - - # IDs das juntas - self.JUNTAS_DIR = [8, 9, 10, 11, 12, 13] - self.JUNTAS_ESQ = [14, 15, 16, 17, 18, 19] - - self.fixed_joints = {} - - def set_fixed_joint(self, joint_id, target_angle): - self.fixed_joints[joint_id] = target_angle - - def solve_leg(self, dx, dy, dz, is_left): - sinal_y = 1.0 if is_left else -1.0 - offset_y = self.D_QUADRIL_OFFSET * sinal_y - - rx = -dx - ry = -(dy - offset_y) - rz = -(dz + self.D_TORNOZELO_SOLA) - - C = math.sqrt(rx**2 + ry**2 + rz**2) - C = min(C, self.L_COXA + self.L_TIBIA - 0.001) - - cos_q5 = (C**2 - self.L_COXA**2 - self.L_TIBIA**2) / (2 * self.L_COXA * self.L_TIBIA) - cos_q5 = max(-1.0, min(1.0, cos_q5)) - q5_knee = math.acos(cos_q5) - - q7_ankle_roll = math.atan2(ry, rz) - alpha = math.asin(max(-1.0, min(1.0, (self.L_COXA / C) * math.sin(q5_knee)))) - q6_ankle_pitch = -math.atan2(rx, math.copysign(1.0, rz) * math.sqrt(ry**2 + rz**2)) - alpha - - q1_hip_yaw = 0.0 - q2_hip_roll = -q7_ankle_roll - q3_hip_pitch = -q6_ankle_pitch - q5_knee - - return [q1_hip_yaw, q2_hip_roll, q3_hip_pitch, q5_knee, q6_ankle_pitch, q7_ankle_roll] - - def apply(self, angulos_dir, angulos_esq): - for i, angle in enumerate(angulos_dir): - p.setJointMotorControl2(self.robot_id, self.JUNTAS_DIR[i], p.POSITION_CONTROL, targetPosition=angle, force=100, positionGain=1.0) - for i, angle in enumerate(angulos_esq): - p.setJointMotorControl2(self.robot_id, self.JUNTAS_ESQ[i], p.POSITION_CONTROL, targetPosition=angle, force=100, positionGain=1.0) - - for j in range(p.getNumJoints(self.robot_id)): - if j not in self.JUNTAS_DIR and j not in self.JUNTAS_ESQ: - target = self.fixed_joints.get(j, 0.0) - p.setJointMotorControl2(self.robot_id, j, p.POSITION_CONTROL, targetPosition=target, force=100, positionGain=1.0) - -class NumericalIK: - - def __init__(self, robot_id, foot_r_link_idx=12, foot_l_link_idx=18): - self.robot_id = robot_id - self.D_TORNOZELO_SOLA = 0.048 - - self.JUNTAS_DIR = [8, 9, 10, 11, 12, 13] - self.JUNTAS_ESQ = [14, 15, 16, 17, 18, 19] - - self.FOOT_R_LINK = foot_r_link_idx - self.FOOT_L_LINK = foot_l_link_idx - - self.fixed_joints = {} - - self.joint_to_dof = {} - dof_idx = 0 - for j in range(p.getNumJoints(self.robot_id)): - if p.getJointInfo(self.robot_id, j)[2] != p.JOINT_FIXED: - self.joint_to_dof[j] = dof_idx - dof_idx += 1 - - def set_fixed_joint(self, joint_id, target_angle): - self.fixed_joints[joint_id] = target_angle - - def solve_leg(self, dx, dy, dz, is_left, h_target=0.22): - - base_pos, _ = p.getBasePositionAndOrientation(self.robot_id) - - target_z = (base_pos[2] - h_target) + (dz + self.D_TORNOZELO_SOLA) - - target_pos = [base_pos[0] + dx, base_pos[1] + dy, target_z] - target_orn = p.getQuaternionFromEuler([0, 0, 0]) # Pé paralelo ao mundo - - link_alvo = self.FOOT_L_LINK if is_left else self.FOOT_R_LINK - - joint_poses = p.calculateInverseKinematics( - self.robot_id, - link_alvo, - targetPosition=target_pos, - targetOrientation=target_orn, - maxNumIterations=100, - residualThreshold=1e-4 - ) - - juntas_da_perna = self.JUNTAS_ESQ if is_left else self.JUNTAS_DIR - angulos = [] - for j in juntas_da_perna: - dof_index = self.joint_to_dof[j] - angulos.append(joint_poses[dof_index]) - - return angulos - - def apply(self, angulos_dir, angulos_esq, force=5000, gain=1.0): - for i, angle in enumerate(angulos_dir): - p.setJointMotorControl2(self.robot_id, self.JUNTAS_DIR[i], p.POSITION_CONTROL, targetPosition=angle, force=force, positionGain=gain) - for i, angle in enumerate(angulos_esq): - p.setJointMotorControl2(self.robot_id, self.JUNTAS_ESQ[i], p.POSITION_CONTROL, targetPosition=angle, force=force, positionGain=gain) - - for j in range(p.getNumJoints(self.robot_id)): - if j not in self.JUNTAS_DIR and j not in self.JUNTAS_ESQ: - target = self.fixed_joints.get(j, 0.0) - p.setJointMotorControl2(self.robot_id, j, p.POSITION_CONTROL, targetPosition=target, force=force, positionGain=gain) \ No newline at end of file diff --git a/pybullet_sim/kinematics.py b/pybullet_sim/kinematics.py new file mode 100644 index 0000000..1213055 --- /dev/null +++ b/pybullet_sim/kinematics.py @@ -0,0 +1,95 @@ +""" +Analytical leg IK for the Aurea robot, relative to the torso. + +The foot target is given as an offset (dx, dy, dz) from the torso centre, so +the walking code never needs the robot's pose in the world. +""" + +import math + +import pybullet as p + +# Joint order inside each leg, as returned by LegIK.solve_leg +LEG_JOINTS = ["hip_yaw", "hip_roll", "hip_pitch", "knee", "ank_pitch", "ank_roll"] + + +class LegIK: + # ── Robot dimensions [m] ────────────────────────────────────────────── + L_THIGH = 0.12 # hip_pitch → knee + L_SHIN = 0.085 # knee → ank_pitch + HIP_OFFSET_Y = 0.0425 # torso centre → hip, lateral + ANKLE_TO_SOLE = 0.045 # ankle joints → foot sole + + # ── Position controller settings ────────────────────────────────────── + MOTOR_FORCE = 100.0 + POSITION_GAIN = 1.0 + + def __init__(self, robot_id: int): + self.robot_id = robot_id + + name_to_id = { + p.getJointInfo(robot_id, j)[1].decode(): j + for j in range(p.getNumJoints(robot_id)) + } + self.right_leg = [name_to_id[f"r_{name}"] for name in LEG_JOINTS] + self.left_leg = [name_to_id[f"l_{name}"] for name in LEG_JOINTS] + self._name_to_id = name_to_id + + # Target angles for every joint that is not part of a leg (default 0) + self.fixed_joints: dict[int, float] = {} + + def set_fixed_joint(self, joint_name: str, target_angle: float) -> None: + self.fixed_joints[self._name_to_id[joint_name]] = target_angle + + def solve_leg(self, dx: float, dy: float, dz: float, is_left: bool) -> list[float]: + """ + Joint angles that place the foot sole at (dx, dy, dz) from the torso centre. + + The foot is kept parallel to the ground and hip_yaw is fixed at 0, so the + problem reduces to a planar 2-link chain (thigh + shin) plus a roll. + Targets out of reach are clamped to a fully stretched leg. + """ + offset_y = self.HIP_OFFSET_Y * (1.0 if is_left else -1.0) + + # Vector from the ankle to the hip + rx = -dx + ry = -(dy - offset_y) + rz = -(dz + self.ANKLE_TO_SOLE) + + C = math.sqrt(rx**2 + ry**2 + rz**2) + C = min(C, self.L_THIGH + self.L_SHIN - 0.001) + + # Law of cosines for the knee + cos_knee = (C**2 - self.L_THIGH**2 - self.L_SHIN**2) / (2 * self.L_THIGH * self.L_SHIN) + knee = math.acos(max(-1.0, min(1.0, cos_knee))) + + ankle_roll = math.atan2(ry, rz) + alpha = math.asin(max(-1.0, min(1.0, (self.L_THIGH / C) * math.sin(knee)))) + ankle_pitch = -math.atan2(rx, math.copysign(1.0, rz) * math.sqrt(ry**2 + rz**2)) - alpha + + # Foot parallel to the ground → hip compensates the ankle and knee + hip_yaw = 0.0 + hip_roll = -ankle_roll + hip_pitch = -ankle_pitch - knee + + return [hip_yaw, hip_roll, hip_pitch, knee, ankle_pitch, ankle_roll] + + def apply(self, right_angles: list[float], left_angles: list[float]) -> None: + """Send leg angles and hold every other joint at its fixed target.""" + targets = dict(zip(self.right_leg, right_angles)) + targets.update(zip(self.left_leg, left_angles)) + for j in range(p.getNumJoints(self.robot_id)): + target = targets.get(j, self.fixed_joints.get(j, 0.0)) + p.setJointMotorControl2(self.robot_id, j, p.POSITION_CONTROL, + targetPosition=target, + force=self.MOTOR_FORCE, + positionGain=self.POSITION_GAIN) + + def reset(self, right_angles: list[float], left_angles: list[float]) -> None: + """Teleport the joints to the given angles (used for the initial pose).""" + for j, angle in zip(self.right_leg, right_angles): + p.resetJointState(self.robot_id, j, angle) + for j, angle in zip(self.left_leg, left_angles): + p.resetJointState(self.robot_id, j, angle) + for j, angle in self.fixed_joints.items(): + p.resetJointState(self.robot_id, j, angle) diff --git a/pybullet_sim/main.py b/pybullet_sim/main.py index 34acea0..9775d6a 100644 --- a/pybullet_sim/main.py +++ b/pybullet_sim/main.py @@ -1,96 +1,126 @@ +""" +Aurea robot walking in PyBullet, with an MPC-planned CoM and analytical IK. + +Run (from any directory): + + python pybullet_sim/main.py # GUI, cmd_vx = 0.1 m/s + python pybullet_sim/main.py --vx 0.05 + python pybullet_sim/main.py --headless --duration 10 + +Note: the MPC runs in open loop. The CoM state fed back into the MPC is its +own prediction, not a measurement from the simulator, so the planner acts as a +trajectory generator and the PD joint controllers do the stabilisation. +""" + +import argparse +import os +import time + import pybullet as p import pybullet_data -import time -from ik_solver import RelativeAnalyticalIK, NumericalIK -from trajectory_generator import TrajectoryGenerator +from kinematics import LegIK +from mpc_planner import MPCPlanner + +URDF_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), + "assets", "aurea", "aurea.urdf") + +DT_SIM = 1 / 240. # physics timestep [s] +DT_MPC = 0.02 # MPC timestep [s] -def setup_first_pose(robotId, ik, brain): - """ Configura a pose inicial de agachamento e braços fixos """ - ID_R_SHO_PITCH, ID_L_SHO_PITCH = 5, 2 - ID_R_SHO_ROLL, ID_L_SHO_ROLL = 6, 3 - ID_R_ELBOW, ID_L_ELBOW = 7, 4 +# KNOWN ISSUE: DT_MPC is not a multiple of DT_SIM. int(0.02 / (1/240)) = 4, so +# each MPC tick advances the trajectory clock t by 0.020 s while physics only +# advances 4/240 ≈ 0.0167 s: the gait runs 1.2× faster than planned (steps of +# ~0.25 s instead of T_STEP = 0.3 s, ~0.12 m/s for cmd_vx = 0.1). The current +# gains were tuned with this mismatch; making the clocks consistent (e.g. +# DT_MPC = 5 * DT_SIM) makes the robot fall until the planner is re-tuned. +PHYSICS_STEPS_PER_MPC = int(DT_MPC / DT_SIM) - ik.set_fixed_joint(ID_R_SHO_ROLL, 1.5) - ik.set_fixed_joint(ID_L_SHO_ROLL, -1.5) - ik.set_fixed_joint(ID_R_SHO_PITCH, 0.6) - ik.set_fixed_joint(ID_L_SHO_PITCH, 0.6) - ik.set_fixed_joint(ID_R_ELBOW, 2.1) - ik.set_fixed_joint(ID_L_ELBOW, -2.1) +# Fixed arm pose [rad] held while walking +ARM_POSE = { + "r_sho_roll": 1.5, "l_sho_roll": -1.5, + "r_sho_pitch": 0.6, "l_sho_pitch": 0.6, + "r_el": 2.1, "l_el": -2.1, +} - print("Iniciando a First Pose...") - ang_d_init = ik.solve_leg(0.0, -brain.Y_SEP/2, -brain.H_TARGET, is_left=False) - ang_e_init = ik.solve_leg(0.0, brain.Y_SEP/2, -brain.H_TARGET, is_left=True) - for i, j in enumerate(ik.JUNTAS_DIR): p.resetJointState(robotId, j, ang_d_init[i]) - for i, j in enumerate(ik.JUNTAS_ESQ): p.resetJointState(robotId, j, ang_e_init[i]) - for j_arm, ang_arm in ik.fixed_joints.items(): p.resetJointState(robotId, j_arm, ang_arm) +def setup_first_pose(ik: LegIK, planner: MPCPlanner, realtime: bool) -> None: + """Crouch to the walking height with the arms fixed, then let it settle.""" + for joint, angle in ARM_POSE.items(): + ik.set_fixed_joint(joint, angle) - for _ in range(360): - ik.apply(ang_d_init, ang_e_init) + print("Moving to the initial pose...") + right = ik.solve_leg(0.0, -planner.Y_SEP / 2, -planner.H_TARGET, is_left=False) + left = ik.solve_leg(0.0, planner.Y_SEP / 2, -planner.H_TARGET, is_left=True) + ik.reset(right, left) + + for _ in range(360): # 1.5 s + ik.apply(right, left) p.stepSimulation() - time.sleep(1/240.) - print("First Pose estabilizada!") + if realtime: + time.sleep(DT_SIM) + print("Initial pose settled.") + -def main(): - p.connect(p.GUI) +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__.split("\n")[1]) + parser.add_argument("--vx", type=float, default=0.1, help="forward speed [m/s]") + parser.add_argument("--headless", action="store_true", help="run without GUI, as fast as possible") + parser.add_argument("--duration", type=float, default=None, help="stop after this many seconds") + args = parser.parse_args() + + p.connect(p.DIRECT if args.headless else p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -9.81) p.loadURDF("plane.urdf") - - robotId = p.loadURDF("urdf/aurea_urdf_pkg.urdf", [0, 0, 0.35], useFixedBase=False) - - dt_mpc = 0.02 - ik = RelativeAnalyticalIK(robotId) - brain = TrajectoryGenerator(h_target=0.2, dt_mpc=dt_mpc) + robot_id = p.loadURDF(URDF_PATH, [0, 0, 0.35], useFixedBase=False) - setup_first_pose(robotId, ik, brain) + ik = LegIK(robot_id) + planner = MPCPlanner(h_target=0.2, dt_mpc=DT_MPC) + realtime = not args.headless + setup_first_pose(ik, planner, realtime) + # MPC state (open loop, see module docstring) com_x, com_vx = 0.0, 0.0 com_y, com_vy = 0.0, 0.0 - - t_global = 0.0 - dt_sim = 1/240. - passos_fisica_por_mpc = int(dt_mpc / dt_sim) - - cmd_vx = 0.1 - - print("Iniciando Caminhada Controlada por MPC!") - while True: - - - com_ref_x, zmp_ref_x, zmp_min_x, zmp_max_x = brain.get_references_x(t_global, cmd_vx) - com_ref_y, zmp_ref_y, zmp_min_y, zmp_max_y = brain.get_references_y(t_global) - - next_com_x, next_com_vx = brain.solve_mpc(com_x, com_vx, com_ref_x, zmp_ref_x, zmp_min_x, zmp_max_x) - next_com_y, next_com_vy = brain.solve_mpc(com_y, com_vy, com_ref_y, zmp_ref_y, zmp_min_y, zmp_max_y) - - - for step_fisica in range(passos_fisica_por_mpc): - t_instante = t_global + (step_fisica * dt_sim) - progresso_mpc = (step_fisica + 1) / passos_fisica_por_mpc - - com_math_x = com_x + (next_com_x - com_x) * progresso_mpc - com_math_y = com_y + (next_com_y - com_y) * progresso_mpc - foot_r_math, foot_l_math = brain.get_foot_trajectories(t_instante, cmd_vx) - - dx_r, dy_r, dz_r = foot_r_math[0] - com_math_x, foot_r_math[1] - com_math_y, foot_r_math[2] - brain.H_TARGET - dx_l, dy_l, dz_l = foot_l_math[0] - com_math_x, foot_l_math[1] - com_math_y, foot_l_math[2] - brain.H_TARGET - - _, orn_b = p.getBasePositionAndOrientation(robotId) - roll, pitch, _ = p.getEulerFromQuaternion(orn_b) - - angulos_d = ik.solve_leg(dx_r, dy_r, dz_r, is_left=False) - angulos_e = ik.solve_leg(dx_l, dy_l, dz_l, is_left=True) - - ik.apply(angulos_d, angulos_e) + + t = 0.0 + + print(f"Walking with MPC at cmd_vx = {args.vx} m/s") + while args.duration is None or t < args.duration: + com_ref_x, zmp_ref_x, zmp_min_x, zmp_max_x = planner.get_references_x(t, args.vx) + com_ref_y, zmp_ref_y, zmp_min_y, zmp_max_y = planner.get_references_y(t) + + next_com_x, next_com_vx = planner.solve_mpc(com_x, com_vx, com_ref_x, zmp_ref_x, zmp_min_x, zmp_max_x) + next_com_y, next_com_vy = planner.solve_mpc(com_y, com_vy, com_ref_y, zmp_ref_y, zmp_min_y, zmp_max_y) + + for i in range(PHYSICS_STEPS_PER_MPC): + # Linear interpolation of the CoM between two MPC samples + alpha = (i + 1) / PHYSICS_STEPS_PER_MPC + com_now_x = com_x + (next_com_x - com_x) * alpha + com_now_y = com_y + (next_com_y - com_y) * alpha + foot_r, foot_l = planner.get_foot_trajectories(t + i * DT_SIM, args.vx) + + # Feet relative to the torso → IK + right = ik.solve_leg(foot_r[0] - com_now_x, foot_r[1] - com_now_y, + foot_r[2] - planner.H_TARGET, is_left=False) + left = ik.solve_leg(foot_l[0] - com_now_x, foot_l[1] - com_now_y, + foot_l[2] - planner.H_TARGET, is_left=True) + + ik.apply(right, left) p.stepSimulation() - time.sleep(dt_sim) - + if realtime: + time.sleep(DT_SIM) + com_x, com_vx = next_com_x, next_com_vx com_y, com_vy = next_com_y, next_com_vy - t_global += dt_mpc + t += DT_MPC + + pos, _ = p.getBasePositionAndOrientation(robot_id) + print(f"Finished at t={t:.1f}s, base = ({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f})") + p.disconnect() + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/pybullet_sim/medir_urdf.py b/pybullet_sim/medir_urdf.py deleted file mode 100644 index 516583f..0000000 --- a/pybullet_sim/medir_urdf.py +++ /dev/null @@ -1,31 +0,0 @@ -import pybullet as p -import pybullet_data -import math - -def main(): - p.connect(p.DIRECT) - p.setAdditionalSearchPath(pybullet_data.getDataPath()) - - robotId = p.loadURDF("urdf/aurea_urdf_pkg.urdf", useFixedBase=True) - - print("\n" + "="*70) - print("URDF (Medidas de Links e Juntas)") - print("="*70) - - for j in range(p.getNumJoints(robotId)): - info = p.getJointInfo(robotId, j) - - id_junta = info[0] - nome_junta = info[1].decode('utf-8') - - pos_relativa = info[14] - - comprimento = math.sqrt(pos_relativa[0]**2 + pos_relativa[1]**2 + pos_relativa[2]**2) - - print(f"ID: {id_junta:02d} | Junta: {nome_junta:<25} | Vetor(X,Y,Z): {pos_relativa[0]:.4f}, {pos_relativa[1]:.4f}, {pos_relativa[2]:.4f} | Comprimento: {comprimento:.4f} m") - - print("="*70 + "\n") - p.disconnect() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/pybullet_sim/mpc_planner.py b/pybullet_sim/mpc_planner.py new file mode 100644 index 0000000..959f4ac --- /dev/null +++ b/pybullet_sim/mpc_planner.py @@ -0,0 +1,178 @@ +""" +CoM trajectory planner using Model Predictive Control (MPC) on the LIPM. + +For each horizontal axis (x and y separately) the planner solves the QP + + minimise Σ_k W_COM·(x_k - x_ref_k)² + W_ZMP·(zmp_k - zmp_ref_k)² + W_VEL·v_k² + subject to x_{k+1} = x_k + v_k·dt + v_{k+1} = v_k + ω²·(x_k - zmp_k)·dt (LIPM, ω = √(g/h)) + zmp_min_k ≤ zmp_k ≤ zmp_max_k (ZMP inside the support polygon) + +over a horizon of N steps, and returns the first predicted state. + +It also generates the reference footstep plan: a fixed step period, alternating +support feet, stride length = cmd_vx · T_STEP. +""" + +import math + +import cvxpy as cp +import numpy as np + + +class MPCPlanner: + # Cost weights + W_COM = 800.0 # track the CoM reference + W_ZMP = 2.0 # keep the ZMP close to the centre of the support foot + W_VEL = 10.0 # penalise CoM velocity (smoothness) + + def __init__(self, h_target=0.22, t_step=0.3, ds_ratio=0.1, y_sep=0.085, + dt_mpc=0.02, n_horizon=25): + self.H_TARGET = h_target # CoM height [m] + self.T_STEP = t_step # step period [s] + self.T_DS = t_step * ds_ratio # double-support duration [s] + self.T_SS = t_step * (1.0 - ds_ratio) # single-support duration [s] + self.Y_SEP = y_sep # distance between the feet [m] + self.SWING_HEIGHT = 0.05 # max swing-foot height [m] + + self.dt_mpc = dt_mpc + self.N = n_horizon + self.g = 9.81 + self.omega = math.sqrt(self.g / self.H_TARGET) + + # Half-size of the ZMP support region around the foot centre [m] + self.FOOT_MARGIN_X = 0.07 + self.FOOT_MARGIN_Y = 0.03 + + self._setup_mpc_problem() + + def _setup_mpc_problem(self): + # Parameters change every solve; the problem structure is built once + self.p_x0 = cp.Parameter() + self.p_vx0 = cp.Parameter() + self.p_com_ref = cp.Parameter(self.N) + self.p_zmp_ref = cp.Parameter(self.N) + self.p_zmp_min = cp.Parameter(self.N) + self.p_zmp_max = cp.Parameter(self.N) + + self.v_x = cp.Variable(self.N + 1) + self.v_vx = cp.Variable(self.N + 1) + self.v_zmp = cp.Variable(self.N) + + cost = 0 + constraints = [self.v_x[0] == self.p_x0, self.v_vx[0] == self.p_vx0] + + for k in range(self.N): + ax = (self.omega**2) * (self.v_x[k] - self.v_zmp[k]) + constraints += [self.v_x[k + 1] == self.v_x[k] + self.v_vx[k] * self.dt_mpc] + constraints += [self.v_vx[k + 1] == self.v_vx[k] + ax * self.dt_mpc] + + constraints += [self.v_zmp[k] >= self.p_zmp_min[k]] + constraints += [self.v_zmp[k] <= self.p_zmp_max[k]] + + cost += self.W_COM * cp.sum_squares(self.v_x[k] - self.p_com_ref[k]) + cost += self.W_ZMP * cp.sum_squares(self.v_zmp[k] - self.p_zmp_ref[k]) + cost += self.W_VEL * cp.sum_squares(self.v_vx[k]) + + self.prob = cp.Problem(cp.Minimize(cost), constraints) + + def solve_mpc(self, x0, vx0, com_ref, zmp_ref, zmp_min, zmp_max): + """Return the CoM (position, velocity) one MPC step ahead.""" + self.p_x0.value = x0 + self.p_vx0.value = vx0 + self.p_com_ref.value = com_ref + self.p_zmp_ref.value = zmp_ref + self.p_zmp_min.value = zmp_min + self.p_zmp_max.value = zmp_max + + try: + self.prob.solve(solver=cp.OSQP, warm_start=True) + except cp.error.SolverError as exc: + print(f"[MPC] solver error: {exc}") + + if self.v_x.value is not None and self.prob.status in (cp.OPTIMAL, cp.OPTIMAL_INACCURATE): + return self.v_x.value[1], self.v_vx.value[1] + + # No solution: keep moving at constant velocity for this step + print(f"[MPC] no solution (status={self.prob.status}); extrapolating") + return x0 + vx0 * self.dt_mpc, vx0 + + def get_foot_trajectories(self, t, cmd_vx): + """World (x, y, z) of the right and left foot soles at time t.""" + step_idx = int(t / self.T_STEP) + t_in_step = t % self.T_STEP + is_left_support = (step_idx % 2 == 0) + + current_x = cmd_vx * (step_idx * self.T_STEP) + next_x = cmd_vx * ((step_idx + 1) * self.T_STEP) + previous_x = cmd_vx * ((step_idx - 1) * self.T_STEP) if step_idx > 0 else 0.0 + + swing_progress = 0.0 if t_in_step < self.T_DS else (t_in_step - self.T_DS) / self.T_SS + swing_z = self.SWING_HEIGHT * math.sin(math.pi * swing_progress) if swing_progress > 0 else 0.0 + swing_x = previous_x + (next_x - previous_x) * self._s_curve(swing_progress) + + if is_left_support: + r_foot = (swing_x, -self.Y_SEP / 2.0, swing_z) + l_foot = (current_x, self.Y_SEP / 2.0, 0.0) + else: + r_foot = (current_x, -self.Y_SEP / 2.0, 0.0) + l_foot = (swing_x, self.Y_SEP / 2.0, swing_z) + + return r_foot, l_foot + + def get_references_y(self, t_current): + """Lateral CoM/ZMP references and ZMP bounds over the horizon.""" + com_ref, zmp_ref = np.zeros(self.N), np.zeros(self.N) + zmp_min, zmp_max = np.zeros(self.N), np.zeros(self.N) + for k in range(self.N): + t_future = t_current + k * self.dt_mpc + step_idx = int(t_future / self.T_STEP) + t_in_step = t_future % self.T_STEP + is_left_support = (step_idx % 2 == 0) + + support_y = self.Y_SEP / 2.0 if is_left_support else -self.Y_SEP / 2.0 + + if step_idx == 0: + previous_y = 0.0 + else: + previous_y = -self.Y_SEP / 2.0 if is_left_support else self.Y_SEP / 2.0 + + if t_in_step < self.T_DS: + # Double support: shift the CoM towards the new support foot; + # the ZMP may lie anywhere between both feet + com_ref[k] = previous_y + (support_y - previous_y) * self._s_curve(t_in_step / self.T_DS) + zmp_ref[k] = support_y + zmp_min[k] = -self.Y_SEP / 2.0 - self.FOOT_MARGIN_Y + zmp_max[k] = self.Y_SEP / 2.0 + self.FOOT_MARGIN_Y + else: + # Single support: the ZMP must stay under the support foot + com_ref[k], zmp_ref[k] = support_y, support_y + zmp_min[k] = support_y - self.FOOT_MARGIN_Y + zmp_max[k] = support_y + self.FOOT_MARGIN_Y + return com_ref, zmp_ref, zmp_min, zmp_max + + def get_references_x(self, t_current, cmd_vx): + """Forward CoM/ZMP references and ZMP bounds over the horizon.""" + com_ref, zmp_ref = np.zeros(self.N), np.zeros(self.N) + zmp_min, zmp_max = np.zeros(self.N), np.zeros(self.N) + for k in range(self.N): + t_future = t_current + k * self.dt_mpc + step_idx = int(t_future / self.T_STEP) + t_in_step = t_future % self.T_STEP + + current_x = cmd_vx * (step_idx * self.T_STEP) + previous_x = cmd_vx * ((step_idx - 1) * self.T_STEP) if step_idx > 0 else 0.0 + + com_ref[k] = cmd_vx * t_future + zmp_ref[k] = current_x + + if t_in_step < self.T_DS: + zmp_min[k], zmp_max[k] = previous_x - self.FOOT_MARGIN_X, current_x + self.FOOT_MARGIN_X + else: + zmp_min[k], zmp_max[k] = current_x - self.FOOT_MARGIN_X, current_x + self.FOOT_MARGIN_X + return com_ref, zmp_ref, zmp_min, zmp_max + + @staticmethod + def _s_curve(x): + """Smooth 0 → 1 cosine ramp for x in [0, 1].""" + return 0.5 * (1.0 - math.cos(math.pi * max(0.0, min(1.0, x)))) diff --git a/pybullet_sim/scene_test.xml b/pybullet_sim/scene_test.xml deleted file mode 100644 index 65a02d2..0000000 --- a/pybullet_sim/scene_test.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/pybullet_sim/trajectory_generator.py b/pybullet_sim/trajectory_generator.py deleted file mode 100644 index 70328f9..0000000 --- a/pybullet_sim/trajectory_generator.py +++ /dev/null @@ -1,137 +0,0 @@ -import math -import numpy as np -import cvxpy as cp - -class TrajectoryGenerator: - def __init__(self, h_target=0.22, t_step=0.3, ds_ratio=0.1, y_sep=0.085, dt_mpc=0.02, n_horizon=25): - self.H_TARGET = h_target - self.T_STEP = t_step - self.T_DS = t_step * ds_ratio - self.T_SS = t_step * (1.0 - ds_ratio) - self.Y_SEP = y_sep - - self.dt_mpc = dt_mpc - self.N = n_horizon - self.g = 9.81 - self.omega = math.sqrt(self.g / self.H_TARGET) - - self.FOOT_MARGIN_X = 0.07 - self.FOOT_MARGIN_Y = 0.03 - - self._setup_mpc_problem() - - def _setup_mpc_problem(self): - self.p_x0 = cp.Parameter() - self.p_vx0 = cp.Parameter() - self.p_com_ref = cp.Parameter(self.N) - self.p_zmp_ref = cp.Parameter(self.N) - self.p_zmp_min = cp.Parameter(self.N) - self.p_zmp_max = cp.Parameter(self.N) - - self.v_x = cp.Variable(self.N + 1) - self.v_vx = cp.Variable(self.N + 1) - self.v_zmp = cp.Variable(self.N) - - cost = 0 - constraints = [self.v_x[0] == self.p_x0, self.v_vx[0] == self.p_vx0] - - for k in range(self.N): - ax = (self.omega**2) * (self.v_x[k] - self.v_zmp[k]) - constraints += [self.v_x[k+1] == self.v_x[k] + self.v_vx[k] * self.dt_mpc] - constraints += [self.v_vx[k+1] == self.v_vx[k] + ax * self.dt_mpc] - - constraints += [self.v_zmp[k] >= self.p_zmp_min[k]] - constraints += [self.v_zmp[k] <= self.p_zmp_max[k]] - - cost += 800.0 * cp.sum_squares(self.v_x[k] - self.p_com_ref[k]) - cost += 2.0 * cp.sum_squares(self.v_zmp[k] - self.p_zmp_ref[k]) - cost += 10.0 * cp.sum_squares(self.v_vx[k]) - - self.prob = cp.Problem(cp.Minimize(cost), constraints) - - def solve_mpc(self, x0, vx0, com_ref, zmp_ref, zmp_min, zmp_max): - self.p_x0.value = x0 - self.p_vx0.value = vx0 - self.p_com_ref.value = com_ref - self.p_zmp_ref.value = zmp_ref - self.p_zmp_min.value = zmp_min - self.p_zmp_max.value = zmp_max - - try: - self.prob.solve(solver=cp.OSQP, warm_start=True) - if self.v_x.value is not None: - return self.v_x.value[1], self.v_vx.value[1] - except: pass - return x0 + vx0 * self.dt_mpc, vx0 - - def get_foot_trajectories(self, t, cmd_vx): - step_idx = int(t / self.T_STEP) - t_in_step = t % self.T_STEP - is_left_support = (step_idx % 2 == 0) - - avanco_atual = cmd_vx * (step_idx * self.T_STEP) - avanco_futuro = cmd_vx * ((step_idx + 1) * self.T_STEP) - passo_anterior = cmd_vx * ((step_idx - 1) * self.T_STEP) if step_idx > 0 else 0.0 - - progresso_swing = 0.0 if t_in_step < self.T_DS else (t_in_step - self.T_DS) / self.T_SS - - z_math = 0.05 * math.sin(math.pi * progresso_swing) if progresso_swing > 0 else 0.0 - - if is_left_support: - r_foot = (passo_anterior + (avanco_futuro - passo_anterior) * self._s_curve(progresso_swing), - -self.Y_SEP / 2.0, - z_math) - l_foot = (avanco_atual, self.Y_SEP / 2.0, 0.0) - else: - r_foot = (avanco_atual, -self.Y_SEP / 2.0, 0.0) - l_foot = (passo_anterior + (avanco_futuro - passo_anterior) * self._s_curve(progresso_swing), - self.Y_SEP / 2.0, - z_math) - - return r_foot, l_foot - - def get_references_y(self, t_current): - com_ref, zmp_ref, zmp_min, zmp_max = np.zeros(self.N), np.zeros(self.N), np.zeros(self.N), np.zeros(self.N) - for k in range(self.N): - t_futuro = t_current + k * self.dt_mpc - step_idx = int(t_futuro / self.T_STEP) - t_in_step = t_futuro % self.T_STEP - is_left_support = (step_idx % 2 == 0) - - alvo_y = self.Y_SEP / 2.0 if is_left_support else -self.Y_SEP / 2.0 - - if step_idx == 0: - pe_anterior_y = 0.0 - else: - pe_anterior_y = -self.Y_SEP / 2.0 if is_left_support else self.Y_SEP / 2.0 - - if t_in_step < self.T_DS: - com_ref[k] = pe_anterior_y + (alvo_y - pe_anterior_y) * self._s_curve(t_in_step / self.T_DS) - zmp_ref[k] = alvo_y - zmp_min[k], zmp_max[k] = -self.Y_SEP/2.0 - self.FOOT_MARGIN_Y, self.Y_SEP/2.0 + self.FOOT_MARGIN_Y - else: - com_ref[k], zmp_ref[k] = alvo_y, alvo_y - zmp_min[k], zmp_max[k] = alvo_y - self.FOOT_MARGIN_Y, alvo_y + self.FOOT_MARGIN_Y - return com_ref, zmp_ref, zmp_min, zmp_max - - def get_references_x(self, t_current, cmd_vx): - com_ref, zmp_ref, zmp_min, zmp_max = np.zeros(self.N), np.zeros(self.N), np.zeros(self.N), np.zeros(self.N) - for k in range(self.N): - t_futuro = t_current + k * self.dt_mpc - step_idx = int(t_futuro / self.T_STEP) - t_in_step = t_futuro % self.T_STEP - - avanco_atual = cmd_vx * (step_idx * self.T_STEP) - passo_anterior = cmd_vx * ((step_idx - 1) * self.T_STEP) if step_idx > 0 else 0.0 - - com_ref[k] = cmd_vx * t_futuro - zmp_ref[k] = avanco_atual - - if t_in_step < self.T_DS: - zmp_min[k], zmp_max[k] = passo_anterior - self.FOOT_MARGIN_X, avanco_atual + self.FOOT_MARGIN_X - else: - zmp_min[k], zmp_max[k] = avanco_atual - self.FOOT_MARGIN_X, avanco_atual + self.FOOT_MARGIN_X - return com_ref, zmp_ref, zmp_min, zmp_max - - def _s_curve(self, x): - return 0.5 * (1.0 - math.cos(math.pi * max(0.0, min(1.0, x)))) \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..47a504b --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,27 @@ +[build-system] +requires = ["setuptools>=64"] +build-backend = "setuptools.build_meta" + +[project] +name = "humanoid-motion-predictive-control" +version = "0.1.0" +description = "Bipedal walking simulations (LIPM, MPC, analytical IK) in PyBullet and MuJoCo" +readme = "README.md" +license = { file = "LICENSE" } +authors = [{ name = "Pedro H. Peres" }] +requires-python = ">=3.10" +dependencies = ["numpy"] + +[project.optional-dependencies] +mujoco = ["mujoco>=3.2", "robot_descriptions>=1.12"] +pybullet = ["pybullet", "cvxpy", "osqp"] +tools = ["matplotlib"] +dev = ["pytest"] + +[tool.setuptools] +packages = ["mujoco_sim", "mujoco_sim.analytical_walking", "mujoco_sim.control", "mujoco_sim.kinematics"] + +[tool.pytest.ini_options] +testpaths = ["tests"] +pythonpath = [".", "pybullet_sim"] +markers = ["slow: runs a full physics simulation (deselect with -m 'not slow')"] diff --git a/tests/test_mujoco_sim.py b/tests/test_mujoco_sim.py new file mode 100644 index 0000000..41ac15f --- /dev/null +++ b/tests/test_mujoco_sim.py @@ -0,0 +1,67 @@ +"""IK against MuJoCo forward kinematics, and a full walking smoke test.""" + +import numpy as np +import pytest + +mujoco = pytest.importorskip("mujoco") +pytest.importorskip("robot_descriptions") + +from mujoco_sim.analytical_walking.walking_engine import WalkTarget # noqa: E402 +from mujoco_sim.kinematics.ik_solver import D_ANKLE_SOLE # noqa: E402 +from mujoco_sim.sim import Simulation # noqa: E402 + + +@pytest.fixture(scope="module") +def kinematic_sim(): + return Simulation(physics=False) + + +@pytest.mark.parametrize( + "body_pos, body_yaw, sole_pos, sole_yaw, leg", + [ + ((0.0, 0.0, 0.22), 0.0, (0.0, 0.054, 0.0), 0.0, "left"), + ((0.0, 0.0, 0.22), 0.0, (0.0, -0.054, 0.0), 0.0, "right"), + ((0.01, 0.0, 0.21), 0.2, (0.03, 0.06, 0.02), 0.3, "left"), + ((0.0, 0.01, 0.20), 0.0, (-0.03, -0.05, 0.01), -0.2, "right"), + ], +) +def test_ik_matches_forward_kinematics(kinematic_sim, body_pos, body_yaw, sole_pos, sole_yaw, leg): + """Set the joints to the IK solution and check where MuJoCo puts the foot.""" + sim = kinematic_sim + body_pos, sole_pos = np.array(body_pos), np.array(sole_pos) + other_sole = np.array([body_pos[0], -sole_pos[1], 0.0]) + if leg == "left": + target = WalkTarget(body_pos, body_yaw, sole_pos, sole_yaw, other_sole, 0.0) + else: + target = WalkTarget(body_pos, body_yaw, other_sole, 0.0, sole_pos, sole_yaw) + + sim._set_free_joint(body_pos, body_yaw) + leg_angles = sim._solve_legs(target) + assert len(leg_angles) == 2 + for angles in leg_angles: + sim._set_joints(angles) + mujoco.mj_forward(sim.model, sim.data) + + # IK places the ankle point (ank_pitch joint) D_ANKLE_SOLE above the sole target. + # 5 mm tolerance: the IK ignores the hip/ankle roll-axis offsets (see ik_solver.py) + side = leg[0] + ankle = sim.data.xanchor[sim.model.joint(f"{side}_ank_pitch").id] + np.testing.assert_allclose(ankle, sole_pos + [0, 0, D_ANKLE_SOLE], atol=5e-3) + + R_foot = sim.data.xmat[sim.model.body(f"{side}_ank_roll_link").id].reshape(3, 3) + assert np.arctan2(R_foot[1, 0], R_foot[0, 0]) == pytest.approx(sole_yaw, abs=1e-3) + assert R_foot[2, 2] == pytest.approx(1.0, abs=1e-4) # sole parallel to the floor + + +@pytest.mark.slow +@pytest.mark.parametrize("command", [{"vx": 0.05}, {"vy": 0.03}, {"wz": 0.5}]) +def test_robot_walks_without_falling(command): + sim = Simulation(physics=True) + sim.engine.set_command(**command) + start = sim.body_pos + while sim.time < 5.0: + sim.step() + assert sim.body_pos[2] > 0.18, f"robot fell at t={sim.time:.2f}s" + assert sum(sim.ik_failures.values()) == 0 + moved = np.linalg.norm(sim.body_pos[:2] - start[:2]) + abs(sim.body_yaw) + assert moved > 0.1 diff --git a/tests/test_pybullet_sim.py b/tests/test_pybullet_sim.py new file mode 100644 index 0000000..6463222 --- /dev/null +++ b/tests/test_pybullet_sim.py @@ -0,0 +1,59 @@ +"""MPC planner and leg IK of the PyBullet simulation.""" + +import numpy as np +import pytest + +pytest.importorskip("cvxpy") + +from mpc_planner import MPCPlanner # noqa: E402 + + +@pytest.fixture(scope="module") +def planner(): + return MPCPlanner(h_target=0.2, dt_mpc=0.02) + + +def test_mpc_respects_zmp_bounds(planner): + refs = planner.get_references_y(0.5) + x, _ = planner.solve_mpc(0.0, 0.0, *refs) + zmp, zmp_min, zmp_max = planner.v_zmp.value, refs[2], refs[3] + assert np.all(zmp >= zmp_min - 1e-4) and np.all(zmp <= zmp_max + 1e-4) + # The returned state is the first step of the optimal prediction + assert x == pytest.approx(planner.v_x.value[1]) + + +def test_mpc_follows_forward_reference(planner): + x, v, t = 0.0, 0.0, 0.0 + for _ in range(100): # 2 s + x, v = planner.solve_mpc(x, v, *planner.get_references_x(t, 0.1)) + t += planner.dt_mpc + assert x == pytest.approx(0.1 * t, abs=0.03) + + +def test_feet_alternate_support(planner): + r0, l0 = planner.get_foot_trajectories(planner.T_STEP * 0.5, 0.1) + r1, l1 = planner.get_foot_trajectories(planner.T_STEP * 1.5, 0.1) + assert l0[2] == 0.0 and r0[2] > 0.0 # step 0: left support, right swings + assert r1[2] == 0.0 and l1[2] > 0.0 # step 1: right support, left swings + + +def test_leg_ik_moves_ankle_vertically(): + """Raising the IK target by 2 cm raises the PyBullet ankle by 2 cm, without drift.""" + p = pytest.importorskip("pybullet") + from kinematics import LegIK + from main import URDF_PATH + + client = p.connect(p.DIRECT) + try: + robot = p.loadURDF(URDF_PATH, [0, 0, 1], useFixedBase=True) + ik = LegIK(robot) + + def ankle_pos(dz): + ik.reset(ik.solve_leg(0.0, -0.0425, dz, False), ik.solve_leg(0.0, 0.0425, dz, True)) + link_state = p.getLinkState(robot, ik.right_leg[4], computeForwardKinematics=True) + return np.array(link_state[4]) # world position of the ank_pitch link frame + + delta = ankle_pos(-0.18) - ankle_pos(-0.20) + np.testing.assert_allclose(delta, [0.0, 0.0, 0.02], atol=2e-3) + finally: + p.disconnect(client) diff --git a/tests/test_torso_controller.py b/tests/test_torso_controller.py new file mode 100644 index 0000000..08b23c8 --- /dev/null +++ b/tests/test_torso_controller.py @@ -0,0 +1,27 @@ +"""Torso orientation controller (pure math, no simulator needed).""" + +import pytest + +from mujoco_sim.control.torso_controller import TorsoOrientationController + + +def _imu(pitch=0.0, roll=0.0, pitch_rate=0.0, roll_rate=0.0): + return {"pitch": pitch, "roll": roll, "pitch_rate": pitch_rate, "roll_rate": roll_rate} + + +def test_no_tilt_gives_no_correction(): + ctrl = TorsoOrientationController() + assert all(v == 0.0 for v in ctrl.compute(_imu()).values()) + + +def test_pitch_correction_is_mirrored_between_legs(): + ctrl = TorsoOrientationController(kp_pitch=1.0, kd_pitch=0.0) + c = ctrl.compute(_imu(pitch=0.1)) + assert c["l_hip_pitch"] == pytest.approx(0.1) + assert c["r_hip_pitch"] == pytest.approx(-0.1) # right hip_pitch axis is flipped + + +def test_corrections_are_clamped(): + ctrl = TorsoOrientationController(max_delta=0.2) + c = ctrl.compute(_imu(pitch=1.0, roll=-1.0, pitch_rate=5.0, roll_rate=-5.0)) + assert all(abs(v) <= 0.2 + 1e-12 for v in c.values()) diff --git a/tests/test_trajectory_generator.py b/tests/test_trajectory_generator.py new file mode 100644 index 0000000..78fe97c --- /dev/null +++ b/tests/test_trajectory_generator.py @@ -0,0 +1,101 @@ +"""Properties of the analytical walking trajectories (no simulator needed).""" + +import numpy as np +import pytest + +from mujoco_sim.analytical_walking.trajectory_generator import ( + FootPose, + Pose2D, + get_com_pose_at_time, + get_swing_foot_pose_at_time, + select_next_poses, +) +from mujoco_sim.analytical_walking.walking_engine import WalkingEngine, WalkState +from mujoco_sim.config import WALK_PARAMS + +T, DS, Z_COM, Z_STEP, G = 0.4, 0.12, 0.22, 0.04, 9.81 + + +def _sample_swing(n=4001): + start = Pose2D(np.array([0.0, 0.054]), 0.0) + end = Pose2D(np.array([0.04, 0.054]), 0.2) + ts = np.linspace(0.0, T, n) + return ts, np.array([get_swing_foot_pose_at_time(t, T, Z_STEP, DS, start, end)[0] for t in ts]) + + +def test_swing_foot_never_goes_below_the_floor(): + _, pos = _sample_swing() + assert pos[:, 2].min() >= 0.0 + + +def test_swing_foot_height_is_continuous(): + ts, pos = _sample_swing() + max_jump = np.abs(np.diff(pos[:, 2])).max() + # Peak vertical speed of the cosine bump is pi * Z_STEP / T_ss; allow 2x margin + t_ss = T * (1 - DS) + allowed = 2 * np.pi * Z_STEP / t_ss * (ts[1] - ts[0]) + assert max_jump < allowed + + +def test_swing_foot_starts_and_ends_on_target(): + _, pos = _sample_swing() + np.testing.assert_allclose(pos[0], [0.0, 0.054, 0.0], atol=1e-9) + np.testing.assert_allclose(pos[-1], [0.04, 0.054, 0.0], atol=1e-9) + assert pos[:, 2].max() == pytest.approx(Z_STEP, rel=0.02) + + +def test_com_satisfies_boundary_conditions(): + start = Pose2D(np.array([0.0, 0.0])) + support = Pose2D(np.array([0.0, -0.054])) + end = Pose2D(np.array([0.02, 0.0])) + com0, _ = get_com_pose_at_time(0.0, T, DS, Z_COM, G, start, end, support) + comT, _ = get_com_pose_at_time(T, T, DS, Z_COM, G, start, end, support) + np.testing.assert_allclose(com0, start.position, atol=1e-9) + np.testing.assert_allclose(comT, end.position, atol=1e-9) + + +def test_com_moves_towards_support_foot(): + start = Pose2D(np.array([0.0, 0.0])) + support = Pose2D(np.array([0.0, -0.054])) + end = Pose2D(np.array([0.0, 0.0])) + com_mid, _ = get_com_pose_at_time(T / 2, T, DS, Z_COM, G, start, end, support) + assert com_mid[1] < -0.01 + + +def test_select_next_poses_rotates_command_into_torso_frame(): + torso = Pose2D(np.array([0.0, 0.0]), np.pi / 2) # facing +y + swing = FootPose(np.array([0.0, 0.0]), np.pi / 2, is_left=True) + next_torso, next_swing = select_next_poses(torso, swing, {"vx": 0.1}, T, 0.054) + np.testing.assert_allclose(next_torso.position, [0.0, 0.1 * T], atol=1e-12) + # Left foot lands to the left of the torso, i.e. towards -x when facing +y + assert next_swing.position[0] < next_torso.position[0] + assert next_swing.is_left + + +def test_engine_walks_and_stops(): + engine = WalkingEngine(**WALK_PARAMS) + dt = 0.008 + engine.set_command(vx=0.05) + for _ in range(int(2.0 / dt)): + engine.step(dt) + assert engine.state == WalkState.WALKING + assert engine.torso.position[0] > 0.05 + + engine.request_stop() + for _ in range(int(WALK_PARAMS["T"] / dt) + 2): + target = engine.step(dt) + assert engine.state == WalkState.IDLE + assert target.left_sole[2] == 0.0 and target.right_sole[2] == 0.0 + + +def test_march_in_place_starts_from_idle_and_stays_put(): + engine = WalkingEngine(**WALK_PARAMS) + dt = 0.008 + engine.march_in_place() + lifted = 0.0 + for _ in range(int(2.0 / dt)): + target = engine.step(dt) + lifted = max(lifted, target.left_sole[2], target.right_sole[2]) + assert engine.state == WalkState.IDLE_MARCH + assert lifted > 0.03 # the feet do leave the ground + np.testing.assert_allclose(engine.torso.position, [0.0, 0.0], atol=1e-12) diff --git a/tools/inspect_urdf.py b/tools/inspect_urdf.py new file mode 100644 index 0000000..667e94a --- /dev/null +++ b/tools/inspect_urdf.py @@ -0,0 +1,46 @@ +""" +Print every joint of the Aurea URDF with its offset from the parent link. + +Useful for measuring link lengths when writing analytical IK. + +Caveat: PyBullet reports parentFramePos relative to the parent's *inertial* +(centre of mass) frame, not the URDF link frame. For exact link lengths, read +the values in the URDF itself. + + python tools/inspect_urdf.py [path/to/robot.urdf] +""" + +import math +import os +import sys + +import pybullet as p + +DEFAULT_URDF = os.path.join(os.path.dirname(os.path.abspath(__file__)), + "..", "pybullet_sim", "assets", "aurea", "aurea.urdf") + + +def main() -> None: + urdf = sys.argv[1] if len(sys.argv) > 1 else DEFAULT_URDF + + p.connect(p.DIRECT) + robot_id = p.loadURDF(urdf, useFixedBase=True) + + print("\n" + "=" * 100) + print(f"Joints of {os.path.basename(urdf)} (offset from the parent's inertial frame)") + print("=" * 100) + + for j in range(p.getNumJoints(robot_id)): + info = p.getJointInfo(robot_id, j) + name = info[1].decode("utf-8") + x, y, z = info[14] # parentFramePos + length = math.sqrt(x**2 + y**2 + z**2) + print(f"ID: {j:02d} | Joint: {name:<14} | Offset (x, y, z): " + f"{x:+.4f}, {y:+.4f}, {z:+.4f} | Length: {length:.4f} m") + + print("=" * 100 + "\n") + p.disconnect() + + +if __name__ == "__main__": + main() diff --git a/tools/plot_trajectories.py b/tools/plot_trajectories.py new file mode 100644 index 0000000..01fb13e --- /dev/null +++ b/tools/plot_trajectories.py @@ -0,0 +1,110 @@ +""" +Plot what the analytical walking engine (mujoco_sim) generates, without +running a simulator: CoM, ZMP reference, footsteps and swing-foot height. + + python tools/plot_trajectories.py # walk forward + python tools/plot_trajectories.py --vx 0 --vy 0.03 # sidestep + python tools/plot_trajectories.py --save walk.png + +The ZMP shown is recovered from the CoM through the LIPM equation +zmp = com - com_ddot / lambda^2, with com_ddot from finite differences. +Each step's LIPM solution matches the CoM *position* at the step boundaries but +not its velocity, so the recovered ZMP spikes at step transitions; the axes are +limited to the feet/CoM range so those spikes do not hide the rest. +""" + +import argparse +import os +import sys + +import matplotlib.pyplot as plt +import numpy as np + +sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), "..")) + +from mujoco_sim.analytical_walking.walking_engine import WalkingEngine # noqa: E402 +from mujoco_sim.config import WALK_PARAMS # noqa: E402 + + +def simulate(vx: float, vy: float, wz: float, duration: float, dt: float) -> dict: + engine = WalkingEngine(**WALK_PARAMS) + engine.set_command(vx=vx, vy=vy, wz=wz) + rows = [engine.step(dt) for _ in range(int(duration / dt))] + t = np.arange(len(rows)) * dt + com = np.array([r.body_pos[:2] for r in rows]) + left = np.array([r.left_sole for r in rows]) + right = np.array([r.right_sole for r in rows]) + + lam2 = engine.g / engine.z_com + com_ddot = np.gradient(np.gradient(com, dt, axis=0), dt, axis=0) + zmp = com - com_ddot / lam2 + return {"t": t, "com": com, "zmp": zmp, "left": left, "right": right} + + +def _limits(d: dict, axis: int) -> tuple[float, float]: + """Axis range covering the feet and CoM (not the ZMP spikes), with margin.""" + values = np.concatenate([d["left"][:, axis], d["right"][:, axis], d["com"][:, axis]]) + lo, hi = values.min(), values.max() + margin = 0.1 * (hi - lo) + 0.01 + return lo - margin, hi + margin + + +def main() -> None: + parser = argparse.ArgumentParser(description="Plot analytical walking trajectories") + parser.add_argument("--vx", type=float, default=0.05, help="forward speed [m/s]") + parser.add_argument("--vy", type=float, default=0.0, help="lateral speed [m/s]") + parser.add_argument("--wz", type=float, default=0.0, help="turn rate [rad/s]") + parser.add_argument("--duration", type=float, default=3.0, help="seconds to plot") + parser.add_argument("--save", metavar="FILE", help="save the figure instead of showing it") + args = parser.parse_args() + + d = simulate(args.vx, args.vy, args.wz, args.duration, dt=0.002) + t = d["t"] + + fig, axes = plt.subplots(2, 2, figsize=(12, 7), constrained_layout=True) + fig.suptitle(f"Analytical walking — vx={args.vx} m/s, vy={args.vy} m/s, wz={args.wz} rad/s") + + for ax, i, label in ((axes[0, 0], 0, "x"), (axes[1, 0], 1, "y")): + ax.plot(t, d["left"][:, i], color="tab:blue", lw=1, label="left foot") + ax.plot(t, d["right"][:, i], color="tab:red", lw=1, label="right foot") + ax.plot(t, d["zmp"][:, i], color="tab:gray", ls="--", lw=1, label="ZMP (LIPM)") + ax.plot(t, d["com"][:, i], color="black", lw=2, label="CoM") + ax.set_ylabel(f"{label} [m]") + ax.set_ylim(*_limits(d, i)) + ax.grid(alpha=0.3) + axes[0, 0].set_title("Horizontal trajectories over time") + axes[1, 0].set_xlabel("time [s]") + axes[0, 0].legend(loc="upper left", fontsize=8) + + ax = axes[0, 1] + ax.plot(t, d["left"][:, 2] * 1000, color="tab:blue", label="left foot") + ax.plot(t, d["right"][:, 2] * 1000, color="tab:red", label="right foot") + ax.set_title("Swing-foot height") + ax.set_xlabel("time [s]") + ax.set_ylabel("z [mm]") + ax.grid(alpha=0.3) + ax.legend(fontsize=8) + + ax = axes[1, 1] + ax.plot(d["com"][:, 0], d["com"][:, 1], color="black", lw=2, label="CoM") + ax.plot(d["zmp"][:, 0], d["zmp"][:, 1], color="tab:gray", ls="--", lw=1, label="ZMP (LIPM)") + for key, color in (("left", "tab:blue"), ("right", "tab:red")): + on_ground = d[key][:, 2] < 1e-6 + ax.scatter(d[key][on_ground, 0], d[key][on_ground, 1], s=4, color=color, label=f"{key} foot (stance)") + ax.set_title("Top view") + ax.set_xlabel("x [m]") + ax.set_ylabel("y [m]") + ax.set_xlim(*_limits(d, 0)) + ax.set_ylim(*_limits(d, 1)) + ax.grid(alpha=0.3) + ax.legend(fontsize=8) + + if args.save: + fig.savefig(args.save, dpi=120) + print(f"Saved {args.save}") + else: + plt.show() + + +if __name__ == "__main__": + main()