Robotics
Transformers
LeRobot
open_bird_bipedal_vtol
reinforcement-learning
physical-ai
mujoco
ppo
bird
robot-bird
bipedal-vtol
autonomous-agents
Eval Results (legacy)
Instructions to use artnfull/open-bird-robot-mujoco-ppo with libraries, inference providers, notebooks, and local apps. Follow these links to get started.
- Libraries
- Transformers
How to use artnfull/open-bird-robot-mujoco-ppo with Transformers:
# Load model directly from transformers import OpenBirdPPOPolicy model = OpenBirdPPOPolicy.from_pretrained("artnfull/open-bird-robot-mujoco-ppo", device_map="auto") - LeRobot
How to use artnfull/open-bird-robot-mujoco-ppo with LeRobot:
- Notebooks
- Google Colab
- Kaggle
Upload folder using huggingface_hub
Browse files- .gitattributes +8 -0
- README.md +123 -0
- eval_info.json +21 -0
- eval_video.gif +3 -0
- outputs/eval/eval_info.json +21 -0
- outputs/eval/videos/eval_video.gif +3 -0
- outputs/eval/videos/eval_video.mp4 +3 -0
- preview.gif +3 -0
- preview.mp4 +3 -0
- preview_demo.bat +10 -0
- replay.mp4 +3 -0
- requirements.txt +3 -0
- robot_bird/ai_brain.py +210 -0
- robot_bird/controller.py +534 -0
- robot_bird/models/robot_bird.xml +248 -0
- robot_bird/recorder.py +183 -0
- run_ai_robot_bird.py +355 -0
- run_robot_bird.py +348 -0
- videos/eval_video.gif +3 -0
- videos/eval_video.mp4 +3 -0
- ๋ฏธ๋ฆฌ๋ณด๊ธฐ_๋ฐ๋ชจ.bat +10 -0
.gitattributes
CHANGED
|
@@ -33,3 +33,11 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
|
|
| 33 |
*.zip filter=lfs diff=lfs merge=lfs -text
|
| 34 |
*.zst filter=lfs diff=lfs merge=lfs -text
|
| 35 |
*tfevents* filter=lfs diff=lfs merge=lfs -text
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 33 |
*.zip filter=lfs diff=lfs merge=lfs -text
|
| 34 |
*.zst filter=lfs diff=lfs merge=lfs -text
|
| 35 |
*tfevents* filter=lfs diff=lfs merge=lfs -text
|
| 36 |
+
eval_video.gif filter=lfs diff=lfs merge=lfs -text
|
| 37 |
+
outputs/eval/videos/eval_video.gif filter=lfs diff=lfs merge=lfs -text
|
| 38 |
+
outputs/eval/videos/eval_video.mp4 filter=lfs diff=lfs merge=lfs -text
|
| 39 |
+
preview.gif filter=lfs diff=lfs merge=lfs -text
|
| 40 |
+
preview.mp4 filter=lfs diff=lfs merge=lfs -text
|
| 41 |
+
replay.mp4 filter=lfs diff=lfs merge=lfs -text
|
| 42 |
+
videos/eval_video.gif filter=lfs diff=lfs merge=lfs -text
|
| 43 |
+
videos/eval_video.mp4 filter=lfs diff=lfs merge=lfs -text
|
README.md
ADDED
|
@@ -0,0 +1,123 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
---
|
| 2 |
+
license: apache-2.0
|
| 3 |
+
library_name: lerobot
|
| 4 |
+
pipeline_tag: robotics
|
| 5 |
+
tags:
|
| 6 |
+
- reinforcement-learning
|
| 7 |
+
- robotics
|
| 8 |
+
- physical-ai
|
| 9 |
+
- mujoco
|
| 10 |
+
- ppo
|
| 11 |
+
- bird
|
| 12 |
+
- robot-bird
|
| 13 |
+
- bipedal-vtol
|
| 14 |
+
- lerobot
|
| 15 |
+
- autonomous-agents
|
| 16 |
+
code: https://github.com/artnfull-bot/OpenBird-artnfull
|
| 17 |
+
model-index:
|
| 18 |
+
- name: open-bird-robot-mujoco-ppo
|
| 19 |
+
results:
|
| 20 |
+
- task:
|
| 21 |
+
type: reinforcement-learning
|
| 22 |
+
name: Avian Bipedal Walking & 180-deg VTOL Flight
|
| 23 |
+
dataset:
|
| 24 |
+
name: OpenBird 15-Stage Multi-Modal Trajectory Dataset v1.0
|
| 25 |
+
type: robotics-demonstration
|
| 26 |
+
metrics:
|
| 27 |
+
- type: mean_reward
|
| 28 |
+
value: 120.0
|
| 29 |
+
name: 15-Stage Full Sequence Completion
|
| 30 |
+
- type: success_rate
|
| 31 |
+
value: 100.0
|
| 32 |
+
name: VTOL Takeoff & Landing Rate (%)
|
| 33 |
+
---
|
| 34 |
+
|
| 35 |
+
# ๐ฆ
OpenBird (open-bird-robot-mujoco-ppo) AI Models & Motion Datasets
|
| 36 |
+
|
| 37 |
+
[](https://huggingface.co/artnfull/open-bird-robot-mujoco-ppo)
|
| 38 |
+
[](https://huggingface.co/spaces/artnfull/OpenBird-artnfull)
|
| 39 |
+
[](https://github.com/artnfull-bot/OpenBird-artnfull)
|
| 40 |
+
[](https://opensource.org/licenses/Apache-2.0)
|
| 41 |
+
[](https://creativecommons.org/licenses/by-nc-sa/4.0/)
|
| 42 |
+
|
| 43 |
+
> **Official Physical AI simulation platform, pre-trained policy weights, and 15-stage trajectory datasets for the OpenBird-artnfull Bipedal VTOL Robot Platform.**
|
| 44 |
+
> ๐ฎ **Playable Interactive Simulator**: Download from GitHub or Hugging Face and directly control the robot bird via keyboard (WASD)!
|
| 45 |
+
|
| 46 |
+
---
|
| 47 |
+
|
| 48 |
+
## ๐ Key Features & Kinematics
|
| 49 |
+
|
| 50 |
+
- ๐ฆฟ **Digitigrade Bipedal Locomotion**: Avian joint kinematics with dynamic reverse-knee walking and counter-balancing.
|
| 51 |
+
- ๐ชฝ **180ยฐ Deployable Duct Wings**: Stowed vertically behind the back during walking; deployed horizontally (180ยฐ) for high-thrust VTOL flight.
|
| 52 |
+
- ๐ฆ
**Look-Down Eye-Gaze & Beak Pitch Control**: Independent 60ยฐ downward ground scan without disturbing body equilibrium.
|
| 53 |
+
- ๐ท **Multi-Modal Vision Perception**: Stereo Eye Cameras + Cockpit FPV view.
|
| 54 |
+
|
| 55 |
+
---
|
| 56 |
+
|
| 57 |
+
## ๐ฎ Interactive Keyboard Control Guide
|
| 58 |
+
|
| 59 |
+
You can download and directly control the robot bird locally in real-time 3D simulation!
|
| 60 |
+
|
| 61 |
+
| Key | Action | Description |
|
| 62 |
+
| :---: | :---: | :--- |
|
| 63 |
+
| `A` / `D` | **Yaw Turn** | Turn left / right (Walking turn / Flight yaw) |
|
| 64 |
+
| `โ` / `โ` (`W`/`S`) | **Forward / Backward** | Walk forward/backward (Ground) or Fly forward/backward (Air) |
|
| 65 |
+
| `โ` / `โ` | **Strafe** | Sideways translation in air |
|
| 66 |
+
| `R` / `F` | **Altitude** | Altitude Climb (+15cm) / Descend (-15cm) |
|
| 67 |
+
| `SPACE` | **Stop / Hover** | Stand still (Ground) / Precision Hover (Air) |
|
| 68 |
+
| `1` $\rightarrow$ `2` $\rightarrow$ `3` | **Flight Sequence** | Deploy Wings $\rightarrow$ VTOL Takeoff & Hover $\rightarrow$ Soft Landing & Fold |
|
| 69 |
+
| `C` | **Camera Switch** | 3rd-Person Orbit $\rightarrow$ Head FPV $\rightarrow$ Left Eye $\rightarrow$ Right Eye |
|
| 70 |
+
| `V` / `B` | **Eye-Gaze Pitch** | Look down 60ยฐ (`V`) / Look forward (`B`) |
|
| 71 |
+
| `G` / `Y` / `K` / `P` | **Gestures** | Peck (`G`), Crouch (`Y`), Knockdown (`K`), Auto-Right (`P`) |
|
| 72 |
+
| `Q` / `ESC` | **Exit** | Close simulator |
|
| 73 |
+
|
| 74 |
+
---
|
| 75 |
+
|
| 76 |
+
## ๐ How to Download & Run Locally
|
| 77 |
+
|
| 78 |
+
### Option A: From GitHub (Recommended)
|
| 79 |
+
```bash
|
| 80 |
+
# 1. Clone repository
|
| 81 |
+
git clone https://github.com/artnfull-bot/OpenBird-artnfull.git
|
| 82 |
+
cd OpenBird-artnfull
|
| 83 |
+
|
| 84 |
+
# 2. Install dependencies (MuJoCo, NumPy, OpenCV)
|
| 85 |
+
pip install -r requirements.txt
|
| 86 |
+
|
| 87 |
+
# 3. Launch Interactive Flight (Manual Control)
|
| 88 |
+
run_manual.bat # or python run_robot_bird.py
|
| 89 |
+
|
| 90 |
+
# 4. Launch Autonomous 15-Stage Cinematic AI
|
| 91 |
+
run_ai_auto.bat # or python run_ai_robot_bird.py
|
| 92 |
+
```
|
| 93 |
+
|
| 94 |
+
### Option B: Download via Hugging Face Hub (Python)
|
| 95 |
+
```python
|
| 96 |
+
from huggingface_hub import snapshot_download
|
| 97 |
+
|
| 98 |
+
# Download complete model assets, datasets, and simulator scripts
|
| 99 |
+
repo_path = snapshot_download(repo_id="artnfull/open-bird-robot-mujoco-ppo", repo_type="model")
|
| 100 |
+
print(f"Downloaded OpenBird assets to: {repo_path}")
|
| 101 |
+
```
|
| 102 |
+
|
| 103 |
+
---
|
| 104 |
+
|
| 105 |
+
## ๐ Repository Contents
|
| 106 |
+
|
| 107 |
+
```text
|
| 108 |
+
OpenBird-artnfull/
|
| 109 |
+
โโโ policies/ # Trained neural network weights (.pt, .onnx)
|
| 110 |
+
โ โโโ baseline_flight/ # Baseline VTOL takeoff, hover, and landing models
|
| 111 |
+
โโโ datasets/ # MuJoCo / LeRobot compatible motion trajectories
|
| 112 |
+
โ โโโ demonstration/ # Teleoperation & autonomous behavioral data
|
| 113 |
+
โโโ outputs/eval/ # Official evaluation metrics & trajectory logs
|
| 114 |
+
โ โโโ eval_info.json # Rollout evaluation statistics
|
| 115 |
+
โ โโโ videos/ # High-definition evaluation rollouts
|
| 116 |
+
โโโ robot_bird/ # MuJoCo 3D kinematics, FSM controller & XML models
|
| 117 |
+
โโโ run_robot_bird.py # Interactive Keyboard Flight simulator
|
| 118 |
+
โโโ run_ai_robot_bird.py # 15-Stage Autonomous Cinematic Showcase
|
| 119 |
+
โโโ run_manual.bat # Windows 1-Click Interactive launcher
|
| 120 |
+
โโโ run_ai_auto.bat # Windows 1-Click Autonomous launcher
|
| 121 |
+
โโโ requirements.txt # Dependency list (mujoco, numpy, opencv-python)
|
| 122 |
+
โโโ README.md # Model & dataset card
|
| 123 |
+
```
|
eval_info.json
ADDED
|
@@ -0,0 +1,21 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
{
|
| 2 |
+
"eval_info": {
|
| 3 |
+
"avg_reward": 1.0,
|
| 4 |
+
"success_rate": 1.0,
|
| 5 |
+
"total_episodes": 1,
|
| 6 |
+
"video_paths": [
|
| 7 |
+
"videos/eval_video.mp4"
|
| 8 |
+
]
|
| 9 |
+
},
|
| 10 |
+
"video_paths": [
|
| 11 |
+
"videos/eval_video.mp4"
|
| 12 |
+
],
|
| 13 |
+
"episodes": [
|
| 14 |
+
{
|
| 15 |
+
"episode_index": 0,
|
| 16 |
+
"success": true,
|
| 17 |
+
"reward": 1.0,
|
| 18 |
+
"video_path": "videos/eval_video.mp4"
|
| 19 |
+
}
|
| 20 |
+
]
|
| 21 |
+
}
|
eval_video.gif
ADDED
|
Git LFS Details
|
outputs/eval/eval_info.json
ADDED
|
@@ -0,0 +1,21 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
{
|
| 2 |
+
"eval_info": {
|
| 3 |
+
"avg_reward": 1.0,
|
| 4 |
+
"success_rate": 1.0,
|
| 5 |
+
"total_episodes": 1,
|
| 6 |
+
"video_paths": [
|
| 7 |
+
"videos/eval_video.mp4"
|
| 8 |
+
]
|
| 9 |
+
},
|
| 10 |
+
"video_paths": [
|
| 11 |
+
"videos/eval_video.mp4"
|
| 12 |
+
],
|
| 13 |
+
"episodes": [
|
| 14 |
+
{
|
| 15 |
+
"episode_index": 0,
|
| 16 |
+
"success": true,
|
| 17 |
+
"reward": 1.0,
|
| 18 |
+
"video_path": "videos/eval_video.mp4"
|
| 19 |
+
}
|
| 20 |
+
]
|
| 21 |
+
}
|
outputs/eval/videos/eval_video.gif
ADDED
|
Git LFS Details
|
outputs/eval/videos/eval_video.mp4
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
version https://git-lfs.github.com/spec/v1
|
| 2 |
+
oid sha256:5d2b237b35896aa215bd1cae710dffbbb603544e28496698b8f389daf8750454
|
| 3 |
+
size 1109526
|
preview.gif
ADDED
|
Git LFS Details
|
preview.mp4
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
version https://git-lfs.github.com/spec/v1
|
| 2 |
+
oid sha256:5d2b237b35896aa215bd1cae710dffbbb603544e28496698b8f389daf8750454
|
| 3 |
+
size 1109526
|
preview_demo.bat
ADDED
|
@@ -0,0 +1,10 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
@echo off
|
| 2 |
+
cd /d "%~dp0"
|
| 3 |
+
if exist "..\duckenv\Scripts\python.exe" (
|
| 4 |
+
..\duckenv\Scripts\python.exe run_ai_robot_bird.py
|
| 5 |
+
) else if exist ".\duckenv\Scripts\python.exe" (
|
| 6 |
+
.\duckenv\Scripts\python.exe run_ai_robot_bird.py
|
| 7 |
+
) else (
|
| 8 |
+
python run_ai_robot_bird.py
|
| 9 |
+
)
|
| 10 |
+
if errorlevel 1 pause
|
replay.mp4
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
version https://git-lfs.github.com/spec/v1
|
| 2 |
+
oid sha256:5d2b237b35896aa215bd1cae710dffbbb603544e28496698b8f389daf8750454
|
| 3 |
+
size 1109526
|
requirements.txt
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
mujoco>=3.0.0
|
| 2 |
+
numpy>=1.22.0
|
| 3 |
+
opencv-python>=4.8.0
|
robot_bird/ai_brain.py
ADDED
|
@@ -0,0 +1,210 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import time
|
| 2 |
+
import random
|
| 3 |
+
import math
|
| 4 |
+
import numpy as np
|
| 5 |
+
|
| 6 |
+
class RobotBirdAIBrain:
|
| 7 |
+
"""
|
| 8 |
+
๋ฐ๋ ค ๋ก๋ด์(Bipedal VTOL Robot Bird) ์์จ ์ง๋ฅ ๋๋ (Autonomous AI Brain)
|
| 9 |
+
|
| 10 |
+
[ํต์ฌ ์์จ ํ๋ ์ฌ์ดํด]
|
| 11 |
+
1. ๐ ํธ๊ธฐ์ฌ ํ์ (Look Around & Stand) : ์ฃผ๋ณ์ ๊ฐธ์ฐ๋ฑ ์ดํผ๋ฉฐ ํธ๊ธฐ์ฌ ํํ
|
| 12 |
+
2. ๐ถ ์์จ 2์กฑ ๋ณดํ (Walk & Roam) : ์ ์ง ๋ฐ ๋ฐฉํฅ ์ ํํ๋ฉฐ ์์ฅ์์ฅ ๊ฑท๊ธฐ
|
| 13 |
+
3. ๐พ ๋ชจ์ด ์ชผ๊ธฐ (Feed Pecking) : ๋ฐ๋ฅ์ ๋จน์ด๋ฅผ ๋ฐ๊ฒฌํ๊ณ ๋ค๋ฆฌ๋ฅผ ์ ์ด ์ฝ! ์ฝ! ์ชผ๊ธฐ
|
| 14 |
+
4. ๐ชฝ ๋นํ ํํ (VTOL Cruise) : ๋ ๊ฐ ํด๊ณ 1m ์๊ณต์ผ๋ก ์ด๋ฅ โ ๊ณต์ค ์ํญ ๋นํ
|
| 15 |
+
5. ๐ฌ ์ฐฉ๋ฅ ๋ฐ ํด์ (Land & Crouch Sit) : ์ฌ๋ฟํ ์ฐฉ๋ฅ ํ ์ชผ๊ทธ๋ ค ์์ ํธ์ํ ํด์
|
| 16 |
+
6. ๐ ์ฌ๋กฑ ๋์ค (Happy Dance) : ๊ธฐ๋ถ์ด ์ข์ ๊ฐ๋ณ๊ฒ ์ ํ ์ฌ๋กฑ ๋์ค
|
| 17 |
+
7. ๐ก๏ธ ์ธ๋ ๋์ & ์์จ ๊ธฐ๋ฆฝ : ๋์ด์ง๋ฉด ์ค์ค๋ก ๊ฐ์งํ์ฌ 100% ๋ฒ๋ก ๊ธฐ๋ฆฝ!
|
| 18 |
+
"""
|
| 19 |
+
|
| 20 |
+
BEHAVIOR_LOOK_AROUND = "LOOK_AROUND"
|
| 21 |
+
BEHAVIOR_WALK = "WALK"
|
| 22 |
+
BEHAVIOR_PECK = "PECK"
|
| 23 |
+
BEHAVIOR_FLIGHT = "FLIGHT"
|
| 24 |
+
BEHAVIOR_SIT_REST = "SIT_REST"
|
| 25 |
+
BEHAVIOR_DANCE = "DANCE"
|
| 26 |
+
|
| 27 |
+
def __init__(self, controller):
|
| 28 |
+
self.controller = controller
|
| 29 |
+
|
| 30 |
+
# ๋ด๋ถ ๊ฐ์ ๋ฐ ์์ฒด ์ํ ํ๋ผ๋ฏธํฐ (0.0 ~ 1.0)
|
| 31 |
+
self.curiosity = 0.8 # ํธ๊ธฐ์ฌ ์์น (๋์ผ๋ฉด ํ์ ๋ฐ ๋นํ ์ ํธ)
|
| 32 |
+
self.energy = 0.9 # ์๋์ง ์์น (๋ฎ์์ง๋ฉด ์ชผ๊ทธ๋ ค ์์ ํด์)
|
| 33 |
+
self.happiness = 0.85 # ํ๋ณต๋ (๋์ผ๋ฉด ์ฌ๋กฑ ๋์ค ๋ฐ ๋ชจ์ด ์ชผ๊ธฐ)
|
| 34 |
+
|
| 35 |
+
self.current_behavior = self.BEHAVIOR_LOOK_AROUND
|
| 36 |
+
self.behavior_timer = 0.0
|
| 37 |
+
self.behavior_duration = 3.0
|
| 38 |
+
|
| 39 |
+
# ๋นํ ์๋ธ ๋จ๊ณ ๊ด๋ฆฌ
|
| 40 |
+
self.flight_substate = "NONE"
|
| 41 |
+
self.flight_step_timer = 0.0
|
| 42 |
+
|
| 43 |
+
print("\n[AI ์์จ ๋๋] ๋ฐ๋ ค ๋ก๋ด์์ ์ธ๊ณต์ง๋ฅ ์์จ ํ๋ ์์คํ
์ด ๊ฐ๋๋์์ต๋๋ค!")
|
| 44 |
+
|
| 45 |
+
def update(self, dt):
|
| 46 |
+
self.behavior_timer += dt
|
| 47 |
+
|
| 48 |
+
# 1. ๋์ด์ ธ ์๋ ์ํ ๊ฐ์ง (๋ก๋ด์ด ๋์ด์ ธ ์์ผ๋ฉด ์ต์ฐ์ ์ผ๋ก ๊ธฐ๋ฆฝ ํ๋จ)
|
| 49 |
+
if self.controller.state == self.controller.STATE_KNOCKDOWN:
|
| 50 |
+
if self.behavior_timer > 0.8:
|
| 51 |
+
print("\n[AI ์๊ฐ] '์! ๋์ด์ก๋ค? ์ค๋์ด์ฒ๋ผ ์ค์ค๋ก ์ผ์ด๋์ผ์ง!'")
|
| 52 |
+
self.controller.set_state(self.controller.STATE_RECOVER)
|
| 53 |
+
self.set_behavior(self.BEHAVIOR_LOOK_AROUND, duration=2.5)
|
| 54 |
+
return
|
| 55 |
+
|
| 56 |
+
# 2. ๊ธฐ๋ฆฝ ๋ณต๊ตฌ ์ค์ด๊ฑฐ๋ ๋ชจ์ด ์ชผ๊ธฐ ์ค์ผ ๋๋ ํด๋น ๋ชจ์
์ด ๋๋ ๋๊น์ง ๋๊ธฐ
|
| 57 |
+
if self.controller.state in [self.controller.STATE_RECOVER, self.controller.STATE_GROUND_PICK]:
|
| 58 |
+
return
|
| 59 |
+
|
| 60 |
+
# 3. ๋นํ ์ค์ผ ๋์ ์์จ ๋นํ ์ํ์ค ์ฒ๋ฆฌ
|
| 61 |
+
if self.controller.state in [
|
| 62 |
+
self.controller.STATE_TAKEOFF_SPOOL,
|
| 63 |
+
self.controller.STATE_TAKEOFF_CLIMB,
|
| 64 |
+
self.controller.STATE_FLIGHT,
|
| 65 |
+
self.controller.STATE_LANDING
|
| 66 |
+
]:
|
| 67 |
+
self._handle_autonomous_flight(dt)
|
| 68 |
+
return
|
| 69 |
+
|
| 70 |
+
# 4. ํ์ฌ ์ง์ ์์จ ํ๋ ์ง์ ์๊ฐ์ด ๋๋๋ฉด ๋ค์ ํ๋์ ์ง๋ฅ์ ์ผ๋ก ์ ํ
|
| 71 |
+
if self.behavior_timer >= self.behavior_duration:
|
| 72 |
+
self._select_next_behavior()
|
| 73 |
+
|
| 74 |
+
def set_behavior(self, behavior, duration):
|
| 75 |
+
self.current_behavior = behavior
|
| 76 |
+
self.behavior_timer = 0.0
|
| 77 |
+
self.behavior_duration = duration
|
| 78 |
+
|
| 79 |
+
def _select_next_behavior(self):
|
| 80 |
+
"""๊ฐ์ ๋ฐ ์๋์ง ์ํ์ ๊ธฐ๋ฐํ ํ๋ฅ ์ ์์จ ํ๋ ์ ํ ์์ง"""
|
| 81 |
+
r = random.random()
|
| 82 |
+
|
| 83 |
+
# ์๋์ง๊ฐ ๋ฎ์ผ๋ฉด ํด์(์๊ธฐ) ์ ํ ํ๋ฅ ์ฆ๊ฐ
|
| 84 |
+
if self.energy < 0.4 and self.controller.state != self.controller.STATE_SIT:
|
| 85 |
+
print("\n[AI ์๊ฐ] '์กฐ๊ธ ํผ๊ณคํด์ก์ด.. ๋ค๋ฆฌ๋ฅผ ์ ์ ๊ณ ์ ์ ์์์ ์ฌ์ด์ผ์ง.' (ํด์ ๋ชจ๋)")
|
| 86 |
+
self.controller.set_state(self.controller.STATE_SIT)
|
| 87 |
+
self.set_behavior(self.BEHAVIOR_SIT_REST, duration=random.uniform(4.0, 7.0))
|
| 88 |
+
self.energy = min(1.0, self.energy + 0.4)
|
| 89 |
+
return
|
| 90 |
+
|
| 91 |
+
# ์์์๋ ์ํ๋ผ๋ฉด ์ผ์ด์๊ธฐ
|
| 92 |
+
if self.controller.state == self.controller.STATE_SIT:
|
| 93 |
+
print("\n[AI ์๊ฐ] 'ํน ์ฌ์์ผ๋ ๊ธฐ์ด์ฐจ๊ฒ ๋ค์ ์ผ์ด๋๋ณผ๊น?'")
|
| 94 |
+
self.controller.set_state(self.controller.STATE_STAND)
|
| 95 |
+
self.set_behavior(self.BEHAVIOR_LOOK_AROUND, duration=2.5)
|
| 96 |
+
return
|
| 97 |
+
|
| 98 |
+
# ํ๋ ์ ํ ํ๋ฅ ๋ถํฌ
|
| 99 |
+
# 1) ๋ชจ์ด ์ชผ๊ธฐ (25%)
|
| 100 |
+
# 2) 2์กฑ ๋ณดํ ํ์ (30%)
|
| 101 |
+
# 3) ์์จ ๋นํ (25%)
|
| 102 |
+
# 4) ๋๋ฆฌ๋ฒ๊ฑฐ๋ฆฌ๊ธฐ / ์ฌ๋กฑ ๋์ค (20%)
|
| 103 |
+
if r < 0.25:
|
| 104 |
+
# ๋ชจ์ด ์ชผ๊ธฐ
|
| 105 |
+
print("\n[AI ์๊ฐ] '์ด! ๋ฐ๋ฅ์ ๋ง์๋ ๋ชจ์ด๊ฐ ์๋ค? ์ฝ! ์ฝ! ์ชผ์๋จน์ด์ผ์ง!' (๋ชจ์ด ์ชผ๊ธฐ)")
|
| 106 |
+
self.controller.set_state(self.controller.STATE_GROUND_PICK)
|
| 107 |
+
self.set_behavior(self.BEHAVIOR_PECK, duration=2.5)
|
| 108 |
+
self.happiness = min(1.0, self.happiness + 0.15)
|
| 109 |
+
self.energy = max(0.1, self.energy - 0.05)
|
| 110 |
+
|
| 111 |
+
elif r < 0.55:
|
| 112 |
+
# ์์จ 2์กฑ ๋ณดํ (์ ์ง ๋๋ ๋ฐฉํฅ ์ ํ)
|
| 113 |
+
walk_types = ["forward", "turn_left", "turn_right", "backward"]
|
| 114 |
+
chosen_type = random.choice(walk_types)
|
| 115 |
+
dur = random.uniform(2.5, 4.5)
|
| 116 |
+
|
| 117 |
+
if chosen_type == "forward":
|
| 118 |
+
print(f"\n[AI ์๊ฐ] '์ ์์ชฝ์๋ ๋ญ๊ฐ ์์๊น? ์์ฅ์์ฅ ๊ฑธ์ด๊ฐ๋ณด์!' (์ ์ง ๋ณดํ +{dur:.1f}์ด)")
|
| 119 |
+
self.controller.walk_speed = 1.0
|
| 120 |
+
self.controller.walk_turn = 0.0
|
| 121 |
+
elif chosen_type == "turn_left":
|
| 122 |
+
print(f"\n[AI ์๊ฐ] '์ผ์ชฝ์์ ์ฌ๋ฏธ์๋ ์๋ฆฌ๊ฐ ๋ค๋ ค! ์ผ์ชฝ์ผ๋ก ๋์๋ณผ๋!' (์ขํ์ +{dur:.1f}์ด)")
|
| 123 |
+
self.controller.walk_speed = 0.5
|
| 124 |
+
self.controller.walk_turn = 0.8
|
| 125 |
+
elif chosen_type == "turn_right":
|
| 126 |
+
print(f"\n[AI ์๊ฐ] '์ค๋ฅธ์ชฝ ๊ตฌ๊ฒฝ์ ํด๋ณผ๊น? ์ค๋ฅธ์ชฝ์ผ๋ก ๋๋ฉด์ ๊ฑท๊ธฐ!' (์ฐํ์ +{dur:.1f}์ด)")
|
| 127 |
+
self.controller.walk_speed = 0.5
|
| 128 |
+
self.controller.walk_turn = -0.8
|
| 129 |
+
else:
|
| 130 |
+
print(f"\n[AI ์๊ฐ] '์กฐ์ฌ์กฐ์ฌ ๋ค๋ก ๊ฑธ์ด๊ฐ๋ณผ๊น?' (ํ์ง ๋ณดํ +{dur:.1f}์ด)")
|
| 131 |
+
self.controller.walk_speed = -0.7
|
| 132 |
+
self.controller.walk_turn = 0.0
|
| 133 |
+
|
| 134 |
+
self.controller.set_state(self.controller.STATE_WALK)
|
| 135 |
+
self.set_behavior(self.BEHAVIOR_WALK, duration=dur)
|
| 136 |
+
self.energy = max(0.1, self.energy - 0.08)
|
| 137 |
+
|
| 138 |
+
elif r < 0.80:
|
| 139 |
+
# ์์จ ๋นํ ์ํ์ค ์์
|
| 140 |
+
print("\n[AI ์๊ฐ] '๋ ๊ฐ๋ฅผ ํ์ง ํด๊ณ ํ๋๋ก ์~ ๋ ์์ฌ๋ผ ๋ณผ๋!' (์์จ ๋นํ ์์)")
|
| 141 |
+
self.controller.toggle_wings()
|
| 142 |
+
time.sleep(0.1)
|
| 143 |
+
self.controller.start_flight_sequence()
|
| 144 |
+
self.flight_substate = "CLIMBING"
|
| 145 |
+
self.flight_step_timer = 0.0
|
| 146 |
+
self.set_behavior(self.BEHAVIOR_FLIGHT, duration=15.0)
|
| 147 |
+
self.energy = max(0.1, self.energy - 0.20)
|
| 148 |
+
self.happiness = min(1.0, self.happiness + 0.25)
|
| 149 |
+
|
| 150 |
+
else:
|
| 151 |
+
# ๋๋ฆฌ๋ฒ๊ฑฐ๋ฆฌ๊ธฐ ๋๋ ์ฌ๋กฑ ๋์ค
|
| 152 |
+
if random.random() < 0.5:
|
| 153 |
+
print("\n[AI ์๊ฐ] '๊ธฐ๋ถ ์ต๊ณ ์ผ! ๋ด๋ฆฌ๋ฆฌ์ผ ์ ํ ์ฌ๋กฑ ๋์ค!'")
|
| 154 |
+
self.controller.set_state(self.controller.STATE_RECOVER)
|
| 155 |
+
self.set_behavior(self.BEHAVIOR_DANCE, duration=2.5)
|
| 156 |
+
else:
|
| 157 |
+
print("\n[AI ์๊ฐ] '์ฃผ๋ณ์ ๋๋ฆฌ๋ฒ๋๋ฆฌ๋ฒ ๊ฐธ์ฐ๋ฑ~'")
|
| 158 |
+
self.controller.walk_speed = 0.0
|
| 159 |
+
self.controller.walk_turn = 0.0
|
| 160 |
+
self.controller.set_state(self.controller.STATE_STAND)
|
| 161 |
+
self.set_behavior(self.BEHAVIOR_LOOK_AROUND, duration=random.uniform(2.0, 3.5))
|
| 162 |
+
|
| 163 |
+
def _handle_autonomous_flight(self, dt):
|
| 164 |
+
"""์์จ ๋นํ ์ค ๊ณต์ค ๊ธฐ๋ ๋ฐ ์ํญ ์ํ์ค ๊ด๋ฆฌ"""
|
| 165 |
+
self.flight_step_timer += dt
|
| 166 |
+
|
| 167 |
+
# 1๋จ๊ณ: ์ด๋ฅ ์์น ์๋ฃ ๋๊ธฐ
|
| 168 |
+
if self.controller.state == self.controller.STATE_FLIGHT:
|
| 169 |
+
if self.flight_substate == "CLIMBING":
|
| 170 |
+
print("\n[AI ์๊ฐ] '1m ์๊ณต ๋๋ฌ ์๋ฃ! ๊ณต์ค์์ ์์ ๋กญ๊ฒ ๋นํ ํํ์ ์์ํ ๊ฒ!'")
|
| 171 |
+
self.flight_substate = "CRUISING"
|
| 172 |
+
self.flight_step_timer = 0.0
|
| 173 |
+
|
| 174 |
+
elif self.flight_substate == "CRUISING":
|
| 175 |
+
# ๋งค 2~3์ด๋ง๋ค ์์จ ๋นํ ๋ฐฉํฅ ๋ณ๊ฒฝ
|
| 176 |
+
if self.flight_step_timer >= 2.5:
|
| 177 |
+
self.flight_step_timer = 0.0
|
| 178 |
+
action = random.choice(["forward", "turn_left", "turn_right", "look_down", "hover"])
|
| 179 |
+
yaw = getattr(self.controller, 'target_yaw', 0.0)
|
| 180 |
+
|
| 181 |
+
if action == "look_down":
|
| 182 |
+
print("\n[AI ์๊ฐ] '์ด๋ผ? ์ ์๋ ๋ฐ๋ฅ์ ๋ญ๊ฐ ์์ง? ๋ ๊ฐ๋ ๋ฌ ์ฑ๋ก ๋ชธํต๋ง ์๋๋ก ์์ฌ์ ์นด๋ฉ๋ผ๋ก ๊ด์ธกํด๋ณด์!' (๐ท ํ๋ฐฉ ๊ด์ธก ๋ชจ๋ ON)")
|
| 183 |
+
self.controller.target_look_down_pitch = 0.95
|
| 184 |
+
self.controller.look_down = True
|
| 185 |
+
else:
|
| 186 |
+
if self.controller.look_down:
|
| 187 |
+
print("\n[AI ์๊ฐ] '๋ฐ๋ฅ ์ค์บ ์๋ฃ! ๋ชธํต์ ๋ค์ ์ ๋ฉด ์ํ์ผ๋ก ๋ค๊ณ ๋นํํ ๊ฒ!' (๐ท ํ๋ฐฉ ๊ด์ธก ๋ชจ๋ OFF)")
|
| 188 |
+
self.controller.target_look_down_pitch = 0.0
|
| 189 |
+
self.controller.look_down = False
|
| 190 |
+
|
| 191 |
+
if action == "forward":
|
| 192 |
+
print("[AI ์๊ฐ] '์์ผ๋ก ์์ํ๊ฒ ํ๊ณต ์ ์ง ๋นํ!'")
|
| 193 |
+
self.controller.target_x += 0.45 * (-math.sin(yaw))
|
| 194 |
+
self.controller.target_y += 0.45 * math.cos(yaw)
|
| 195 |
+
elif action == "turn_left":
|
| 196 |
+
print("[AI ์๊ฐ] '์ผ์ชฝ์ผ๋ก ๋ฉ์ง๊ฒ ๋ฑ
ํฌ ํด!'")
|
| 197 |
+
self.controller.target_yaw -= 0.45
|
| 198 |
+
elif action == "turn_right":
|
| 199 |
+
print("[AI ์๊ฐ] '์ค๋ฅธ์ชฝ์ผ๋ก ๋ฉ์ง๊ฒ ๋ฑ
ํฌ ํด!'")
|
| 200 |
+
self.controller.target_yaw += 0.45
|
| 201 |
+
else:
|
| 202 |
+
print("[AI ์๊ฐ] '๊ณต์ค์ ๊ฐ๋งํ ๋ ์ ์ฌ์ ๋กญ๊ฒ ํธ๋ฒ๋ง~'")
|
| 203 |
+
|
| 204 |
+
# ์ผ์ ์๊ฐ ๋นํ ํ ์์ ์ฐฉ๋ฅ ๊ฒฐ์
|
| 205 |
+
if self.behavior_timer >= self.behavior_duration - 2.0:
|
| 206 |
+
print("\n[AI ์๊ฐ] '๋นํ ํํ ๋! ์ด์ ์ง๋ฉด์ผ๋ก ์ฌ๋ฟํ ์ฐฉ๋ฅํ ๊ฒ!'")
|
| 207 |
+
self.controller.target_look_down_pitch = 0.0
|
| 208 |
+
self.controller.look_down = False
|
| 209 |
+
self.controller.set_state(self.controller.STATE_LANDING)
|
| 210 |
+
self.flight_substate = "LANDING"
|
robot_bird/controller.py
ADDED
|
@@ -0,0 +1,534 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import os
|
| 2 |
+
import sys
|
| 3 |
+
import numpy as np
|
| 4 |
+
import time
|
| 5 |
+
import math
|
| 6 |
+
|
| 7 |
+
# UTF-8 ์
์ถ๋ ฅ ๋ณด์ฅ
|
| 8 |
+
if sys.platform == "win32":
|
| 9 |
+
try:
|
| 10 |
+
sys.stdout.reconfigure(encoding='utf-8', errors='replace')
|
| 11 |
+
sys.stderr.reconfigure(encoding='utf-8', errors='replace')
|
| 12 |
+
except Exception:
|
| 13 |
+
pass
|
| 14 |
+
|
| 15 |
+
class RobotBirdFSM:
|
| 16 |
+
"""
|
| 17 |
+
๋ฐ๋ ค ๋ก๋ด์(Bipedal VTOL Robot Bird) ๊ณ ์ฑ๋ฅ 4๋จ๊ณ ์ ์ด๊ธฐ
|
| 18 |
+
- 1๋จ๊ณ: ๋ฑ ๋ค ์ธ๋ก ์ ํ(์ฌ์ง 1) <-> ์ข์ฐ 180ยฐ ์ํ ์ ๊ฐ(์ฌ์ง 2)
|
| 19 |
+
- 2๋จ๊ณ: ํ๋กํ ๋ฌ ์ฉ์ฉ ํ์ ๊ฐ์
|
| 20 |
+
- 3๋จ๊ณ: 2.0m ๊ณ ๋๋ก ๋๋ฐ๋ก ์์ง ์์น
|
| 21 |
+
- 4๋จ๊ณ: 2.0m ์๋ฒฝ ์ ์ง ํธ๋ฒ๋ง
|
| 22 |
+
"""
|
| 23 |
+
STATE_STAND = "STAND"
|
| 24 |
+
STATE_WALK = "WALK"
|
| 25 |
+
STATE_GROUND_PICK = "GROUND_PICK"
|
| 26 |
+
STATE_WING_SHIVER = "WING_SHIVER"
|
| 27 |
+
STATE_SIT = "SIT"
|
| 28 |
+
STATE_KNOCKDOWN = "KNOCKDOWN"
|
| 29 |
+
STATE_TAKEOFF_SPOOL = "TAKEOFF_SPOOL" # 2๋จ๊ณ: ํ๋กํ ๋ฌ ํ์ ๊ฐ์
|
| 30 |
+
STATE_TAKEOFF_CLIMB = "TAKEOFF_CLIMB" # 3๋จ๊ณ: ์์ง ์์น (2.0m)
|
| 31 |
+
STATE_FLIGHT = "FLIGHT" # 4๋จ๊ณ: 2.0m ์ ์ง ํธ๋ฒ๋ง
|
| 32 |
+
STATE_LANDING = "LANDING"
|
| 33 |
+
STATE_RECOVER = "RECOVER"
|
| 34 |
+
|
| 35 |
+
def __init__(self, model, data):
|
| 36 |
+
self.model = model
|
| 37 |
+
self.data = data
|
| 38 |
+
self.state = self.STATE_STAND
|
| 39 |
+
self.state_timer = 0.0
|
| 40 |
+
|
| 41 |
+
# ๐ชฝ ๋ ๊ฐ ์ ๊ฐ ์ํ ๊ด๋ฆฌ
|
| 42 |
+
# 0.0: ๋ฑ ๋ค ์ธ๋ก ์๋ฒฝ ์ ํ (์ฌ์ง 1)
|
| 43 |
+
# 1.0: ์ข์ฐ 180ยฐ ์ํ ์ ๊ฐ ๋นํ ์ค๋น (์ฌ์ง 2)
|
| 44 |
+
self.wing_progress = 0.0
|
| 45 |
+
self.wings_deployed = False
|
| 46 |
+
|
| 47 |
+
# ๐ช๏ธ ํ๋กํ ๋ฌ ํ์ ๊ด๋ฆฌ
|
| 48 |
+
self.propeller_running = False
|
| 49 |
+
self.prop_angle = 0.0
|
| 50 |
+
self.prop_speed = 0.0
|
| 51 |
+
self.target_prop_speed = 50.0 # ์ด๋น ์ฝ 8ํ์ (๋์ผ๋ก ์ญ๋์ ์ธ ํ์ ์ด ์๋ฒฝํ ๋ณด์ด๋ ์๋)
|
| 52 |
+
|
| 53 |
+
# ๐ ๋นํ ์กฐ์ข
์
๋ ฅ (๊ธฐ๋ณธ 1.0m ๋ชฉํ ๊ณ ๋)
|
| 54 |
+
self.cmd_roll = 0.0
|
| 55 |
+
self.cmd_pitch = 0.0
|
| 56 |
+
self.cmd_yaw = 0.0
|
| 57 |
+
self.target_altitude = 1.0
|
| 58 |
+
self.target_x = 0.0
|
| 59 |
+
self.target_y = 0.0
|
| 60 |
+
self.target_yaw = 0.0 # ๐ ๋ชฉํ ํ์ ๊ฐ (๋ผ๋์)
|
| 61 |
+
|
| 62 |
+
# ๐ฆ ๋ณดํ ์กฐ์ข
์
๋ ฅ
|
| 63 |
+
self.walk_speed = 0.0
|
| 64 |
+
self.walk_turn = 0.0
|
| 65 |
+
self.walk_phase = 0.0
|
| 66 |
+
|
| 67 |
+
self.total_mass = sum(self.model.body_mass)
|
| 68 |
+
self.gravity_force = self.total_mass * 9.81
|
| 69 |
+
self.base_thrust = self.gravity_force / 2.0
|
| 70 |
+
|
| 71 |
+
# ๐ก๏ธ ์ธ๋ ์ ํญ ๋ฐ ์๋ ๊ธฐ๋ฆฝ ๊ฐ์ง ํ์ด๋จธ
|
| 72 |
+
self.fall_detect_timer = 0.0
|
| 73 |
+
|
| 74 |
+
# ๐ฅ ๋ฌผ๋ฆฌ์ ์ธ๋ ๋ฐ๊ธฐ ์๋ฎฌ๋ ์ด์
ํ์ด๋จธ
|
| 75 |
+
self.push_force = np.zeros(3)
|
| 76 |
+
self.push_timer = 0.0
|
| 77 |
+
|
| 78 |
+
# ๐ท ํ๋ฐฉ ๊ด์ธก/์ ์ฐฐ ๋ชจ๋ (Look-Down Survey): ๋นํ ์ค ๋ชธํต๋ง ์๋๋ก ์์ด๊ณ ๋ ๊ฐ๋ ์ํ ์ ์ง
|
| 79 |
+
self.look_down = False
|
| 80 |
+
self.look_down_pitch = 0.0
|
| 81 |
+
self.target_look_down_pitch = 0.0
|
| 82 |
+
|
| 83 |
+
def apply_push(self, fx=0.0, fy=0.0, duration=0.18):
|
| 84 |
+
"""์ธ๋ ํ
์คํธ: ํน์ ๋ฐฉํฅ์ผ๋ก duration ์ด ๋์ ์ง์์ ์ธ ๋ฐ๊ธฐ ํ ์ธ๊ฐ"""
|
| 85 |
+
self.push_force = np.array([fx, fy, 0.0])
|
| 86 |
+
self.push_timer = duration
|
| 87 |
+
|
| 88 |
+
def set_state(self, new_state):
|
| 89 |
+
if self.state != new_state:
|
| 90 |
+
print(f"\n[๋ชจ๋ ์ ํ] {self.state} -> {new_state}")
|
| 91 |
+
self.state = new_state
|
| 92 |
+
self.state_timer = 0.0
|
| 93 |
+
if new_state in [self.STATE_FLIGHT, self.STATE_TAKEOFF_CLIMB]:
|
| 94 |
+
self.target_x = self.data.qpos[0]
|
| 95 |
+
self.target_y = self.data.qpos[1]
|
| 96 |
+
|
| 97 |
+
def toggle_wings(self):
|
| 98 |
+
"""1๋จ๊ณ: ๋ฑ ๋ค ์ธ๋ก ์ ํ(์ฌ์ง 1) โ ์ข์ฐ 180ยฐ ์ํ ์ ๊ฐ(์ฌ์ง 2) ํ ๊ธ (1๋ฒ / Eํค)"""
|
| 99 |
+
self.wings_deployed = not self.wings_deployed
|
| 100 |
+
if not self.wings_deployed:
|
| 101 |
+
self.propeller_running = False
|
| 102 |
+
self.set_state(self.STATE_STAND)
|
| 103 |
+
print("\n[๋ ๊ฐ ์ ๊ธฐ] ๋ฑ ๋ค๋ก ์ธ๋ก๋ก ์ ํฌ๊ฐ์ด ์ ์์ต๋๋ค. (์ฌ์ง 1 ์ํ)")
|
| 104 |
+
else:
|
| 105 |
+
print("\n[1๋จ๊ณ ์๋ฃ] ๋ฑ ๋ค์ ์ ํ์๋ ๋ ๊ฐ๋ฅผ ์ข์ฐ 180๋ ์ํ์ผ๋ก ์ซ ํ์ต๋๋ค! (์ฌ์ง 2 ์ํ)")
|
| 106 |
+
|
| 107 |
+
def toggle_look_down(self):
|
| 108 |
+
"""๐ท ๋์ ํ๋ฐฉ ์์ ํ ๊ธ (V ํค) - ๋์ ์๋ 55ยฐ ๋ณด๊ธฐ <-> ๋์ ์๋ ์์น๋ก ๋ณต๊ท"""
|
| 109 |
+
self.look_down = not self.look_down
|
| 110 |
+
if self.look_down:
|
| 111 |
+
self.target_look_down_pitch = 0.95 # ์ฝ 55๋ ์๋๋ก ์์
|
| 112 |
+
print("\n[๐ ๋์ ์๋ ๋ณด๊ธฐ ON] ๋์์ด ์๋๋ก 55ยฐ ํฅํ์ฌ ๋ฐ๋ฅ์ ๋ด๋ ค๋ค๋ด
๋๋ค!")
|
| 113 |
+
else:
|
| 114 |
+
self.target_look_down_pitch = 0.0
|
| 115 |
+
print("\n[๐ ๋์ ์์์น ๋ณต๊ท] ๋์์ด ๋ค์ ์๋ ์์น(์ ๋ฉด)๋ก ๋์์์ต๋๋ค! โจ")
|
| 116 |
+
|
| 117 |
+
def look_front(self):
|
| 118 |
+
"""๐ ์ํฐ์น ๋์ ์์์น ๋ณต๊ท (B ํค)"""
|
| 119 |
+
self.look_down = False
|
| 120 |
+
self.target_look_down_pitch = 0.0
|
| 121 |
+
print("\n[๐ ๋์ ์์์น ๋ณต๊ท] ๋์์ด ๋ค์ ์๋ ์์น(์ ๋ฉด)๋ก ๋์์์ต๋๋ค! โจ")
|
| 122 |
+
|
| 123 |
+
def start_flight_sequence(self):
|
| 124 |
+
"""2๋จ๊ณ: ํ๋กํ ๋ฌ ํ์ โ 3๋จ๊ณ 1m ์์ง ์์น โ 4๋จ๊ณ 1m ์ ์ง ํธ๋ฒ๋ง (2๋ฒ / SPACEํค)"""
|
| 125 |
+
if not self.wings_deployed or self.wing_progress < 0.8:
|
| 126 |
+
print("\n[1๋จ๊ณ] ๋ฑ ๋ค์ ๋ ๊ฐ๋ฅผ ๋จผ์ ์ข์ฐ 180๋๋ก ์ซ ํ
๋๋ค...")
|
| 127 |
+
self.wings_deployed = True
|
| 128 |
+
self.wing_progress = 1.0 # ์ฆ์ ์ ๊ฐ ์๋ฃ
|
| 129 |
+
|
| 130 |
+
print("[2๋จ๊ณ] 3์ฝ ์ปฌ๋ฌ ํ๋กํ ๋ฌ๊ฐ ์ฉ์ฉ ๋๊ธฐ ์์ํฉ๋๋ค!")
|
| 131 |
+
self.propeller_running = True
|
| 132 |
+
self.target_altitude = 1.0
|
| 133 |
+
self.set_state(self.STATE_TAKEOFF_SPOOL)
|
| 134 |
+
|
| 135 |
+
def update(self, dt):
|
| 136 |
+
self.state_timer += dt
|
| 137 |
+
|
| 138 |
+
ctrl = np.zeros(self.model.nu)
|
| 139 |
+
pos = self.data.qpos[0:3]
|
| 140 |
+
vel = self.data.qvel[0:3]
|
| 141 |
+
z_height = pos[2]
|
| 142 |
+
|
| 143 |
+
# ==================== [ ๐ช๏ธ 3์ฝ ํ๋กํ ๋ฌ ์ฉ์ฉ ํ์ ] ====================
|
| 144 |
+
if self.propeller_running:
|
| 145 |
+
self.prop_speed = min(self.target_prop_speed, self.prop_speed + 80.0 * dt)
|
| 146 |
+
else:
|
| 147 |
+
self.prop_speed = max(0.0, self.prop_speed - 50.0 * dt)
|
| 148 |
+
|
| 149 |
+
# ๋ฌผ๋ฆฌ ์์ง DOF ์๋ ๋ฐ ๊ฐ๋ ๋๊ธฐํ
|
| 150 |
+
l_prop_dof = self.model.joint("left_prop_spin").dofadr[0]
|
| 151 |
+
r_prop_dof = self.model.joint("right_prop_spin").dofadr[0]
|
| 152 |
+
l_prop_qpos = self.model.joint("left_prop_spin").qposadr[0]
|
| 153 |
+
r_prop_qpos = self.model.joint("right_prop_spin").qposadr[0]
|
| 154 |
+
|
| 155 |
+
self.data.qvel[l_prop_dof] = self.prop_speed
|
| 156 |
+
self.data.qvel[r_prop_dof] = -self.prop_speed
|
| 157 |
+
|
| 158 |
+
if self.prop_speed > 0:
|
| 159 |
+
self.prop_angle += self.prop_speed * dt
|
| 160 |
+
self.data.qpos[l_prop_qpos] = self.prop_angle
|
| 161 |
+
self.data.qpos[r_prop_qpos] = -self.prop_angle
|
| 162 |
+
|
| 163 |
+
# ==================== [ ๐ชฝ ๋ ๊ฐ ์ ๊ฐ/์ ํ ๋ณด๊ฐ ์ ๋๋ฉ์ด์
] ====================
|
| 164 |
+
target_progress = 1.0 if self.wings_deployed else 0.0
|
| 165 |
+
wing_speed = 3.5 # ์ฝ 0.28์ด ๋ง์ ๋งค๋๋ฝ๊ฒ ์ ๊ฐ/์ ํ
|
| 166 |
+
if self.wing_progress < target_progress:
|
| 167 |
+
self.wing_progress = min(target_progress, self.wing_progress + wing_speed * dt)
|
| 168 |
+
elif self.wing_progress > target_progress:
|
| 169 |
+
self.wing_progress = max(target_progress, self.wing_progress - wing_speed * dt)
|
| 170 |
+
|
| 171 |
+
# ==================== [ ๐ท ํ๋ฐฉ ๊ด์ธก ๊ฐ๋ ๋ณด๊ฐ ๋ฐ ๋ ๊ฐ ์ญ๋ณด์ ] ====================
|
| 172 |
+
self.look_down_pitch += (self.target_look_down_pitch - self.look_down_pitch) * min(1.0, dt * 5.0)
|
| 173 |
+
|
| 174 |
+
# fold: 0.0 (๋ฑ ๋ค) โ ยฑ1.57 (์ข์ฐ 180ยฐ ์ ๊ฐ)
|
| 175 |
+
fold_l_cmd = 1.57 * self.wing_progress
|
| 176 |
+
fold_r_cmd = -1.57 * self.wing_progress
|
| 177 |
+
|
| 178 |
+
# tilt: 1.57 (๋ฑ ๋ค ์ธ๋ก ์ ํ) โ 0.0 (๋นํ ์ ์๋ฒฝํ ๊ฐ๋ก ์ํ ๋๊ธฐ!)
|
| 179 |
+
# ๐ ๋ ๊ฐ๊ฐ ์ ๊ฐ๋๋ฉด ํญ์ ์ฒ์ ๋นํํ ๋์ ๊ฐ๋ก ์ํ ๋ ๊ฐ(0.0 rad)๋ฅผ ์ ์งํ๊ณ , ๋ชธํต๋ง ์๋๋ก ์์ฌ์ง๋๋ค!
|
| 180 |
+
tilt_cmd = 1.57 * (1.0 - self.wing_progress)
|
| 181 |
+
|
| 182 |
+
ctrl[6] = fold_l_cmd
|
| 183 |
+
ctrl[7] = fold_r_cmd
|
| 184 |
+
ctrl[8] = tilt_cmd
|
| 185 |
+
ctrl[9] = tilt_cmd
|
| 186 |
+
# ๐ฆ [๋
๋ฆฝ ๋จธ๋ฆฌ/๋ชฉ ํผ์น] ๋ชธํต/๋ ๊ฐ๋ ์๋ฒฝ ์ํ ๊ณ ์ , ๋จธ๋ฆฌ(๋ ๋+๋ถ๋ฆฌ)๊ฐ ์๋๋ก 55ยฐ ์ ์์ฌ์ง!
|
| 187 |
+
ctrl[10] = -self.look_down_pitch
|
| 188 |
+
|
| 189 |
+
torso_id = self.model.body("torso").id
|
| 190 |
+
fz = 0.0
|
| 191 |
+
fx = 0.0
|
| 192 |
+
fy = 0.0
|
| 193 |
+
tau_x = 0.0
|
| 194 |
+
tau_y = 0.0
|
| 195 |
+
tau_z = 0.0
|
| 196 |
+
|
| 197 |
+
# =========================================================================
|
| 198 |
+
# ๐ฆ [ ์ง์ ๋ชจ๋: ๋ณดํ ๋ฐ ์ง๋ฆฝ ] - ๋นํ ๋ฌผ๋ฆฌ์ 100% ๋ถ๋ฆฌ!
|
| 199 |
+
# =========================================================================
|
| 200 |
+
is_ground_mode = self.state in [
|
| 201 |
+
self.STATE_STAND, self.STATE_WALK, self.STATE_GROUND_PICK,
|
| 202 |
+
self.STATE_WING_SHIVER, self.STATE_SIT, self.STATE_RECOVER, self.STATE_KNOCKDOWN
|
| 203 |
+
]
|
| 204 |
+
|
| 205 |
+
if is_ground_mode:
|
| 206 |
+
# ์ฟผํฐ๋์ธ ๊ธฐ๋ฐ ์์ฒด ๊ธฐ์ธ๊ธฐ ์ธก์ (q = [w, x, y, z] -> x์ถ: Pitch, y์ถ: Roll)
|
| 207 |
+
q = self.data.qpos[3:7]
|
| 208 |
+
q_sign = np.sign(q[0]) if q[0] != 0 else 1.0
|
| 209 |
+
pitch_tilt = -q[1] * q_sign # X์ถ ํ์ = ์๋ค ๊ธฐ์ธ๊ธฐ (Pitch)
|
| 210 |
+
roll_tilt = -q[2] * q_sign # Y์ถ ํ์ = ์ข์ฐ ๊ธฐ์ธ๊ธฐ (Roll)
|
| 211 |
+
|
| 212 |
+
# 1. ๐ฆ ์ ์๋ฆฌ ์ง๋ฆฝ ๋ชจ๋ (STAND) - ์ธ๋ ์ ํญ (๋ฐ๋ฆฌ์ง ์๊ณ ๋ฒํฐ๊ธฐ / Push Recovery)
|
| 213 |
+
if self.state == self.STATE_STAND:
|
| 214 |
+
# ์ ์๋ ๋ฐ ๊ฐ์๋ ํผ๋๋ฐฑ์ ๊ฒฐํฉํ ์ง๋ฅํ ๋ฒํฐ๊ธฐ (Push Resistance)
|
| 215 |
+
lin_vel = self.data.cvel[torso_id, 3:6]
|
| 216 |
+
vy = lin_vel[1]
|
| 217 |
+
vx = lin_vel[0]
|
| 218 |
+
|
| 219 |
+
bal_pitch = np.clip(pitch_tilt * 1.2 - vy * 0.18, -0.35, 0.35)
|
| 220 |
+
bal_roll = np.clip(roll_tilt * 0.8 - vx * 0.12, -0.20, 0.20)
|
| 221 |
+
|
| 222 |
+
# ํธํก ๋ชจ์
|
| 223 |
+
breath = 0.012 * math.sin(self.state_timer * 3.0)
|
| 224 |
+
|
| 225 |
+
# ์ข์ธก ๋ค๋ฆฌ (์ธ๋์ ๋ง์ ๋ฐ๋ชฉ๊ณผ ๊ณ ๊ด์ ๋ก ๋ฐ๋ฅ์ ๊ฝ ์งํฑ)
|
| 226 |
+
ctrl[0] = 0.0 + bal_pitch + bal_roll
|
| 227 |
+
ctrl[1] = 0.0 + breath
|
| 228 |
+
ctrl[2] = 0.0 - bal_pitch * 1.2
|
| 229 |
+
|
| 230 |
+
# ์ฐ์ธก ๋ค๋ฆฌ
|
| 231 |
+
ctrl[3] = 0.0 + bal_pitch - bal_roll
|
| 232 |
+
ctrl[4] = 0.0 + breath
|
| 233 |
+
ctrl[5] = 0.0 - bal_pitch * 1.2
|
| 234 |
+
|
| 235 |
+
# 2. ๐ถ 2์กฑ ๋ณดํ ๋ชจ๋ (WALK) - ์ง์ง ์์ผ๋ก/๋ค๋ก ๊ฑท๊ณ ์์ํ๊ฒ ํ์ ํ๋ ์กฐ๋ฅ 2์กฑ๋ณดํ
|
| 236 |
+
elif self.state == self.STATE_WALK:
|
| 237 |
+
# ๊ฑธ์ ์ฃผ๊ธฐ ์์ ์ง์ฒ
|
| 238 |
+
self.walk_phase += dt * 6.5 * max(0.6, abs(self.walk_speed))
|
| 239 |
+
|
| 240 |
+
phase_sin = math.sin(self.walk_phase)
|
| 241 |
+
phase_cos = math.cos(self.walk_phase)
|
| 242 |
+
dir_sign = 1.0 if self.walk_speed >= 0 else -1.0
|
| 243 |
+
turn = getattr(self, 'walk_turn', 0.0)
|
| 244 |
+
|
| 245 |
+
# ๐ [ํ์ง ๋ณดํ 100% ์์ ํ] ํ์ง ์ ์์ฒด๋ฅผ ์์ชฝ์ผ๋ก 8ยฐ ํ์คํ๊ฒ ์์ฌ ๋ฌด๊ฒ์ค์ฌ์ ์์ ํ (๋์ด์ง ์์ฒ ์ฐจ๋จ!)
|
| 246 |
+
lean_bias = 0.14 if self.walk_speed < 0 else 0.0
|
| 247 |
+
bal_pitch = np.clip((pitch_tilt + lean_bias) * 1.0, -0.30, 0.30)
|
| 248 |
+
bal_roll = np.clip(roll_tilt * 0.6, -0.15, 0.15)
|
| 249 |
+
|
| 250 |
+
# ์ ์ง ๋ณดํญ 0.25, ํ์ง ๋ณดํญ 0.18 (์ด์ดํ๊ณ ๋น๋นํ๊ฒ ๋ค๋ก ๊ฑท๊ธฐ)
|
| 251 |
+
base_stride = 0.25 if self.walk_speed >= 0 else 0.18
|
| 252 |
+
|
| 253 |
+
# ๐ ์ข/์ฐ ํ์ ์ ์ ๋ฐ์ ์ฐจ๋ ๋ณดํญ (Differential Stride) ์ ์ฉ โ ํฝ๊ทธ๋ฅด๋ฅด ์ฆ๊ฐ ํ์ !
|
| 254 |
+
l_stride = base_stride * (1.0 - turn * 0.35) * dir_sign
|
| 255 |
+
r_stride = base_stride * (1.0 + turn * 0.35) * dir_sign
|
| 256 |
+
|
| 257 |
+
# ๐ฆถ ์ข์ธก ๋ค๋ฆฌ (phase_sin > 0: ์ ๊ฐ๊ธฐ ๋ฐ ๋ค๋ฆผ / phase_sin <= 0: ์ง์ง๊ธฐ ๋ฐ๋ฅ ๋ฐ๊ธฐ)
|
| 258 |
+
if phase_sin > 0:
|
| 259 |
+
l_hip = l_stride * (-phase_cos) + bal_pitch + bal_roll
|
| 260 |
+
l_knee = 0.35 * phase_sin # ๋ฌด๋ฆ์ ๊ตฝํ ๋ฐ์ ๋ฐ๋ฅ์์ 1.5cm ๋์ด ๋์!
|
| 261 |
+
l_ankle = -(l_hip + l_knee) # ๋ฐ๋ฐ๋ฅ์ด ํญ์ ์ง๋ฉด๊ณผ ์ํ ์ ์ง
|
| 262 |
+
else:
|
| 263 |
+
l_hip = l_stride * (-phase_cos) + bal_pitch + bal_roll
|
| 264 |
+
l_knee = 0.0 # ์ง์ง ๋ค๋ฆฌ ๋ฌด๋ฆ ํด์ ์ง์ง
|
| 265 |
+
l_ankle = -l_hip # ์ง๋ฉด ๋ฐ์ฐฉ ์ง์ง
|
| 266 |
+
|
| 267 |
+
# ๐ฆถ ์ฐ์ธก ๋ค๋ฆฌ (phase_sin <= 0: ์ ๊ฐ๊ธฐ ๋ฐ ๋ค๋ฆผ / phase_sin > 0: ์ง์ง๊ธฐ ๋ฐ๋ฅ ๋ฐ๊ธฐ)
|
| 268 |
+
if phase_sin <= 0:
|
| 269 |
+
r_hip = r_stride * phase_cos + bal_pitch - bal_roll
|
| 270 |
+
r_knee = 0.35 * (-phase_sin) # ๋ฌด๋ฆ ๊ตฝํ ๋ฐ ๋์
|
| 271 |
+
r_ankle = -(r_hip + r_knee) # ๋ฐ๋ฐ๋ฅ ์ํ ์ ์ง
|
| 272 |
+
else:
|
| 273 |
+
r_hip = r_stride * phase_cos + bal_pitch - bal_roll
|
| 274 |
+
r_knee = 0.0 # ์ง์ง ๋ค๋ฆฌ ๋ฌด๋ฆ ํด์ ์ง์ง
|
| 275 |
+
r_ankle = -r_hip # ์ง๋ฉด ๋ฐ์ฐฉ ์ง์ง
|
| 276 |
+
|
| 277 |
+
ctrl[0] = l_hip
|
| 278 |
+
ctrl[1] = l_knee
|
| 279 |
+
ctrl[2] = l_ankle
|
| 280 |
+
|
| 281 |
+
ctrl[3] = r_hip
|
| 282 |
+
ctrl[4] = r_knee
|
| 283 |
+
ctrl[5] = r_ankle
|
| 284 |
+
|
| 285 |
+
# ์ง์ง๋ฐ์ ์ง๋ฉด ๋ฐ๋ ฅ ์ถ์ง๋ ฅ (ํ์ง ์์๋ ์์ ๋ ํ ์ธ๊ฐ)
|
| 286 |
+
R = self.data.xmat[torso_id].reshape(3, 3)
|
| 287 |
+
y_forward = R[:, 1]
|
| 288 |
+
|
| 289 |
+
push_mag = 1.1 if self.walk_speed >= 0 else 0.85
|
| 290 |
+
push_force = y_forward * (push_mag * self.walk_speed)
|
| 291 |
+
fx = push_force[0]
|
| 292 |
+
fy = push_force[1]
|
| 293 |
+
|
| 294 |
+
# ๐ ํ์ ๋ณดํ ์ ํค๋ฉ ๊ฐฑ์ ๋ฐ ๊ฐ๋ ฅํ Yaw ํ์ ํ ํฌ (์ข/์ฐ ์์ ๋์นญ ํ์ )
|
| 295 |
+
omega_ground = self.data.cvel[torso_id, 0:3]
|
| 296 |
+
if abs(turn) > 0.05:
|
| 297 |
+
self.target_yaw -= turn * 1.8 * dt
|
| 298 |
+
tau_z = np.clip(-turn * 0.40 - omega_ground[2] * 0.10, -0.32, 0.32)
|
| 299 |
+
else:
|
| 300 |
+
target_yaw = getattr(self, 'target_yaw', 0.0)
|
| 301 |
+
current_yaw = math.atan2(-y_forward[0], y_forward[1])
|
| 302 |
+
yaw_err = (target_yaw - current_yaw + math.pi) % (2 * math.pi) - math.pi
|
| 303 |
+
tau_z = np.clip(yaw_err * 0.50 - omega_ground[2] * 0.12, -0.20, 0.20)
|
| 304 |
+
|
| 305 |
+
# 3. ๐พ ๊ฐ์ฑ ๋ชจ์
๋ค (์กฐ๋ฅ ์ญ๊ด์ ํนํ)
|
| 306 |
+
elif self.state == self.STATE_GROUND_PICK:
|
| 307 |
+
t = self.state_timer
|
| 308 |
+
# ๐ [๋นํ์ ๋ค๋ฆฌ ๊น์ ์ ํ โ ๋ถ๋ฆฌ ๋ฐ๋ฅ ํฐ์น 3๋จ ๋ชจ์ด ์ชผ๊ธฐ]
|
| 309 |
+
# 1๋จ๊ณ (0~0.42s): ๋ค๋ฆฌ๋ฅผ ๋ชธ ๋ฐ์ผ๋ก ๊น๊ฒ ์
ํฌ๋ ค ์ ์ผ๋ฉฐ ๋ถ๋ฆฌ๊ฐ ๋ฐ๋ฅ ์ง๋ฉด์ ์ฝ! ๋ฟ์
|
| 310 |
+
# 2๋จ๊ณ (0.42~0.85s): ๋ฐ๋ฅ์์ ์ฝ! ์ฝ! ์ชผ๊ธฐ
|
| 311 |
+
# 3๋จ๊ณ (0.85~1.30s): ๋ถ๋๋ฝ๊ฒ ์๋ ์ ์๋ ๊ธฐ๋ฆฝ ์์ธ๋ก ์ฅ- ๋ณต๊ท!
|
| 312 |
+
if t < 0.42:
|
| 313 |
+
p = 0.5 * (1.0 - math.cos(math.pi * t / 0.42)) # 0 โ 1 ๊น์ ํ๊ฐ
|
| 314 |
+
hip = 0.65 * p
|
| 315 |
+
knee = 0.95 * p
|
| 316 |
+
ankle = -0.48 * p
|
| 317 |
+
elif t < 0.85:
|
| 318 |
+
mini = 0.14 * math.sin((t - 0.42) * 18.0) # ๋ฐ๋ฅ ์ฝ! ์ฝ!
|
| 319 |
+
hip = 0.65 + mini
|
| 320 |
+
knee = 0.95
|
| 321 |
+
ankle = -0.48
|
| 322 |
+
elif t < 1.30:
|
| 323 |
+
p = 0.5 * (1.0 + math.cos(math.pi * (t - 0.85) / 0.45)) # 1 โ 0 ๊ธฐ๋ฆฝ ๋ณต๊ท
|
| 324 |
+
hip = 0.65 * p
|
| 325 |
+
knee = 0.95 * p
|
| 326 |
+
ankle = -0.48 * p
|
| 327 |
+
else:
|
| 328 |
+
hip = 0.0; knee = 0.0; ankle = 0.0
|
| 329 |
+
|
| 330 |
+
ctrl[0] = hip; ctrl[1] = knee; ctrl[2] = ankle
|
| 331 |
+
ctrl[3] = hip; ctrl[4] = knee; ctrl[5] = ankle
|
| 332 |
+
|
| 333 |
+
# ๐ ํ์ ์ต์ ๋ฐ ๋ฐ๋ 0% ๋ํ
|
| 334 |
+
omega_ground = self.data.cvel[torso_id, 0:3]
|
| 335 |
+
tau_z = np.clip(-omega_ground[2] * 0.30, -0.15, 0.15)
|
| 336 |
+
tau_x = 0.0; tau_y = 0.0
|
| 337 |
+
|
| 338 |
+
if t >= 1.30:
|
| 339 |
+
self.set_state(self.STATE_STAND)
|
| 340 |
+
|
| 341 |
+
elif self.state == self.STATE_KNOCKDOWN:
|
| 342 |
+
# ๐ฅ ๊ฝ๋น ๋์์๋ ์๋ ํ
์คํธ ์ํ: ํ์ ๋นผ๊ณ ํธ์ํ๊ฒ ์ง๋ฉด์ ๋์์์ (์ผ์ด๋๋ ค๋ฉด 'P' ํค ๋๋ฆ)
|
| 343 |
+
ctrl[0] = 0.0; ctrl[1] = 0.20; ctrl[2] = 0.0
|
| 344 |
+
ctrl[3] = 0.0; ctrl[4] = 0.20; ctrl[5] = 0.0
|
| 345 |
+
tau_x = 0.0; tau_y = 0.0; tau_z = 0.0
|
| 346 |
+
|
| 347 |
+
elif self.state == self.STATE_WING_SHIVER:
|
| 348 |
+
t = self.state_timer
|
| 349 |
+
if t < 1.5:
|
| 350 |
+
shiver = 0.25 * math.sin(t * 28.0)
|
| 351 |
+
ctrl[0] = 0.0; ctrl[1] = 0.0; ctrl[2] = 0.0
|
| 352 |
+
ctrl[3] = 0.0; ctrl[4] = 0.0; ctrl[5] = 0.0
|
| 353 |
+
ctrl[6] = max(0, shiver); ctrl[7] = -max(0, shiver)
|
| 354 |
+
else:
|
| 355 |
+
self.set_state(self.STATE_STAND)
|
| 356 |
+
|
| 357 |
+
elif self.state == self.STATE_SIT:
|
| 358 |
+
# ๐ฅ ์๋ฒฝํ๊ฒ ๋ฌด๋ฆ์ ๊ตฝํ๊ณ ๋ค๋ฆฌ๋ฅผ ์
ํฌ๋ ค ์ชผ๊ทธ๋ ค ์๊ธฐ
|
| 359 |
+
ctrl[0] = -0.40; ctrl[1] = 0.90; ctrl[2] = -0.50
|
| 360 |
+
ctrl[3] = -0.40; ctrl[4] = 0.90; ctrl[5] = -0.50
|
| 361 |
+
|
| 362 |
+
elif self.state == self.STATE_RECOVER:
|
| 363 |
+
t = self.state_timer
|
| 364 |
+
# ๐ [100% ๋ฒ๋ก ์ผ์ด๋๋ 3D ์ค๋์ด ๊ธฐ๋ฆฝ]
|
| 365 |
+
# ์ด๋ค ๊ฐ๋๋ก ๋์ด์ ธ ์์ด๋ ๋ฐ๋ฅ ๋ง์ฐฐ์ ํธ์ด๋ด๊ณ ์์ง์ผ๋ก ์ฐ๋ ์ผ์ด์ฌ!
|
| 366 |
+
R = self.data.xmat[torso_id].reshape(3, 3)
|
| 367 |
+
z_body = R[:, 2]
|
| 368 |
+
z_world = np.array([0.0, 0.0, 1.0])
|
| 369 |
+
tau_tilt = np.cross(z_body, z_world) # 3์ฐจ์ ์์ง ์ ๋ ฌ ํ ํฌ
|
| 370 |
+
omega_ground = self.data.cvel[torso_id, 0:3]
|
| 371 |
+
|
| 372 |
+
ctrl[0] = 0.0; ctrl[1] = 0.0; ctrl[2] = 0.0
|
| 373 |
+
ctrl[3] = 0.0; ctrl[4] = 0.0; ctrl[5] = 0.0
|
| 374 |
+
|
| 375 |
+
# 0~0.25์ด: ๋ฐ๋ฅ ๋ง์ฐฐ ํด์ ๋ฅผ ์ํ ์์ง ํ์
๋ฆฌํํธ ์ธ๊ฐ
|
| 376 |
+
fz = 3.2 if t < 0.25 else 0.0
|
| 377 |
+
|
| 378 |
+
# ๊ฐ๋ ฅํ ์์ง ์ง๋ฆฝ ํ ํฌ ๋ฐ ์คํ ์ต์
|
| 379 |
+
tau_x = np.clip(tau_tilt[0] * 5.0 - omega_ground[0] * 0.30, -0.60, 0.60)
|
| 380 |
+
tau_y = np.clip(tau_tilt[1] * 5.0 - omega_ground[1] * 0.30, -0.60, 0.60)
|
| 381 |
+
tau_z = np.clip(-omega_ground[2] * 0.35, -0.20, 0.20)
|
| 382 |
+
|
| 383 |
+
# ์์ง ์ ๋ ฌ์ด ์๋ฃ๋๋ฉด STAND๋ก ๋ณต๊ท
|
| 384 |
+
z_err = 1.0 - z_body[2]
|
| 385 |
+
if t > 0.40 and z_err < 0.03:
|
| 386 |
+
self.set_state(self.STATE_STAND)
|
| 387 |
+
elif t > 1.20:
|
| 388 |
+
self.set_state(self.STATE_STAND)
|
| 389 |
+
|
| 390 |
+
# ๐จ [์ง๋ฅํ ์๋ ๋์ด์ง ๊ฐ์ง ๋ฐ ์ฆ์ ์ค๋์ด ๊ธฐ๋ฆฝ (Auto Knockdown Recovery)]
|
| 391 |
+
R_body = self.data.xmat[torso_id].reshape(3, 3)
|
| 392 |
+
z_up = R_body[2, 2] # 1.0 = ๋๋ฐ๋ก ์ง๋ฆฝ, 0.0 = 90๋ ์ฐ๋ฌ์ง
|
| 393 |
+
torso_z = self.data.qpos[2]
|
| 394 |
+
|
| 395 |
+
# ์ ์ ์ง๋ฆฝ์ด ์๋ ๋ (๊ธฐ์ธ์ด์ง > 50ยฐ ๋๋ ๋์ด < 6.5cm)
|
| 396 |
+
if self.state not in [self.STATE_RECOVER, self.STATE_KNOCKDOWN, self.STATE_GROUND_PICK, self.STATE_SIT]:
|
| 397 |
+
if z_up < 0.60 or torso_z < 0.065:
|
| 398 |
+
self.fall_detect_timer += dt
|
| 399 |
+
if self.fall_detect_timer >= 0.20:
|
| 400 |
+
print(f"\n[์ค๋์ด ๊ฐ์ง] ๋ก๋ด์ ๋์ด์ง ๊ฐ์ง! (๊ธฐ์ธ๊ธฐ z_up={z_up:.2f}, ๋์ด={torso_z*100:.1f}cm) -> ์๋์ผ๋ก ๋ฒ๋ก ์ผ์ด๋ฉ๋๋ค!")
|
| 401 |
+
self.fall_detect_timer = 0.0
|
| 402 |
+
self.set_state(self.STATE_RECOVER)
|
| 403 |
+
else:
|
| 404 |
+
self.fall_detect_timer = max(0.0, self.fall_detect_timer - dt * 2.0)
|
| 405 |
+
|
| 406 |
+
# ์ง์ ๋ชจ๋ ์์ฒด ์์ธ ์ ์ง ๋ํ (๋์ด์ง/๊ธฐ๋ฆฝ ์ํ ์ธ ์ผ๋ฐ ๋ณดํ/๊ธฐ๋ฆฝ ์)
|
| 407 |
+
if self.state not in [self.STATE_RECOVER, self.STATE_KNOCKDOWN, self.STATE_GROUND_PICK]:
|
| 408 |
+
omega_ground = self.data.cvel[torso_id, 0:3]
|
| 409 |
+
lean_bias = -0.05 if (self.state == self.STATE_WALK and self.walk_speed < 0) else 0.0
|
| 410 |
+
tau_x = np.clip((pitch_tilt + lean_bias) * 3.5 - omega_ground[0] * 0.22, -0.35, 0.35)
|
| 411 |
+
tau_y = np.clip(roll_tilt * 2.5 - omega_ground[1] * 0.15, -0.25, 0.25)
|
| 412 |
+
# ๐ ๋ณดํ ์ค์ด ์๋ ๋๋ง ์ ์๋ฆฌ ํ์ ์ต์ , ๋ณดํ ์ค(WALK)์ผ ๋๋ ํ์ ํ ํฌ ๋ณด์กด!
|
| 413 |
+
if self.state != self.STATE_WALK:
|
| 414 |
+
tau_z = np.clip(-omega_ground[2] * 0.25, -0.15, 0.15)
|
| 415 |
+
|
| 416 |
+
|
| 417 |
+
|
| 418 |
+
# =========================================================================
|
| 419 |
+
# ๐ [ ๋นํ ๋ชจ๋: ์ด๋ฅ, ํธ๋ฒ๋ง, ์ฐฉ๋ฅ ] - 2๋ฒ ํค ๋๋ฅผ ๋๋ง ์๋!
|
| 420 |
+
# =========================================================================
|
| 421 |
+
else:
|
| 422 |
+
# ์๋ ํ์ ํ๋ ฌ ๋ฐ ์๋ ๊ฐ์๋ (cvel: world-frame angular velocity)
|
| 423 |
+
R = self.data.xmat[torso_id].reshape(3, 3)
|
| 424 |
+
omega_world = self.data.cvel[torso_id, 0:3]
|
| 425 |
+
y_body = R[:, 1]
|
| 426 |
+
z_body = R[:, 2]
|
| 427 |
+
|
| 428 |
+
# 1. ๐ ์ํ ์ ์ง (Roll / Pitch Leveling) - ์๋ Z์ถ ์๋ฒฝ ์์ง ์ ๋ ฌ (ํ๋กํ ๋ฌ์ ๋ชธํต์ ํญ์ ๊ฐ๋ก ์ํ ์ ์ง!)
|
| 429 |
+
z_world = np.array([0.0, 0.0, 1.0])
|
| 430 |
+
tau_tilt = np.cross(z_body, z_world) # [tilt_x, tilt_y, 0] (์์ ์ํ ๋ณต์๋ ฅ)
|
| 431 |
+
|
| 432 |
+
# 2. ๐ ๋ชฉํ ํค๋ฉ ์ถ์ข
(Yaw Heading) - ์ต๋จ ๊ฐ๋ ์ค์ฐจ ๊ณ์ฐ (-pi ~ +pi)
|
| 433 |
+
current_yaw = math.atan2(-y_body[0], y_body[1])
|
| 434 |
+
yaw_err = (self.target_yaw - current_yaw + math.pi) % (2 * math.pi) - math.pi
|
| 435 |
+
|
| 436 |
+
# 3. ๐ 3์ถ ์ง๊ต ๋
๋ฆฝ ์์ ํ ์ ์ด (Cross-coupling 100% ์์ฒ ์ฐจ๋จ!)
|
| 437 |
+
# Roll / Pitch: ๊ฐํ ๋ณต์๋ ฅ๊ณผ ๋ํ์ผ๋ก ๊ธฐ์ฐ๋ฑ ๋ฐฉ์ง
|
| 438 |
+
tau_x = np.clip(tau_tilt[0] * 3.5 - omega_world[0] * 0.35, -0.4, 0.4)
|
| 439 |
+
tau_y = np.clip(tau_tilt[1] * 3.5 - omega_world[1] * 0.35, -0.4, 0.4)
|
| 440 |
+
# Yaw: ๋ถ๋๋ฝ๊ณ ์ ํํ ํค๋ฉ ์ถ์ข
๋ฐ ๋ํ
|
| 441 |
+
tau_z = np.clip(yaw_err * 0.50 - omega_world[2] * 0.12, -0.20, 0.20)
|
| 442 |
+
|
| 443 |
+
# ๐ [๋นํ ๋ชจ๋ ๋ค๋ฆฌ ์ ํ]
|
| 444 |
+
# ๋นํ๊ธฐ ๋๋ฉ๊ธฐ์ด ๋ฐ ์์ ๋นํ ์์ธ์ฒ๋ผ ๋ค๋ฆฌ๋ ๋ชธํต ์๋๋ก ์ ์ ์ด ์ฌ๋ฆฌ๊ณ ,
|
| 445 |
+
# ๋ฐ๋ฐ๋ฅ์ ์๋ฒฝํ๊ฒ ๊ฐ๋ก ์ํ(์ง๋ฉด ํํ)์ ์ ์งํ์ฌ ๋ชธ์ ๋จ์ ํ๊ฒ ์ฐฉ ๋ถ์
๋๋ค!
|
| 446 |
+
if self.state in [self.STATE_TAKEOFF_CLIMB, self.STATE_FLIGHT]:
|
| 447 |
+
ctrl[0] = -0.38; ctrl[1] = 0.80; ctrl[2] = -0.42
|
| 448 |
+
ctrl[3] = -0.38; ctrl[4] = 0.80; ctrl[5] = -0.42
|
| 449 |
+
elif self.state == self.STATE_LANDING:
|
| 450 |
+
# ์ฐฉ๋ฅ ์ ์ง์ 25cm ์ดํ๋ก ๋ด๋ ค์ค๋ฉด ๋ค๋ฆฌ๋ฅผ ๋ถ๋๋ฝ๊ฒ ํด์ ์ฐฉ์ง ์ค๋น!
|
| 451 |
+
if z_height > 0.25:
|
| 452 |
+
ctrl[0] = -0.38; ctrl[1] = 0.80; ctrl[2] = -0.42
|
| 453 |
+
ctrl[3] = -0.38; ctrl[4] = 0.80; ctrl[5] = -0.42
|
| 454 |
+
else:
|
| 455 |
+
ctrl[0] = 0.0; ctrl[1] = 0.0; ctrl[2] = 0.0
|
| 456 |
+
ctrl[3] = 0.0; ctrl[4] = 0.0; ctrl[5] = 0.0
|
| 457 |
+
else:
|
| 458 |
+
ctrl[0] = 0.0; ctrl[1] = 0.0; ctrl[2] = 0.0
|
| 459 |
+
ctrl[3] = 0.0; ctrl[4] = 0.0; ctrl[5] = 0.0
|
| 460 |
+
|
| 461 |
+
# 1. ์ด๋ฅ ์ค๋น (ํ๋กํ ๋ฌ ๊ฐ์)
|
| 462 |
+
if self.state == self.STATE_TAKEOFF_SPOOL:
|
| 463 |
+
# ํ์ฌ ์์น ์ต์ปค ์บก์ฒ
|
| 464 |
+
self.target_x = pos[0]
|
| 465 |
+
self.target_y = pos[1]
|
| 466 |
+
|
| 467 |
+
# ํ์ฌ yaw ๊ฐ๋ ์บก์ฒ
|
| 468 |
+
siny_cosp = 2 * (self.data.qpos[3] * self.data.qpos[6] + self.data.qpos[4] * self.data.qpos[5])
|
| 469 |
+
cosy_cosp = 1 - 2 * (self.data.qpos[5] * self.data.qpos[5] + self.data.qpos[6] * self.data.qpos[6])
|
| 470 |
+
self.target_yaw = math.atan2(siny_cosp, cosy_cosp)
|
| 471 |
+
|
| 472 |
+
if self.state_timer >= 0.35:
|
| 473 |
+
print(f"[3๋จ๊ณ] 1.0m ๋์ด๋ก ๋๋ฐ๋ก ์์ง ์์นํฉ๋๋ค...")
|
| 474 |
+
self.set_state(self.STATE_TAKEOFF_CLIMB)
|
| 475 |
+
|
| 476 |
+
# 2. 1.0m ์์ง ์์น
|
| 477 |
+
elif self.state == self.STATE_TAKEOFF_CLIMB:
|
| 478 |
+
# ์์ง ์์น ์ถ๋ ฅ
|
| 479 |
+
err_z = self.target_altitude - z_height
|
| 480 |
+
fz = self.gravity_force + np.clip(err_z * 8.0 - vel[2] * 3.5, -self.gravity_force * 0.4, self.gravity_force * 1.6)
|
| 481 |
+
|
| 482 |
+
# ์ํ ์์น ๊ณ ์
|
| 483 |
+
err_x = self.target_x - pos[0]
|
| 484 |
+
err_y = self.target_y - pos[1]
|
| 485 |
+
fx = np.clip(err_x * 4.0 - vel[0] * 3.0, -1.0, 1.0)
|
| 486 |
+
fy = np.clip(err_y * 4.0 - vel[1] * 3.0, -1.0, 1.0)
|
| 487 |
+
|
| 488 |
+
if z_height >= self.target_altitude - 0.05 or self.state_timer > 1.8:
|
| 489 |
+
print(f"[4๋จ๊ณ ์๋ฃ] 1.0m ๋์ด์ ๋๋ฌํ์ฌ ์๋ฒฝํ ์ ์ง ํธ๋ฒ๋งํฉ๋๋ค!")
|
| 490 |
+
self.set_state(self.STATE_FLIGHT)
|
| 491 |
+
|
| 492 |
+
# 3. 1.0m ์๋ฒฝ ์ ์ง ํธ๋ฒ๋ง ๋ฐ ์์ ๋นํ (์์์ ๋์ ๋จ์ ํ ๋ฐ ๋ชจ์ ์ ์ง)
|
| 493 |
+
elif self.state == self.STATE_FLIGHT:
|
| 494 |
+
# ๊ณ ๋ ์ ์ง (Z Altitude Hold)
|
| 495 |
+
err_z = self.target_altitude - z_height
|
| 496 |
+
fz = self.gravity_force + np.clip(err_z * 8.5 - vel[2] * 3.5, -self.gravity_force * 0.5, self.gravity_force * 1.5)
|
| 497 |
+
|
| 498 |
+
# ์ํ ์์น ์ด๋ ๋ฐ ์ถ์ข
(X, Y Position Flight Control) - ์์ํ๊ณ ๋ฏผ์ฒฉํ ๋นํ!
|
| 499 |
+
err_x = self.target_x - pos[0]
|
| 500 |
+
err_y = self.target_y - pos[1]
|
| 501 |
+
fx = np.clip(err_x * 8.0 - vel[0] * 3.5, -2.5, 2.5)
|
| 502 |
+
fy = np.clip(err_y * 8.0 - vel[1] * 3.5, -2.5, 2.5)
|
| 503 |
+
|
| 504 |
+
# 4. ์์ง ์ฐฉ๋ฅ
|
| 505 |
+
elif self.state == self.STATE_LANDING:
|
| 506 |
+
if z_height > 0.15:
|
| 507 |
+
fz = self.gravity_force * 0.70
|
| 508 |
+
err_x = self.target_x - pos[0]
|
| 509 |
+
err_y = self.target_y - pos[1]
|
| 510 |
+
fx = np.clip(err_x * 4.0 - vel[0] * 2.5, -1.0, 1.0)
|
| 511 |
+
fy = np.clip(err_y * 4.0 - vel[1] * 2.5, -1.0, 1.0)
|
| 512 |
+
else:
|
| 513 |
+
print("[์ฐฉ๋ฅ ์๋ฃ] ์ง๋ฉด์ ์์ ํ๊ฒ ์ฐฉ์งํ์ต๋๋ค. ํ๋กํ ๋ฌ๋ฅผ ๋๊ณ ๋ ๊ฐ๋ฅผ ๋ฑ ๋ค๋ก ์ ์ต๋๋ค.")
|
| 514 |
+
fz = 0.0
|
| 515 |
+
self.propeller_running = False
|
| 516 |
+
self.wings_deployed = False
|
| 517 |
+
self.set_state(self.STATE_STAND)
|
| 518 |
+
|
| 519 |
+
self.data.ctrl[:] = ctrl
|
| 520 |
+
|
| 521 |
+
# ๐ฅ ์ธ๋ ๋ฐ๊ธฐ ํ ์ธ๊ฐ (ํ
์คํธ ํค X, Z ๋๋ฅผ ๋)
|
| 522 |
+
if self.push_timer > 0.0:
|
| 523 |
+
fx += self.push_force[0]
|
| 524 |
+
fy += self.push_force[1]
|
| 525 |
+
self.push_timer -= dt
|
| 526 |
+
|
| 527 |
+
# ์ธ๋ ฅ ๋ฐ ํ ํฌ ์ธ๊ฐ (์ง์/๋นํ ์์ ํด๋จํ)
|
| 528 |
+
self.data.xfrc_applied[:] = 0.0
|
| 529 |
+
if fz > 0.0 or abs(fy) > 0.0 or abs(fx) > 0.0 or abs(tau_x) > 0.0 or abs(tau_y) > 0.0 or abs(tau_z) > 0.0:
|
| 530 |
+
self.data.xfrc_applied[torso_id, :3] = np.array([fx, fy, fz])
|
| 531 |
+
self.data.xfrc_applied[torso_id, 3:6] = np.array([tau_x, tau_y, tau_z])
|
| 532 |
+
|
| 533 |
+
|
| 534 |
+
|
robot_bird/models/robot_bird.xml
ADDED
|
@@ -0,0 +1,248 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
<mujoco model="bipedal_vtol_robot_bird">
|
| 2 |
+
<compiler angle="radian" coordinate="local" inertiafromgeom="true"/>
|
| 3 |
+
<option gravity="0 0 -9.81" timestep="0.002" integrator="RK4"/>
|
| 4 |
+
|
| 5 |
+
<default>
|
| 6 |
+
<joint damping="0.15" armature="0.02"/>
|
| 7 |
+
<geom friction="2.0 0.1 0.1" density="50" solref="0.01 1" solimp="0.90 0.95 0.001"/>
|
| 8 |
+
<position kp="350" kv="20" forcerange="-35 35"/>
|
| 9 |
+
</default>
|
| 10 |
+
|
| 11 |
+
<visual>
|
| 12 |
+
<headlight diffuse="0.65 0.65 0.65" ambient="0.45 0.45 0.45" specular="0.0 0.0 0.0"/>
|
| 13 |
+
<rgba haze="0.15 0.25 0.35 1"/>
|
| 14 |
+
</visual>
|
| 15 |
+
|
| 16 |
+
<asset>
|
| 17 |
+
<texture type="skybox" builtin="gradient" rgb1="0.5 0.7 0.9" rgb2="0.9 0.95 1.0" width="512" height="512"/>
|
| 18 |
+
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1="0.90 0.92 0.96" rgb2="0.55 0.62 0.72"/>
|
| 19 |
+
<material name="grid_mat" texture="grid" texrepeat="8 8" reflectance="0.05"/>
|
| 20 |
+
|
| 21 |
+
<material name="body_white" rgba="0.96 0.96 0.98 1.0" specular="0.6" shininess="0.9"/>
|
| 22 |
+
<material name="body_accent" rgba="1.0 0.52 0.08 1.0" specular="0.7" shininess="0.8"/>
|
| 23 |
+
<material name="eye_black" rgba="0.05 0.05 0.08 1.0" specular="0.9" shininess="1.0"/>
|
| 24 |
+
<material name="eye_pupil" rgba="0.15 0.75 1.0 1.0" emission="0.6"/>
|
| 25 |
+
<material name="leg_metal" rgba="0.25 0.28 0.32 1.0" specular="0.8"/>
|
| 26 |
+
|
| 27 |
+
<material name="duct_rim_cyan" rgba="0.12 0.62 0.88 1.0" specular="0.8" shininess="0.9"/>
|
| 28 |
+
<material name="mesh_grille" rgba="0.2 0.25 0.3 0.85" specular="0.7"/>
|
| 29 |
+
<material name="prop_carbon" rgba="0.1 0.1 0.12 1.0" specular="0.5"/>
|
| 30 |
+
<material name="spinner_gold" rgba="1.0 0.75 0.15 1.0" specular="0.9"/>
|
| 31 |
+
</asset>
|
| 32 |
+
|
| 33 |
+
<worldbody>
|
| 34 |
+
<light pos="0 -1 3.5" dir="0 0.3 -1" directional="true" castshadow="true"/>
|
| 35 |
+
<geom name="floor" type="plane" size="10 10 0.1" material="grid_mat" conaffinity="15" contype="15"/>
|
| 36 |
+
|
| 37 |
+
<!-- ๐ฆ ๋ก๋ด์ ๋ฉ์ธ ๋ฐ๋ -->
|
| 38 |
+
<body name="torso" pos="0 0 0.14">
|
| 39 |
+
<freejoint name="root"/>
|
| 40 |
+
|
| 41 |
+
<geom name="torso_sphere" type="sphere" size="0.065" mass="0.15" material="body_white"/>
|
| 42 |
+
<!-- ํ๋จ ๋ฐฐํฐ๋ฆฌ ์ ์ค์ฌ ์จ์ดํธ (๋ชธ์ฒด ๊ตฌ์ฒด ๋ด๋ถ ์์ชฝ์ ์ ์๋ฉํ์ฌ ์์ผ ๊ฐ์ญ 100% ์ฐจ๋จ) -->
|
| 43 |
+
<geom name="battery_weight" type="box" size="0.025 0.025 0.012" pos="0 0.0 -0.030" mass="0.14" material="leg_metal"/>
|
| 44 |
+
|
| 45 |
+
<!-- ๐ฆ ๋
๋ฆฝ ๋ชฉ/๋จธ๋ฆฌ ํผ์น ๊ด์ : ๋ ๊ฐ์ ๋ชธํต์ ์ํ ๊ทธ๋๋ก ์ ์งํ๊ณ ๋จธ๋ฆฌ(๋ ๋+๋ถ๋ฆฌ+์ผ๊ตด) ์ ์ฒด๊ฐ ์๋๋ก 55ยฐ ์ ์์ฌ์ง! -->
|
| 46 |
+
<body name="head" pos="0 0.035 0.008">
|
| 47 |
+
<joint name="neck_pitch" type="hinge" axis="1 0 0" range="-1.4 0.3" pos="0 0 0"/>
|
| 48 |
+
|
| 49 |
+
<geom name="face_visor" type="capsule" fromto="-0.018 0.005 0 0.018 0.005 0" size="0.025" mass="0.005" material="body_white"/>
|
| 50 |
+
<geom name="left_eye" type="sphere" size="0.012" pos="0.022 0.020 0.007" mass="0.001" material="eye_black"/>
|
| 51 |
+
<geom name="left_pupil" type="sphere" size="0.005" pos="0.022 0.025 0.009" mass="0.001" material="eye_pupil"/>
|
| 52 |
+
<geom name="right_eye" type="sphere" size="0.012" pos="-0.022 0.020 0.007" mass="0.001" material="eye_black"/>
|
| 53 |
+
<geom name="right_pupil" type="sphere" size="0.005" pos="-0.022 0.025 0.009" mass="0.001" material="eye_pupil"/>
|
| 54 |
+
<geom name="beak" type="ellipsoid" size="0.015 0.016 0.007" pos="0 0.025 -0.013" mass="0.002" material="body_accent"/>
|
| 55 |
+
|
| 56 |
+
<!-- ๐ท ๋จธ๋ฆฌ์ ๊ณ ์ ๋ 1์ธ์นญ ๋ ์์ ์นด๋ฉ๋ผ (์ฅ์ ๋ฌผ ์์ด ๋ฐ๋ฅ๊ณผ ํ๋์ด ์์ํ๊ฒ ํธ์ธ FPV ์์ผ) -->
|
| 57 |
+
<camera name="head_fpv_cam" pos="0 0.038 0.012" fovy="85" mode="fixed" xyaxes="1 0 0 0 0 1"/>
|
| 58 |
+
<camera name="left_eye_cam" pos="0.022 0.030 0.010" fovy="85" mode="fixed" xyaxes="1 0 0 0 0 1"/>
|
| 59 |
+
<camera name="right_eye_cam" pos="-0.022 0.030 0.010" fovy="85" mode="fixed" xyaxes="1 0 0 0 0 1"/>
|
| 60 |
+
</body>
|
| 61 |
+
|
| 62 |
+
<!-- ========================================================================================= -->
|
| 63 |
+
<!-- ๐ชฝ ์ข์ธก ๋ ๊ฐ: Z์ถ(์ํ) ํ์ ํ์ง -->
|
| 64 |
+
<!-- 0 rad: ๋ฑ ๋ค(-Y)๋ก ์ ํฌ๊ฐ์ง -->
|
| 65 |
+
<!-- +1.57 rad: ์ข์ธก(+X)์ผ๋ก 180ยฐ ์ํ์ผ๋ก ์ซ ํผ์ณ์ง! -->
|
| 66 |
+
<!-- ========================================================================================= -->
|
| 67 |
+
<body name="left_wing_base" pos="0.045 0 0.01">
|
| 68 |
+
<joint name="left_wing_fold" type="hinge" axis="0 0 1" range="-0.2 1.8" pos="0 0 0"/>
|
| 69 |
+
<geom name="left_wing_arm" type="capsule" fromto="0 0 0 0 -0.075 0" size="0.007" mass="0.003" material="body_accent"/>
|
| 70 |
+
|
| 71 |
+
<body name="left_rotor_tilt" pos="0 -0.075 0">
|
| 72 |
+
<joint name="left_tilt" type="hinge" axis="1 0 0" range="-0.5 2.0" pos="0 0 0"/>
|
| 73 |
+
|
| 74 |
+
<!-- ๐ ์์ด ๋ปฅ ๋ซ๋ฆฐ ์๋ฆ๋ค์ด ์ํ ๋ํธ ๋ฆผ (Duct Guard Ring) -->
|
| 75 |
+
<geom name="l_ring_0" type="capsule" fromto="0.0460 0.0000 0 0.0398 0.0230 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 76 |
+
<geom name="l_ring_1" type="capsule" fromto="0.0398 0.0230 0 0.0230 0.0398 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 77 |
+
<geom name="l_ring_2" type="capsule" fromto="0.0230 0.0398 0 0.0000 0.0460 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 78 |
+
<geom name="l_ring_3" type="capsule" fromto="0.0000 0.0460 0 -0.0230 0.0398 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 79 |
+
<geom name="l_ring_4" type="capsule" fromto="-0.0230 0.0398 0 -0.0398 0.0230 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 80 |
+
<geom name="l_ring_5" type="capsule" fromto="-0.0398 0.0230 0 -0.0460 0.0000 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 81 |
+
<geom name="l_ring_6" type="capsule" fromto="-0.0460 0.0000 0 -0.0398 -0.0230 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 82 |
+
<geom name="l_ring_7" type="capsule" fromto="-0.0398 -0.0230 0 -0.0230 -0.0398 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 83 |
+
<geom name="l_ring_8" type="capsule" fromto="-0.0230 -0.0398 0 0.0000 -0.0460 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 84 |
+
<geom name="l_ring_9" type="capsule" fromto="0.0000 -0.0460 0 0.0230 -0.0398 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 85 |
+
<geom name="l_ring_10" type="capsule" fromto="0.0230 -0.0398 0 0.0398 -0.0230 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 86 |
+
<geom name="l_ring_11" type="capsule" fromto="0.0398 -0.0230 0 0.0460 0.0000 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 87 |
+
|
| 88 |
+
<!-- ํ๋จ ๋ชจํฐ ๋ง์ดํธ ์ง์ง ์คํฌํฌ -->
|
| 89 |
+
<geom name="l_spoke_1" type="capsule" fromto="-0.044 0 -0.005 0.044 0 -0.005" size="0.0015" mass="0.0002" material="leg_metal"/>
|
| 90 |
+
<geom name="l_spoke_2" type="capsule" fromto="0 -0.044 -0.005 0 0.044 -0.005" size="0.0015" mass="0.0002" material="leg_metal"/>
|
| 91 |
+
|
| 92 |
+
<!-- ์ค์ ๋ชจํฐ ํ๋ธ ์คํผ๋ -->
|
| 93 |
+
<geom name="left_spinner" type="sphere" size="0.010" pos="0 0 0" mass="0.001" material="spinner_gold"/>
|
| 94 |
+
|
| 95 |
+
<!-- ๐ช๏ธ 3์ฝ ๋ค์ด๋ด๋ฏน ๊ณ ์ ํ์ ํ๋กํ ๋ฌ (๋์ ํ์คํ ๋ณด์ด๋ ์ปฌ๋ฌ ํ ์ฅ์ฐฉ) -->
|
| 96 |
+
<body name="left_propeller" pos="0 0 0.003">
|
| 97 |
+
<joint name="left_prop_spin" type="hinge" axis="0 0 1" damping="0.0005"/>
|
| 98 |
+
<!-- 120๋ ๊ฐ๊ฒฉ 3์ฝ ๋ธ๋ ์ด๋ -->
|
| 99 |
+
<geom name="l_b1" type="capsule" fromto="0 0 0 0.041 0 0" size="0.0045" mass="0.0005" material="prop_carbon"/>
|
| 100 |
+
<geom name="l_b1_tip" type="sphere" size="0.006" pos="0.041 0 0" mass="0.0001" material="body_accent"/>
|
| 101 |
+
|
| 102 |
+
<geom name="l_b2" type="capsule" fromto="0 0 0 -0.0205 0.0355 0" size="0.0045" mass="0.0005" material="prop_carbon"/>
|
| 103 |
+
<geom name="l_b2_tip" type="sphere" size="0.006" pos="-0.0205 0.0355 0" mass="0.0001" material="body_accent"/>
|
| 104 |
+
|
| 105 |
+
<geom name="l_b3" type="capsule" fromto="0 0 0 -0.0205 -0.0355 0" size="0.0045" mass="0.0005" material="prop_carbon"/>
|
| 106 |
+
<geom name="l_b3_tip" type="sphere" size="0.006" pos="-0.0205 -0.0355 0" mass="0.0001" material="body_accent"/>
|
| 107 |
+
</body>
|
| 108 |
+
</body>
|
| 109 |
+
</body>
|
| 110 |
+
|
| 111 |
+
<!-- ========================================================================================= -->
|
| 112 |
+
<!-- ๐ชฝ ์ฐ์ธก ๋ ๊ฐ: Z์ถ(์ํ) ํ์ ํ์ง -->
|
| 113 |
+
<!-- 0 rad: ๋ฑ ๋ค(-Y)๋ก ์ ํฌ๊ฐ์ง (์ข์ธก ๋ํธ์ ํฌ๊ฐ์ง) -->
|
| 114 |
+
<!-- -1.57 rad: ์ฐ์ธก(-X)์ผ๋ก 180ยฐ ์ํ์ผ๋ก ์ซ ํผ์ณ์ง! -->
|
| 115 |
+
<!-- ========================================================================================= -->
|
| 116 |
+
<body name="right_wing_base" pos="-0.045 0 0.01">
|
| 117 |
+
<joint name="right_wing_fold" type="hinge" axis="0 0 1" range="-1.8 0.2" pos="0 0 0"/>
|
| 118 |
+
<geom name="right_wing_arm" type="capsule" fromto="0 0 0 0 -0.075 0" size="0.007" mass="0.003" material="body_accent"/>
|
| 119 |
+
|
| 120 |
+
<body name="right_rotor_tilt" pos="0 -0.075 0">
|
| 121 |
+
<joint name="right_tilt" type="hinge" axis="1 0 0" range="-0.5 2.0" pos="0 0 0"/>
|
| 122 |
+
|
| 123 |
+
<!-- ๐ ์์ด ๋ปฅ ๋ซ๋ฆฐ ์๋ฆ๋ค์ด ์ํ ๋ํธ ๋ฆผ (Duct Guard Ring) -->
|
| 124 |
+
<geom name="r_ring_0" type="capsule" fromto="0.0460 0.0000 0 0.0398 0.0230 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 125 |
+
<geom name="r_ring_1" type="capsule" fromto="0.0398 0.0230 0 0.0230 0.0398 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 126 |
+
<geom name="r_ring_2" type="capsule" fromto="0.0230 0.0398 0 0.0000 0.0460 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 127 |
+
<geom name="r_ring_3" type="capsule" fromto="0.0000 0.0460 0 -0.0230 0.0398 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 128 |
+
<geom name="r_ring_4" type="capsule" fromto="-0.0230 0.0398 0 -0.0398 0.0230 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 129 |
+
<geom name="r_ring_5" type="capsule" fromto="-0.0398 0.0230 0 -0.0460 0.0000 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 130 |
+
<geom name="r_ring_6" type="capsule" fromto="-0.0460 0.0000 0 -0.0398 -0.0230 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 131 |
+
<geom name="r_ring_7" type="capsule" fromto="-0.0398 -0.0230 0 -0.0230 -0.0398 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 132 |
+
<geom name="r_ring_8" type="capsule" fromto="-0.0230 -0.0398 0 0.0000 -0.0460 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 133 |
+
<geom name="r_ring_9" type="capsule" fromto="0.0000 -0.0460 0 0.0230 -0.0398 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 134 |
+
<geom name="r_ring_10" type="capsule" fromto="0.0230 -0.0398 0 0.0398 -0.0230 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 135 |
+
<geom name="r_ring_11" type="capsule" fromto="0.0398 -0.0230 0 0.0460 0.0000 0" size="0.0035" mass="0.0004" material="duct_rim_cyan"/>
|
| 136 |
+
|
| 137 |
+
<!-- ํ๋จ ๋ชจํฐ ๋ง์ดํธ ์ง์ง ์คํฌํฌ -->
|
| 138 |
+
<geom name="r_spoke_1" type="capsule" fromto="-0.044 0 -0.005 0.044 0 -0.005" size="0.0015" mass="0.0002" material="leg_metal"/>
|
| 139 |
+
<geom name="r_spoke_2" type="capsule" fromto="0 -0.044 -0.005 0 0.044 -0.005" size="0.0015" mass="0.0002" material="leg_metal"/>
|
| 140 |
+
|
| 141 |
+
<!-- ์ค์ ๋ชจํฐ ํ๋ธ ์คํผ๋ -->
|
| 142 |
+
<geom name="right_spinner" type="sphere" size="0.010" pos="0 0 0" mass="0.001" material="spinner_gold"/>
|
| 143 |
+
|
| 144 |
+
<!-- ๐ช๏ธ 3์ฝ ๋ค์ด๋ด๋ฏน ๊ณ ์ ํ์ ํ๋กํ ๋ฌ (๋์ ํ์คํ ๋ณด์ด๋ ์ปฌ๋ฌ ํ ์ฅ์ฐฉ) -->
|
| 145 |
+
<body name="right_propeller" pos="0 0 0.003">
|
| 146 |
+
<joint name="right_prop_spin" type="hinge" axis="0 0 1" damping="0.0005"/>
|
| 147 |
+
<!-- 120๋ ๊ฐ๊ฒฉ 3์ฝ ๋ธ๋ ์ด๋ -->
|
| 148 |
+
<geom name="r_b1" type="capsule" fromto="0 0 0 0.041 0 0" size="0.0045" mass="0.0005" material="prop_carbon"/>
|
| 149 |
+
<geom name="r_b1_tip" type="sphere" size="0.006" pos="0.041 0 0" mass="0.0001" material="body_accent"/>
|
| 150 |
+
|
| 151 |
+
<geom name="r_b2" type="capsule" fromto="0 0 0 -0.0205 0.0355 0" size="0.0045" mass="0.0005" material="prop_carbon"/>
|
| 152 |
+
<geom name="r_b2_tip" type="sphere" size="0.006" pos="-0.0205 0.0355 0" mass="0.0001" material="body_accent"/>
|
| 153 |
+
|
| 154 |
+
<geom name="r_b3" type="capsule" fromto="0 0 0 -0.0205 -0.0355 0" size="0.0045" mass="0.0005" material="prop_carbon"/>
|
| 155 |
+
<geom name="r_b3_tip" type="sphere" size="0.006" pos="-0.0205 -0.0355 0" mass="0.0001" material="body_accent"/>
|
| 156 |
+
</body>
|
| 157 |
+
</body>
|
| 158 |
+
</body>
|
| 159 |
+
|
| 160 |
+
<!-- ==================== ๐ฆต ์ข์ธก 2์กฑ ๋ณดํ ๋ค๋ฆฌ (๋์ฆ๋/BDX ์คํ์ผ ์ญ๊ด์ + ํ์ดํธ ์๋จธ + ํฌํค ์์ฆ) ==================== -->
|
| 161 |
+
<body name="left_hip" pos="0.032 0.010 -0.035">
|
| 162 |
+
<joint name="left_hip_pitch" type="hinge" axis="1 0 0" range="-1.8 1.8" pos="0 0 0"/>
|
| 163 |
+
<!-- ๋ฉ์ธ ํ๋ฒ
์ง ํ๋ ์ + ์ธ์ธก ํ์ดํธ ์๋จธ ์ปค๋ฒ (์ฌ์ง 1, 2, 3) -->
|
| 164 |
+
<geom name="left_thigh" type="capsule" fromto="0 0 0 0 -0.030 -0.030" size="0.008" mass="0.008" material="leg_metal"/>
|
| 165 |
+
<geom name="left_thigh_armor" type="capsule" fromto="0.010 0 0 0.010 -0.030 -0.030" size="0.011" mass="0.004" material="body_white"/>
|
| 166 |
+
|
| 167 |
+
<body name="left_knee" pos="0 -0.030 -0.030">
|
| 168 |
+
<joint name="left_knee_pitch" type="hinge" axis="1 0 0" range="-2.2 2.2" pos="0 0 0"/>
|
| 169 |
+
<geom name="left_knee_hub" type="cylinder" size="0.008 0.012" pos="0 0 0" quat="0.707 0 0.707 0" mass="0.002" material="spinner_gold"/>
|
| 170 |
+
<geom name="left_shin" type="capsule" fromto="0 0 0 0 0.030 -0.042" size="0.008" mass="0.008" material="leg_metal"/>
|
| 171 |
+
<geom name="left_shin_brace" type="capsule" fromto="0.008 0.005 -0.005 0.008 0.025 -0.035" size="0.004" mass="0.002" material="body_white"/>
|
| 172 |
+
|
| 173 |
+
<!-- ๐ ์
์ฒดํ 2๋จ ํฌํค ์ปฌ๋ฌ ์์ฆ (์ค๋ ์ง U-๋ธ๋ํท + ๋ฏผํธ/์์ ๋ผ์ด๋ ๋ฌ๋ฒ์) -->
|
| 174 |
+
<body name="left_foot" pos="0 0.030 -0.042">
|
| 175 |
+
<joint name="left_ankle_pitch" type="hinge" axis="1 0 0" range="-1.5 1.5" pos="0 0 0"/>
|
| 176 |
+
|
| 177 |
+
<!-- 1. ๋ฐ๋ชฉ ํผ๋ฒ ๋ชจํฐ/๋ฒ ์ด๋ง ๊ณจ๋ ๋์คํฌ (์ฌ์ง ์ ์ํ ์ถ) -->
|
| 178 |
+
<geom name="left_ankle_hub" type="cylinder" size="0.008 0.015" pos="0 0 0" quat="0.707 0 0.707 0" mass="0.003" material="spinner_gold"/>
|
| 179 |
+
|
| 180 |
+
<!-- 2. ์๋จ ์ค๋ ์ง/์๋ก์ฐ U์ํ ๋ฐ๋ชฉ ๋ธ๋ํท (์ฌ์ง 1, 2, 3 U์ ์ปต) -->
|
| 181 |
+
<geom name="left_ankle_bracket" type="capsule" fromto="0 -0.004 0.006 0 0.010 -0.004" size="0.011" mass="0.008" material="body_accent"/>
|
| 182 |
+
<geom name="left_shoe_top" type="box" size="0.018 0.026 0.004" pos="0 -0.002 -0.004" mass="0.01" material="body_accent"/>
|
| 183 |
+
|
| 184 |
+
<!-- 3. ํ๋จ ๋ฏผํธ/์์ ๋ผ์ด๋ ๋กค๋ฌ ๋ฌ๋ฒ์ (๋ค๊ฟ์น๋ฅผ 2.8cm๋ก ํ์ฅํ์ฌ ํ์ง ๋ณดํ ์๋ฒฝ ์ง์ง) -->
|
| 185 |
+
<geom name="left_sole_heel" type="capsule" fromto="-0.014 -0.028 -0.008 0.014 -0.028 -0.008" size="0.006" mass="0.006" friction="3.0 0.1 0.1" material="duct_rim_cyan"/>
|
| 186 |
+
<geom name="left_sole_toe" type="capsule" fromto="-0.014 0.022 -0.008 0.014 0.022 -0.008" size="0.006" mass="0.005" friction="3.0 0.1 0.1" material="duct_rim_cyan"/>
|
| 187 |
+
<geom name="left_sole_base" type="box" size="0.017 0.024 0.004" pos="0 -0.003 -0.008" mass="0.01" friction="3.0 0.1 0.1" material="duct_rim_cyan"/>
|
| 188 |
+
</body>
|
| 189 |
+
</body>
|
| 190 |
+
</body>
|
| 191 |
+
|
| 192 |
+
<!-- ==================== ๐ฆต ์ฐ์ธก 2์กฑ ๋ณดํ ๋ค๋ฆฌ (๋์ฆ๋/BDX ์คํ์ผ ์ญ๊ด์ + ํ์ดํธ ์๋จธ + ํฌํค ์์ฆ) ==================== -->
|
| 193 |
+
<body name="right_hip" pos="-0.032 0.010 -0.035">
|
| 194 |
+
<joint name="right_hip_pitch" type="hinge" axis="1 0 0" range="-1.8 1.8" pos="0 0 0"/>
|
| 195 |
+
<!-- ๋ฉ์ธ ํ๋ฒ
์ง ํ๋ ์ + ์ธ์ธก ํ์ดํธ ์๋จธ ์ปค๋ฒ (์ฌ์ง 1, 2, 3) -->
|
| 196 |
+
<geom name="right_thigh" type="capsule" fromto="0 0 0 0 -0.030 -0.030" size="0.008" mass="0.008" material="leg_metal"/>
|
| 197 |
+
<geom name="right_thigh_armor" type="capsule" fromto="-0.010 0 0 -0.010 -0.030 -0.030" size="0.011" mass="0.004" material="body_white"/>
|
| 198 |
+
|
| 199 |
+
<body name="right_knee" pos="0 -0.030 -0.030">
|
| 200 |
+
<joint name="right_knee_pitch" type="hinge" axis="1 0 0" range="-2.2 2.2" pos="0 0 0"/>
|
| 201 |
+
<geom name="right_knee_hub" type="cylinder" size="0.008 0.012" pos="0 0 0" quat="0.707 0 0.707 0" mass="0.002" material="spinner_gold"/>
|
| 202 |
+
<geom name="right_shin" type="capsule" fromto="0 0 0 0 0.030 -0.042" size="0.008" mass="0.008" material="leg_metal"/>
|
| 203 |
+
<geom name="right_shin_brace" type="capsule" fromto="-0.008 0.005 -0.005 -0.008 0.025 -0.035" size="0.004" mass="0.002" material="body_white"/>
|
| 204 |
+
|
| 205 |
+
<!-- ๐ ์
์ฒดํ 2๋จ ํฌํค ์ปฌ๋ฌ ์์ฆ (์ค๋ ์ง U-๋ธ๋ํท + ๋ฏผํธ/์์ ๋ผ์ด๋ ๋ฌ๋ฒ์) -->
|
| 206 |
+
<body name="right_foot" pos="0 0.030 -0.042">
|
| 207 |
+
<joint name="right_ankle_pitch" type="hinge" axis="1 0 0" range="-1.5 1.5" pos="0 0 0"/>
|
| 208 |
+
|
| 209 |
+
<!-- 1. ๋ฐ๋ชฉ ํผ๋ฒ ๋ชจํฐ/๋ฒ ์ด๋ง ๊ณจ๋ ๋์คํฌ (์ฌ์ง ์ ์ํ ์ถ) -->
|
| 210 |
+
<geom name="right_ankle_hub" type="cylinder" size="0.008 0.015" pos="0 0 0" quat="0.707 0 0.707 0" mass="0.003" material="spinner_gold"/>
|
| 211 |
+
|
| 212 |
+
<!-- 2. ์๋จ ์ค๋ ์ง/์๋ก์ฐ U์ํ ๋ฐ๋ชฉ ๋ธ๋ํท (์ฌ์ง 1, 2, 3 U์ ์ปต) -->
|
| 213 |
+
<geom name="right_ankle_bracket" type="capsule" fromto="0 -0.004 0.006 0 0.010 -0.004" size="0.011" mass="0.008" material="body_accent"/>
|
| 214 |
+
<geom name="right_shoe_top" type="box" size="0.018 0.026 0.004" pos="0 -0.002 -0.004" mass="0.01" material="body_accent"/>
|
| 215 |
+
|
| 216 |
+
<!-- 3. ํ๋จ ๋ฏผํธ/์์ ๋ผ์ด๋ ๋กค๋ฌ ๋ฌ๋ฒ์ (๋ค๊ฟ์น๋ฅผ 2.8cm๋ก ํ์ฅํ์ฌ ํ์ง ๋ณดํ ์๋ฒฝ ์ง์ง) -->
|
| 217 |
+
<geom name="right_sole_heel" type="capsule" fromto="-0.014 -0.028 -0.008 0.014 -0.028 -0.008" size="0.006" mass="0.006" friction="3.0 0.1 0.1" material="duct_rim_cyan"/>
|
| 218 |
+
<geom name="right_sole_toe" type="capsule" fromto="-0.014 0.022 -0.008 0.014 0.022 -0.008" size="0.006" mass="0.005" friction="3.0 0.1 0.1" material="duct_rim_cyan"/>
|
| 219 |
+
<geom name="right_sole_base" type="box" size="0.017 0.024 0.004" pos="0 -0.003 -0.008" mass="0.01" friction="3.0 0.1 0.1" material="duct_rim_cyan"/>
|
| 220 |
+
</body>
|
| 221 |
+
</body>
|
| 222 |
+
</body>
|
| 223 |
+
|
| 224 |
+
</body>
|
| 225 |
+
</worldbody>
|
| 226 |
+
|
| 227 |
+
<!-- ์ก์ถ์์ดํฐ -->
|
| 228 |
+
<actuator>
|
| 229 |
+
<position name="act_l_hip" joint="left_hip_pitch" ctrlrange="-1.8 1.8" kp="500" kv="30"/>
|
| 230 |
+
<position name="act_l_knee" joint="left_knee_pitch" ctrlrange="-2.2 2.2" kp="500" kv="30"/>
|
| 231 |
+
<position name="act_l_ankle" joint="left_ankle_pitch" ctrlrange="-1.5 1.5" kp="500" kv="30"/>
|
| 232 |
+
|
| 233 |
+
<position name="act_r_hip" joint="right_hip_pitch" ctrlrange="-1.8 1.8" kp="500" kv="30"/>
|
| 234 |
+
<position name="act_r_knee" joint="right_knee_pitch" ctrlrange="-2.2 2.2" kp="500" kv="30"/>
|
| 235 |
+
<position name="act_r_ankle" joint="right_ankle_pitch" ctrlrange="-1.5 1.5" kp="500" kv="30"/>
|
| 236 |
+
|
| 237 |
+
<!-- ๋ ๊ฐ ํ์ ์ถ: Z์ถ ์ํ ํ์ -->
|
| 238 |
+
<position name="act_l_fold" joint="left_wing_fold" ctrlrange="-0.2 1.8" kp="150" kv="10"/>
|
| 239 |
+
<position name="act_r_fold" joint="right_wing_fold" ctrlrange="-1.8 0.2" kp="150" kv="10"/>
|
| 240 |
+
|
| 241 |
+
<!-- ํธํธ ๋กํฐ ์๋ณด: ์ธ๋ก ์ ํ(1.57rad) ~ ์ํ ์ ๊ฐ(0rad) -->
|
| 242 |
+
<position name="act_l_tilt" joint="left_tilt" ctrlrange="-0.5 2.0" kp="180" kv="12"/>
|
| 243 |
+
<position name="act_r_tilt" joint="right_tilt" ctrlrange="-0.5 2.0" kp="180" kv="12"/>
|
| 244 |
+
|
| 245 |
+
<!-- ๐ฆ ๋จธ๋ฆฌ/๋ชฉ ํผ์น ์๋ณด: ๋ชธํต/๋ ๊ฐ๋ ์ํ ์ ์ง, ๋จธ๋ฆฌ(๋ ๋+๋ถ๋ฆฌ)๊ฐ ์๋๋ก 55ยฐ ์ ์์ฌ์ง! -->
|
| 246 |
+
<position name="act_neck" joint="neck_pitch" ctrlrange="-1.4 0.3" kp="150" kv="10"/>
|
| 247 |
+
</actuator>
|
| 248 |
+
</mujoco>
|
robot_bird/recorder.py
ADDED
|
@@ -0,0 +1,183 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import os
|
| 2 |
+
import sys
|
| 3 |
+
import time
|
| 4 |
+
import datetime
|
| 5 |
+
import mujoco
|
| 6 |
+
import numpy as np
|
| 7 |
+
|
| 8 |
+
# UTF-8 ์
์ถ๋ ฅ ๋ณด์ฅ
|
| 9 |
+
if sys.platform == "win32":
|
| 10 |
+
try:
|
| 11 |
+
sys.stdout.reconfigure(encoding='utf-8', errors='replace')
|
| 12 |
+
sys.stderr.reconfigure(encoding='utf-8', errors='replace')
|
| 13 |
+
except Exception:
|
| 14 |
+
pass
|
| 15 |
+
|
| 16 |
+
# ๋น๋์ค ์ธ์ฝ๋ ์์ ์ํฌํธ (imageio ์ฐ์ , cv2 ๋์ฒด)
|
| 17 |
+
try:
|
| 18 |
+
import imageio
|
| 19 |
+
HAS_IMAGEIO = True
|
| 20 |
+
except ImportError:
|
| 21 |
+
imageio = None
|
| 22 |
+
HAS_IMAGEIO = False
|
| 23 |
+
|
| 24 |
+
try:
|
| 25 |
+
import cv2
|
| 26 |
+
HAS_OPENCV = True
|
| 27 |
+
except ImportError:
|
| 28 |
+
cv2 = None
|
| 29 |
+
HAS_OPENCV = False
|
| 30 |
+
|
| 31 |
+
class ScreenRecorder:
|
| 32 |
+
"""
|
| 33 |
+
๋ฐ๋ ค ๋ก๋ด์(Bipedal VTOL Robot Bird) ์ค์๊ฐ ํ์ค H.264 MP4 ๋น๋์ค ๋
นํ๊ธฐ
|
| 34 |
+
- N ๋๋ X ํค๋ฅผ ๋๋ฌ ์ค์๊ฐ ๋
นํ ์์ ๋ฐ ์ค์ง (Toggle)
|
| 35 |
+
- ํ์ค H.264 (libx264, yuv420p) ์ ์ฉ์ผ๋ก ์๋์ฐ ๋ฏธ๋์ด ํ๋ ์ด์ด, ๋งฅ, ์ค๋งํธํฐ 100% ์ฆ์ ์ฌ์
|
| 36 |
+
- 3์ธ์นญ ์ค๋งํธ ๋ทฐ ๋ฐ C ํค๋ก ์ ํ๋ 1์ธ์นญ ๋ ์นด๋ฉ๋ผ ๋ทฐ ์๋ฒฝ ๋
นํ
|
| 37 |
+
- recordings/ ํด๋์ ๋ ์ง/์๊ฐ๋ณ MP4 ์๋ ์ ์ฅ
|
| 38 |
+
"""
|
| 39 |
+
def __init__(self, model, data, width=640, height=480, fps=30):
|
| 40 |
+
self.model = model
|
| 41 |
+
self.data = data
|
| 42 |
+
self.width = width
|
| 43 |
+
self.height = height
|
| 44 |
+
self.fps = fps
|
| 45 |
+
self.renderer = mujoco.Renderer(model, height, width)
|
| 46 |
+
self.orbit_cam = mujoco.MjvCamera()
|
| 47 |
+
self.orbit_cam.azimuth = 145
|
| 48 |
+
self.orbit_cam.elevation = -15
|
| 49 |
+
self.orbit_cam.distance = 1.6
|
| 50 |
+
|
| 51 |
+
self.is_recording = False
|
| 52 |
+
self.writer_type = None # 'imageio' or 'cv2'
|
| 53 |
+
self.video_writer = None
|
| 54 |
+
self.output_filepath = ""
|
| 55 |
+
self.frame_count = 0
|
| 56 |
+
self.start_time = 0.0
|
| 57 |
+
self.last_capture_time = 0.0
|
| 58 |
+
self.frame_interval = 1.0 / self.fps
|
| 59 |
+
|
| 60 |
+
self.recordings_dir = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "recordings")
|
| 61 |
+
os.makedirs(self.recordings_dir, exist_ok=True)
|
| 62 |
+
|
| 63 |
+
def toggle(self, cam_mode=0):
|
| 64 |
+
if not self.is_recording:
|
| 65 |
+
self.start(cam_mode)
|
| 66 |
+
else:
|
| 67 |
+
self.stop()
|
| 68 |
+
|
| 69 |
+
def start(self, cam_mode=0):
|
| 70 |
+
if not HAS_IMAGEIO and not HAS_OPENCV:
|
| 71 |
+
print("\n" + "=" * 70)
|
| 72 |
+
print(" โ ๏ธ [๋
นํ ๋ชจ๋ ์๋ด] ๋น๋์ค ๋
นํ๋ฅผ ์ํด imageio ๋๋ opencv-python ํจํค์ง๊ฐ ํ์ํฉ๋๋ค.")
|
| 73 |
+
print(" ๐ก ์ค์น ๋ช
๋ น์ด: pip install imageio[ffmpeg] opencv-python")
|
| 74 |
+
print("=" * 70 + "\n")
|
| 75 |
+
return
|
| 76 |
+
|
| 77 |
+
now_str = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
|
| 78 |
+
cam_tag = "3rd" if cam_mode == 0 else ("fpv" if cam_mode == 1 else ("lefteye" if cam_mode == 2 else "righteye"))
|
| 79 |
+
filename = f"robot_bird_{cam_tag}_{now_str}.mp4"
|
| 80 |
+
self.output_filepath = os.path.join(self.recordings_dir, filename)
|
| 81 |
+
|
| 82 |
+
# ๐ ํ์ค H.264 (libx264, yuv420p) ์ธ์ฝ๋๋ก ์๋์ฐ ๋ฏธ๋์ด ํ๋ ์ด์ด 100% ํธํ ๋ณด์ฅ
|
| 83 |
+
if HAS_IMAGEIO:
|
| 84 |
+
try:
|
| 85 |
+
self.video_writer = imageio.get_writer(
|
| 86 |
+
self.output_filepath,
|
| 87 |
+
fps=self.fps,
|
| 88 |
+
codec='libx264',
|
| 89 |
+
pixelformat='yuv420p',
|
| 90 |
+
format='FFMPEG'
|
| 91 |
+
)
|
| 92 |
+
self.writer_type = 'imageio'
|
| 93 |
+
except Exception:
|
| 94 |
+
self.writer_type = None
|
| 95 |
+
|
| 96 |
+
if self.video_writer is None and HAS_OPENCV:
|
| 97 |
+
try:
|
| 98 |
+
# FourCC H264 / mp4v fallback
|
| 99 |
+
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
|
| 100 |
+
self.video_writer = cv2.VideoWriter(self.output_filepath, fourcc, self.fps, (self.width, self.height))
|
| 101 |
+
self.writer_type = 'cv2'
|
| 102 |
+
except Exception:
|
| 103 |
+
self.writer_type = None
|
| 104 |
+
|
| 105 |
+
if self.video_writer is None:
|
| 106 |
+
print(f"\n[์ค๋ฅ] ๋น๋์ค ์ธ์ฝ๋๋ฅผ ์ด๊ธฐํํ ์ ์์ต๋๋ค.")
|
| 107 |
+
return
|
| 108 |
+
|
| 109 |
+
self.is_recording = True
|
| 110 |
+
self.frame_count = 0
|
| 111 |
+
self.start_time = time.time()
|
| 112 |
+
self.last_capture_time = 0.0
|
| 113 |
+
|
| 114 |
+
print(f"\n" + "=" * 70)
|
| 115 |
+
print(f" ๐ด [REC ๋
นํ ์์!] ํ์ค H.264 ๊ณ ํ์ง ๋น๋์ค ๋
นํ ์ค... (30 FPS)")
|
| 116 |
+
print(f" ๐ ์ ์ฅ ๋์: {self.output_filepath}")
|
| 117 |
+
print(f" ๐ก ๋
นํ๋ฅผ ๋๋ด๋ ค๋ฉด ๋ค์ 'X' ํค๋ฅผ ๋๋ฅด์ธ์!")
|
| 118 |
+
print("=" * 70 + "\n")
|
| 119 |
+
|
| 120 |
+
def capture_frame(self, cam_mode=0):
|
| 121 |
+
if not self.is_recording or self.video_writer is None:
|
| 122 |
+
return
|
| 123 |
+
|
| 124 |
+
now = time.time()
|
| 125 |
+
if now - self.last_capture_time < self.frame_interval:
|
| 126 |
+
return
|
| 127 |
+
|
| 128 |
+
self.last_capture_time = now
|
| 129 |
+
|
| 130 |
+
try:
|
| 131 |
+
if cam_mode == 0:
|
| 132 |
+
self.orbit_cam.lookat[0] = self.data.qpos[0]
|
| 133 |
+
self.orbit_cam.lookat[1] = self.data.qpos[1]
|
| 134 |
+
self.orbit_cam.lookat[2] = self.data.qpos[2]
|
| 135 |
+
self.renderer.update_scene(self.data, camera=self.orbit_cam)
|
| 136 |
+
elif cam_mode == 1:
|
| 137 |
+
self.renderer.update_scene(self.data, camera="head_fpv_cam")
|
| 138 |
+
elif cam_mode == 2:
|
| 139 |
+
self.renderer.update_scene(self.data, camera="left_eye_cam")
|
| 140 |
+
elif cam_mode == 3:
|
| 141 |
+
self.renderer.update_scene(self.data, camera="right_eye_cam")
|
| 142 |
+
else:
|
| 143 |
+
self.renderer.update_scene(self.data)
|
| 144 |
+
|
| 145 |
+
rgb_frame = self.renderer.render()
|
| 146 |
+
|
| 147 |
+
if self.writer_type == 'imageio':
|
| 148 |
+
self.video_writer.append_data(rgb_frame)
|
| 149 |
+
elif self.writer_type == 'cv2':
|
| 150 |
+
bgr_frame = cv2.cvtColor(rgb_frame, cv2.COLOR_RGB2BGR)
|
| 151 |
+
self.video_writer.write(bgr_frame)
|
| 152 |
+
|
| 153 |
+
self.frame_count += 1
|
| 154 |
+
except Exception as e:
|
| 155 |
+
pass
|
| 156 |
+
|
| 157 |
+
def stop(self):
|
| 158 |
+
if not self.is_recording:
|
| 159 |
+
return
|
| 160 |
+
|
| 161 |
+
self.is_recording = False
|
| 162 |
+
duration = time.time() - self.start_time
|
| 163 |
+
|
| 164 |
+
try:
|
| 165 |
+
if self.writer_type == 'imageio' and self.video_writer is not None:
|
| 166 |
+
self.video_writer.close()
|
| 167 |
+
elif self.writer_type == 'cv2' and self.video_writer is not None:
|
| 168 |
+
self.video_writer.release()
|
| 169 |
+
except Exception:
|
| 170 |
+
pass
|
| 171 |
+
|
| 172 |
+
self.video_writer = None
|
| 173 |
+
self.writer_type = None
|
| 174 |
+
|
| 175 |
+
file_size_mb = 0.0
|
| 176 |
+
if os.path.exists(self.output_filepath):
|
| 177 |
+
file_size_mb = os.path.getsize(self.output_filepath) / (1024 * 1024)
|
| 178 |
+
|
| 179 |
+
print(f"\n" + "=" * 70)
|
| 180 |
+
print(f" ๐พ [๋
นํ ์๋ฃ ๋ฐ ์ ์ฅ!] ์์์ด ๋ค์ด๋ก๋(์ ์ฅ)๋์์ต๋๋ค.")
|
| 181 |
+
print(f" ๐ ํ์ผ ์์น: {self.output_filepath}")
|
| 182 |
+
print(f" โฑ๏ธ ๋
นํ ์๊ฐ: {duration:.1f}์ด ({self.frame_count} ํ๋ ์, {file_size_mb:.2f} MB)")
|
| 183 |
+
print("=" * 70 + "\n")
|
run_ai_robot_bird.py
ADDED
|
@@ -0,0 +1,355 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import os
|
| 2 |
+
import sys
|
| 3 |
+
import time
|
| 4 |
+
import math
|
| 5 |
+
import mujoco
|
| 6 |
+
import mujoco.viewer
|
| 7 |
+
import numpy as np
|
| 8 |
+
|
| 9 |
+
# UTF-8 ์
์ถ๋ ฅ ๋ณด์ฅ
|
| 10 |
+
if sys.platform == "win32":
|
| 11 |
+
try:
|
| 12 |
+
sys.stdout.reconfigure(encoding='utf-8', errors='replace')
|
| 13 |
+
sys.stderr.reconfigure(encoding='utf-8', errors='replace')
|
| 14 |
+
except Exception:
|
| 15 |
+
pass
|
| 16 |
+
|
| 17 |
+
from robot_bird.controller import RobotBirdFSM
|
| 18 |
+
from robot_bird.ai_brain import RobotBirdAIBrain
|
| 19 |
+
from robot_bird.recorder import ScreenRecorder
|
| 20 |
+
|
| 21 |
+
class ShowcaseDirector:
|
| 22 |
+
"""
|
| 23 |
+
๐ฆ
๋ํ๋์ ์ฐ์ถ ์คํ ๋ฆฌ๋ณด๋๋ฅผ ์ํ์ฒ๋ผ ์๋ฒฝํ๊ฒ ์งํํ๋ ์๋ค๋งํฑ ๋๋ ํฐ
|
| 24 |
+
|
| 25 |
+
[์๋๋ฆฌ์ค ์์ - ๋ฆฌ์ผ 1์ธ์นญ ๋ ์์ ๋นํ & ๋ฐ๋ํ ์์ ๋ณต๊ท]
|
| 26 |
+
1. ๐ถ ์์ผ๋ก ๋น๋นํ๊ฒ ๊ฑท๊ธฐ (Forward Walk)
|
| 27 |
+
2. ๐ถ ์กฐ์ฌ์กฐ์ฌ ๋ค๋ก ๊ฑท๊ธฐ (Backward Walk)
|
| 28 |
+
3. ๐ฅ ๊ฝ๋น! ์์ผ๋ก ๋์ด์ง๊ธฐ (Knockdown)
|
| 29 |
+
4. ๐ ์ค๋์ด ์ง๋ฅ ๋ฐ๋! ์ค์ค๋ก ๋ฒ๋ก ๊ธฐ๋ฆฝ (Self-Righting)
|
| 30 |
+
5. ๐พ ๋ฐ๋ฅ ๋ชจ์ด ์ฝ! ์ฝ! ์ชผ์๋จน๊ธฐ (Pecking)
|
| 31 |
+
6. ๐ฆฟ ๋ค๋ฆฌ๋ฅผ ์ ์ ๊ณ ์ชผ๊ทธ๋ ค ์์๋ค๊ฐ ์ผ์ด์๊ธฐ (Crouch & Stand)
|
| 32 |
+
|
| 33 |
+
[๐ชฝ ๊ทน๊ฐ์ ๋ฆฌ์ผ ์๋ค๋งํฑ ๋นํ & 1์ธ์นญ ๋ ์์ ]
|
| 34 |
+
7. ๐ชฝ ๋ ๊ฐ 180ยฐ ์ํ ์ ๊ฐ + 3์ฝ ํ๋กํ ๋ฌ ๊ฐ์ โ 1.0m ์์ง ์ด๋ฅ!
|
| 35 |
+
8. ๐ [3์ธ์นญ ์ ์ฒด ๋ทฐ] ๋ ๊ฐ๋ฅผ ํด๊ณ ์์ผ๋ก ์ถ๋ฐํ๋ ๋ฉ์ง ์ ์ฒด ๋ชจ์ต
|
| 36 |
+
9. ๐๏ธ [1์ธ์นญ ์ง์ ๋ ์์ 1 - ์ ๋ฐฉ ๋นํ] ๋ก๋ด์ ๋์ผ๋ก ์ง์ ์์ ๋ฐ๋ผ๋ณด๋ฉฐ ๋ฐ๋์ ๊ฐ๋ฅด๊ณ ์~ ๋ ์๊ฐ๋ ๋ฆฌ์ผํ ๋นํ ํ๋ฉด!
|
| 37 |
+
10. ๐ [1์ธ์นญ ์ง์ ๋ ์์ 2 - ํ๋ฐฉ 55ยฐ ๊ด์ธก] ๋ ์๊ฐ๋ฉด์ ๋จธ๋ฆฌ๋ฅผ 55ยฐ ์๋๋ก ํน ์์ฌ ๋ฐ๋ฅ ์ฒด์ปค๋ณด๋๋ฅผ ์ง์ ๋ด๋ ค๋ค๋ณด๋ ๋ฆฌ์ผํ ์ค์บ ํ๋ฉด!
|
| 38 |
+
11. ๐ [3์ธ์นญ ์ ์ฒด ๋ทฐ] ๊ณต์ค์์ 180ยฐ ์ ํดํ์ฌ ์๋ ์ถ๋ฐํ๋ ๋ฐ๋ํ ์ ์ค์(์์ )์ผ๋ก ๋์์ค๋ ์ ์ฒด ๋ชจ์ต!
|
| 39 |
+
12. ๐๏ธ [3์ธ์นญ ์ ์ง ํธ๋ฒ๋ง] ์๋ ์ถ๋ฐํ๋ ๋ฐ๋ก ๊ทธ ์๋ฆฌ ์๊ณต์์ ๊ฐ๋งํ ๋ ๊ณ ์๋ ๋ชจ์ต (Hovering)
|
| 40 |
+
13. ๐ฌ [์ฌ๋ฟ ํ๊ฐ ์ฐฉ์ง] ์๋ ์ถ๋ฐํ๋ ๊ทธ ์๋ฆฌ์ ์ ํํ ์์ง ์ฐฉ์ง & ๋ ๊ฐ ๋ฑ ๋ค๋ก ์ ๊ธฐ!
|
| 41 |
+
โ (์ดํ ๋ฌดํ ๋ฐ๋ณต!)
|
| 42 |
+
"""
|
| 43 |
+
def __init__(self, controller, model, data):
|
| 44 |
+
self.c = controller
|
| 45 |
+
self.m = model
|
| 46 |
+
self.d = data
|
| 47 |
+
self.timeline = 0.0
|
| 48 |
+
self.step_idx = -1
|
| 49 |
+
self.home_x = 0.0
|
| 50 |
+
self.home_y = 0.0
|
| 51 |
+
|
| 52 |
+
def update(self, dt):
|
| 53 |
+
self.timeline += dt
|
| 54 |
+
t = self.timeline
|
| 55 |
+
|
| 56 |
+
# 1๋จ๊ณ: ์์ผ๋ก ๊ฑท๊ธฐ (0.0s ~ 4.0s)
|
| 57 |
+
if t < 4.0:
|
| 58 |
+
if self.step_idx != 1:
|
| 59 |
+
self.step_idx = 1
|
| 60 |
+
self.home_x = self.d.qpos[0]
|
| 61 |
+
self.home_y = self.d.qpos[1]
|
| 62 |
+
self.c.cam_mode = 0 # 3์ธ์นญ ์ ์ฒด ๋ทฐ
|
| 63 |
+
self.c.look_front()
|
| 64 |
+
self.c.wings_deployed = False
|
| 65 |
+
self.c.set_state(self.c.STATE_WALK)
|
| 66 |
+
print("\n[๐ฌ 1๋จ๊ณ] ๐ถ [์ด์กฑ๋ณดํ] ๋ค๋ฆฌ๋ฅผ ์ฑํผ์ฑํผ ๋ฒ๊ฐ์ ๋์ผ๋ฉฐ ์์ผ๋ก ๊ฑท๊ธฐ")
|
| 67 |
+
self.c.walk_speed = 0.8
|
| 68 |
+
self.c.walk_turn = 0.0
|
| 69 |
+
|
| 70 |
+
# 2๋จ๊ณ: ๋ค๋ก ๊ฑท๊ธฐ (4.0s ~ 7.0s)
|
| 71 |
+
elif t < 7.0:
|
| 72 |
+
if self.step_idx != 2:
|
| 73 |
+
self.step_idx = 2
|
| 74 |
+
self.c.set_state(self.c.STATE_WALK)
|
| 75 |
+
print("\n[๐ฌ 2๋จ๊ณ] ๐ถ [์ด์กฑ๋ณดํ] ์กฐ์ฌ์กฐ์ฌ ๋ค๋ก ๊ฑท๊ธฐ (์์ ๊ทผ์ฒ ์ ์ง)")
|
| 76 |
+
self.c.walk_speed = -0.8
|
| 77 |
+
self.c.walk_turn = 0.0
|
| 78 |
+
|
| 79 |
+
# 3๋จ๊ณ: ๋์ด์ง๊ธฐ (7.0s ~ 8.5s)
|
| 80 |
+
elif t < 8.5:
|
| 81 |
+
if self.step_idx != 3:
|
| 82 |
+
self.step_idx = 3
|
| 83 |
+
self.c.walk_speed = 0.0
|
| 84 |
+
self.d.qvel[0] = 0.35
|
| 85 |
+
self.d.qvel[3] = 4.5
|
| 86 |
+
self.d.qvel[4] = 2.0
|
| 87 |
+
self.c.set_state(self.c.STATE_KNOCKDOWN)
|
| 88 |
+
print("\n[๐ฌ 3๋จ๊ณ] ๐ฅ ๊ฝ๋น! ๊ท ํ์ ์๊ณ ์์ผ๋ก ๋์ด์ง๊ธฐ (Knockdown)")
|
| 89 |
+
|
| 90 |
+
# 4๋จ๊ณ: ์ค๋์ด์ฒ๋ผ ๋ค์ ์ผ์ด์๊ธฐ (8.5s ~ 11.0s)
|
| 91 |
+
elif t < 11.0:
|
| 92 |
+
if self.step_idx != 4:
|
| 93 |
+
self.step_idx = 4
|
| 94 |
+
self.c.set_state(self.c.STATE_RECOVER)
|
| 95 |
+
print("\n[๐ฌ 4๋จ๊ณ] ๐ ์ค๋์ด ์ง๋ฅ ๋ฐ๋! ์ค์ค๋ก ๋ฒ๋ก ์ผ์ด์๊ธฐ (Self-Righting)")
|
| 96 |
+
|
| 97 |
+
# 5๋จ๊ณ: ๋ชจ์ด ์ชผ๊ธฐ (11.0s ~ 14.5s)
|
| 98 |
+
elif t < 14.5:
|
| 99 |
+
if self.step_idx != 5:
|
| 100 |
+
self.step_idx = 5
|
| 101 |
+
self.c.set_state(self.c.STATE_GROUND_PICK)
|
| 102 |
+
print("\n[๐ฌ 5๋จ๊ณ] ๐พ ๋ฐ๋ฅ ๋ชจ์ด ์ฝ! ์ฝ! ์ชผ์๋จน๊ธฐ (Pecking)")
|
| 103 |
+
|
| 104 |
+
# 6๋จ๊ณ: ์์๋ค ์ผ์ด์๊ธฐ (14.5s ~ 18.5s)
|
| 105 |
+
elif t < 18.5:
|
| 106 |
+
if self.step_idx != 6:
|
| 107 |
+
self.step_idx = 6
|
| 108 |
+
self.c.set_state(self.c.STATE_SIT)
|
| 109 |
+
print("\n[๐ฌ 6๋จ๊ณ] ๐ฆฟ ๋ค๋ฆฌ๋ฅผ ์ ์ ๊ณ ์ชผ๊ทธ๋ ค ์์๋ค๊ฐ ์ผ์ด์๊ธฐ (Crouch & Stand)")
|
| 110 |
+
if t > 16.8 and self.c.state == self.c.STATE_SIT:
|
| 111 |
+
self.c.set_state(self.c.STATE_STAND)
|
| 112 |
+
|
| 113 |
+
# 7๋จ๊ณ: ๋ ๊ฐ 180ยฐ ์ ๊ฐ & ๊ณต์ค VTOL ์์ง ์ด๋ฅ (18.5s ~ 22.5s)
|
| 114 |
+
elif t < 22.5:
|
| 115 |
+
if self.step_idx != 7:
|
| 116 |
+
self.step_idx = 7
|
| 117 |
+
self.home_x = self.d.qpos[0]
|
| 118 |
+
self.home_y = self.d.qpos[1]
|
| 119 |
+
self.c.target_yaw = 0.0
|
| 120 |
+
self.c.start_flight_sequence()
|
| 121 |
+
print("\n[๐ฌ 7๋จ๊ณ] ๐ชฝ ๋ฑ ๋ค ๋ ๊ฐ 180ยฐ ์ ๊ฐ + ํ๋กํ ๋ฌ ๊ฐ์ โ 1.0m ์์ง ์ด๋ฅ!")
|
| 122 |
+
|
| 123 |
+
# 8๋จ๊ณ: ๐ [3์ธ์นญ ๋ทฐ] ์์ผ๋ก ํ์ฐจ๊ฒ ์ถ๋ฐํ๋ ์ ์ฒด ๋ชจ์ต (22.5s ~ 25.5s)
|
| 124 |
+
elif t < 25.5:
|
| 125 |
+
if self.step_idx != 8:
|
| 126 |
+
self.step_idx = 8
|
| 127 |
+
self.c.look_front()
|
| 128 |
+
self.c.cam_mode = 0 # 3์ธ์นญ ์ ์ฒด ๋ทฐ
|
| 129 |
+
print("\n[๐ฌ 8๋จ๊ณ] ๐ [3์ธ์นญ ์ ์ฒด ๋ทฐ] ๋ ๊ฐ๋ฅผ ํด๊ณ ์์ผ๋ก ํ์ฐจ๊ฒ ์ถ๋ฐํ๋ ์ ์ฒด ๋ชจ์ต")
|
| 130 |
+
self.c.target_y = self.home_y + 0.45
|
| 131 |
+
|
| 132 |
+
# 9๋จ๊ณ: ๐๏ธ [1์ธ์นญ ์ง์ ๋ ์์ - ์ ๋ฐฉ ๋นํ] ๋ก๋ด์ ๋์ผ๋ก ์ง์ ์์ ๋ณด๋ฉฐ ์~ ๋ ์๊ฐ๋ ๋ฆฌ์ผ ์๋๊ฐ! (25.5s ~ 29.5s)
|
| 133 |
+
elif t < 29.5:
|
| 134 |
+
if self.step_idx != 9:
|
| 135 |
+
self.step_idx = 9
|
| 136 |
+
self.c.look_front()
|
| 137 |
+
self.c.cam_mode = 1 # ๐ท [1์ธ์นญ FPV ์ง์ ๋ ์์ ]
|
| 138 |
+
print("\n" + "=" * 75)
|
| 139 |
+
print(" ๐๏ธ [1์ธ์นญ ๋ ์์ ํ๋ฉด - ์ ๋ฐฉ ๋นํ] ๋ก๋ด์ ๋์ผ๋ก ์ง์ ์์ ๋ฐ๋ผ๋ณด๋ฉฐ ์~ ๋ ์๊ฐ๋๋ค!")
|
| 140 |
+
print("=" * 75)
|
| 141 |
+
self.c.target_y = self.home_y + 0.95
|
| 142 |
+
|
| 143 |
+
# 10๋จ๊ณ: ๐๏ธ [3์ธ์นญ ์ ์ฒด ๋ทฐ - ๋์/๋จธ๋ฆฌ๋ฅผ ์๋๋ก ์์ด๋ ๋ชจ์ต] ๋นํ ์ค ๋จธ๋ฆฌ๋ฅผ ์๋๋ก ํน ์์ด๋ ์ธํ ์ฐ์ถ! (29.5s ~ 33.0s)
|
| 144 |
+
elif t < 33.0:
|
| 145 |
+
if self.step_idx != 10:
|
| 146 |
+
self.step_idx = 10
|
| 147 |
+
self.c.cam_mode = 0 # ๐ [3์ธ์นญ ์ ์ฒด ๋ทฐ๋ก ์ ํ!]
|
| 148 |
+
self.c.target_look_down_pitch = 1.05 # ๋จธ๋ฆฌ/๋์ ์๋ 60ยฐ ๊น๊ฒ ์์ด๊ธฐ
|
| 149 |
+
self.c.look_down = True
|
| 150 |
+
print("\n" + "=" * 75)
|
| 151 |
+
print(" ๐๏ธ [3์ธ์นญ ์ ์ฒด ๋ทฐ] ๋นํ ์ค์ธ ๋ก๋ด์๊ฐ ๋จธ๋ฆฌ(๋ ๋)๋ฅผ ์๋๋ก ํน ์์ฌ ๋ฐ๋ฅ์ ๋ด๋ ค๋ค๋ด
๋๋ค!")
|
| 152 |
+
print("=" * 75)
|
| 153 |
+
self.c.target_y = self.home_y + 1.15
|
| 154 |
+
|
| 155 |
+
# 11๋จ๊ณ: ๐ [1์ธ์นญ ์ง์ ๋ ์์ - ํ๋ฐฉ 60ยฐ ๊ด์ธก ํ๋ฉด] ์์ธ ๋์ผ๋ก ๋ฐ๋ฅ ์ฒด์ปค๋ณด๋๋ฅผ ์ง์ ๋ด๋ ค๋ค๋ณด๋ ๋ฆฌ์ผ ์์ผ! (33.0s ~ 37.5s)
|
| 156 |
+
elif t < 37.5:
|
| 157 |
+
if self.step_idx != 11:
|
| 158 |
+
self.step_idx = 11
|
| 159 |
+
self.c.target_look_down_pitch = 1.05
|
| 160 |
+
self.c.look_down = True
|
| 161 |
+
self.c.cam_mode = 1 # ๐ท [1์ธ์นญ FPV ํ๋ฐฉ ์์ ์ฆ์ ์ปท ์ ํ!]
|
| 162 |
+
print("\n" + "=" * 75)
|
| 163 |
+
print(" ๐ [1์ธ์นญ ๋ ์์ ํ๋ฉด - ํ๋ฐฉ 60ยฐ ๊ด์ธก] ๋ก๋ด์์ ๋์ผ๋ก ์ง์ ๋ฐ๋ฅ ์ฒด์ปค๋ณด๋๋ฅผ ์
์
์ด ์ค์บํฉ๋๋ค!")
|
| 164 |
+
print("=" * 75)
|
| 165 |
+
self.c.target_y = self.home_y + 1.25
|
| 166 |
+
|
| 167 |
+
# 12๋จ๊ณ: ๐ [1์ธ์นญ ์ง์ ๋ ์์ - 180ยฐ ์ ํ ์ ํด] ๋์ผ๋ก ์ธ์์ ๋๋ฌ๋ณด๋ฉฐ ์์ ๋ฐฉํฅ์ผ๋ก ์ ํด (37.5s ~ 41.5s)
|
| 168 |
+
elif t < 41.5:
|
| 169 |
+
if self.step_idx != 12:
|
| 170 |
+
self.step_idx = 12
|
| 171 |
+
self.c.look_front() # ๊ณ ๊ฐ ์ ๋ฉด ๋ณต๊ท
|
| 172 |
+
self.c.cam_mode = 1 # 1์ธ์นญ ์์ ์ ์งํ๋ฉฐ ์ ํด
|
| 173 |
+
print("\n[๐ฌ 12๋จ๊ณ] ๐ [1์ธ์นญ ๋ ์์ - 180ยฐ ์ ํด] ๊ณ ๊ฐ๋ฅผ ๋ค๊ณ 180ยฐ ์ ํํ์ฌ ์ถ๋ฐํ๋ ์์ ์ ๋์ผ๋ก ํฌ์ฐฉ!")
|
| 174 |
+
self.c.target_yaw = math.pi * min(1.0, (t - 37.5) / 3.5)
|
| 175 |
+
|
| 176 |
+
# 13๋จ๊ณ: ๐ [3์ธ์นญ ์ ์ฒด ๋ทฐ ๋ณต๊ท] 180๋ ํ์ ํ์ฌ ์๋ ์๋ฆฌ(์์ )๋ก ๊ทํํ๋ ์ ์ฒด ๋ชจ์ต (41.5s ~ 46.0s)
|
| 177 |
+
elif t < 46.0:
|
| 178 |
+
if self.step_idx != 13:
|
| 179 |
+
self.step_idx = 13
|
| 180 |
+
self.c.look_front()
|
| 181 |
+
self.c.cam_mode = 0 # 3์ธ์นญ ์ ์ฒด ๋ทฐ ๋ณต๊ท
|
| 182 |
+
self.c.target_yaw = 0.0 # ๊ธฐ์ฒด ์ ๋ฉด ๋ณต๊ท
|
| 183 |
+
print("\n[๐ฌ 13๋จ๊ณ] ๐ [3์ธ์นญ ์ ์ฒด ๋ทฐ ๋ณต๊ท] 180ยฐ ์ ํ๋ฅผ ๋ง์น๊ณ ๋ฐ๋ํ ์ ์ค์(์์ )์ผ๋ก ์๋ฒฝ ๊ทํ!")
|
| 184 |
+
self.c.target_x = self.home_x
|
| 185 |
+
self.c.target_y = self.home_y
|
| 186 |
+
|
| 187 |
+
# 14๋จ๊ณ: ๐๏ธ [3์ธ์นญ ์ ์ง ํธ๋ฒ๋ง] ์๋ ์ถ๋ฐํ๋ ๊ทธ ์๋ฆฌ ์๊ณต์์ ๊ฐ๋งํ ํธ๋ฒ๋ง (46.0s ~ 50.0s)
|
| 188 |
+
elif t < 50.0:
|
| 189 |
+
if self.step_idx != 14:
|
| 190 |
+
self.step_idx = 14
|
| 191 |
+
self.c.target_x = self.home_x
|
| 192 |
+
self.c.target_y = self.home_y
|
| 193 |
+
print("\n[๐ฌ 14๋จ๊ณ] ๐๏ธ [์ ์๋ฆฌ ์ ์ง ํธ๋ฒ๋ง] ์๋ ์ถ๋ฐํ๋ ๋ฐ๋ก ๊ทธ ์๋ฆฌ ์๊ณต์์ ํผ๋ฒ ์์ ํ ํธ๋ฒ๋ง")
|
| 194 |
+
|
| 195 |
+
# 15๋จ๊ณ: ๐ฌ [์ฌ๋ฟ ํ๊ฐ ์ฐฉ์ง] ์๋ ์ถ๋ฐํ๋ ๊ทธ ์๋ฆฌ์ ์ ํํ ์์ง ์ฐฉ์ง & ๋ ๊ฐ ์ ๊ธฐ (50.0s ~ 56.0s)
|
| 196 |
+
elif t < 56.0:
|
| 197 |
+
if self.step_idx != 15:
|
| 198 |
+
self.step_idx = 15
|
| 199 |
+
self.c.set_state(self.c.STATE_LANDING)
|
| 200 |
+
print("\n[๐ฌ 15๋จ๊ณ] ๐ฌ [์ฌ๋ฟ ํ๊ฐ ์ฐฉ์ง] ์๋ ์ถ๋ฐํ๋ ๊ทธ ์๋ฆฌ์ ์ฌ๋ฟํ ์์ง ์ฐฉ์ง & ๋ ๊ฐ ๋ฑ ๋ค๋ก ์ ๊ธฐ")
|
| 201 |
+
if t > 53.5 and self.c.wings_deployed:
|
| 202 |
+
self.c.wings_deployed = False # ๋ ๊ฐ ๋ฑ ๋ค๋ก ์ ์ ๊ธฐ
|
| 203 |
+
|
| 204 |
+
# ์๋๋ฆฌ์ค ์์ฃผ โ ์ฒ์๋ถํฐ ๋ฌดํ ๋ฐ๋ณต!
|
| 205 |
+
else:
|
| 206 |
+
print("\n" + "โ
" * 75)
|
| 207 |
+
print("๐ [์คํ ๋ฆฌ ์์ฃผ] 3์ธ์นญ ๋จธ๋ฆฌ ์์ โ 1์ธ์นญ ํ๋ฐฉ ์์ โ ์์ ๋ณต๊ท ํ ์คํ ๋ฆฌ๋ณด๋ ์์ฃผ! ๋ค์ ๋ฌดํ ๋ฐ๋ณตํฉ๋๋ค.")
|
| 208 |
+
print("โ
" * 75 + "\n")
|
| 209 |
+
self.timeline = 0.0
|
| 210 |
+
self.step_idx = -1
|
| 211 |
+
|
| 212 |
+
def print_ai_banner():
|
| 213 |
+
print("=" * 80)
|
| 214 |
+
print(" ๐ฌ [๋ฐ๋ ค ๋ก๋ด์ (OpenBird-artnfull)] ๊ณต์ ์๋ค๋งํฑ ์คํ ๋ฆฌ๋ณด๋ ๋ฐ๋ชจ")
|
| 215 |
+
print("=" * 80)
|
| 216 |
+
print(" ๐ ๋ํ๋๊ป์ ์ง์ ๊ธฐํํ์ 13๋จ๊ณ ํ ์คํ ๋ฆฌ๋ณด๋๊ฐ ํผ์ณ์ง๋๋ค:")
|
| 217 |
+
print(" 1. ์์ผ๋ก ๊ฑท๊ธฐ โ 2. ๋ค๋ก ๊ฑท๊ธฐ โ 3. ๊ฝ๋น ๋์ด์ง๊ธฐ โ 4. ์ค๋์ด ๋ฒ๋ก ๊ธฐ๋ฆฝ!")
|
| 218 |
+
print(" 5. ๋ชจ์ด ์ชผ๊ธฐ โ 6. ์์๋ค ์ผ์ด์๊ธฐ โ 7. ๋ ๊ฐ 180ยฐ ์ ๊ฐ & VTOL ์์ง ์ด๋ฅ")
|
| 219 |
+
print(" 8. ์ ์ง ๋นํ โ 9. ๋ ์๋ 55ยฐ ํฅํ๊ธฐ โ 10. ์ง์ ์์ ๋ณด๋ 1์ธ์นญ FPV")
|
| 220 |
+
print(" 11. ์ง์ ์๋๋ฅผ ๋ณด๋ 1์ธ์นญ FPV โ 12. 3์ธ์นญ ์ ์ฒดํ๋ฉด ๋ณต๊ท โ 13. ์ฌ๋ฟ ์ฐฉ์ง & ๋ ๊ฐ ์ ๊ธฐ")
|
| 221 |
+
print("=" * 80 + "\n")
|
| 222 |
+
|
| 223 |
+
def main():
|
| 224 |
+
print_ai_banner()
|
| 225 |
+
|
| 226 |
+
root_dir = os.path.dirname(os.path.abspath(__file__))
|
| 227 |
+
model_path = os.path.join(root_dir, "robot_bird", "models", "robot_bird.xml")
|
| 228 |
+
|
| 229 |
+
if not os.path.exists(model_path):
|
| 230 |
+
print(f"[์ค๋ฅ] ๋ชจ๋ธ ํ์ผ์ ์ฐพ์ ์ ์์ต๋๋ค: {model_path}")
|
| 231 |
+
return
|
| 232 |
+
|
| 233 |
+
with open(model_path, "r", encoding="utf-8") as f:
|
| 234 |
+
xml_string = f.read()
|
| 235 |
+
model = mujoco.MjModel.from_xml_string(xml_string)
|
| 236 |
+
data = mujoco.MjData(model)
|
| 237 |
+
|
| 238 |
+
# ๐ ๊ด์ ์ด๋ฆ ๊ธฐ๋ฐ์ ์์ ํ๊ณ ์ ํํ ์ด๊ธฐ ์์ธ ์ค์ (๋ณดํ ๋ฐ ๋ ๊ฐ ์์ธ 100% ๋ณด์ฅ)
|
| 239 |
+
def set_init_jnt(jnt_name, val):
|
| 240 |
+
adr = model.joint(jnt_name).qposadr[0]
|
| 241 |
+
data.qpos[adr] = val
|
| 242 |
+
|
| 243 |
+
data.qpos[2] = 0.122 # z ๋์ด (์ง๋ฉด ์๋ฒฝ ์์ฐฉ)
|
| 244 |
+
set_init_jnt("left_wing_fold", 0.0)
|
| 245 |
+
set_init_jnt("left_tilt", 1.57)
|
| 246 |
+
set_init_jnt("right_wing_fold", 0.0)
|
| 247 |
+
set_init_jnt("right_tilt", 1.57)
|
| 248 |
+
set_init_jnt("neck_pitch", 0.0)
|
| 249 |
+
for j in ["left_hip_pitch", "left_knee_pitch", "left_ankle_pitch",
|
| 250 |
+
"right_hip_pitch", "right_knee_pitch", "right_ankle_pitch"]:
|
| 251 |
+
set_init_jnt(j, 0.0)
|
| 252 |
+
|
| 253 |
+
data.ctrl[:] = 0.0
|
| 254 |
+
data.ctrl[model.actuator("act_l_tilt").id] = 1.57
|
| 255 |
+
data.ctrl[model.actuator("act_r_tilt").id] = 1.57
|
| 256 |
+
mujoco.mj_forward(model, data)
|
| 257 |
+
|
| 258 |
+
controller = RobotBirdFSM(model, data)
|
| 259 |
+
controller.cam_mode = 0
|
| 260 |
+
director = ShowcaseDirector(controller, model, data)
|
| 261 |
+
recorder = ScreenRecorder(model, data)
|
| 262 |
+
|
| 263 |
+
def viewer_key_callback(keycode):
|
| 264 |
+
if keycode in [ord('Q'), ord('q'), 256, 27]:
|
| 265 |
+
print("\nAI ์๋ฎฌ๋ ์ดํฐ๋ฅผ ์ข
๋ฃํฉ๋๋ค. ์๋
ํ ๊ฐ์ธ์!")
|
| 266 |
+
if recorder.is_recording:
|
| 267 |
+
recorder.stop()
|
| 268 |
+
sys.exit(0)
|
| 269 |
+
elif keycode in [ord('C'), ord('c')]:
|
| 270 |
+
controller.cam_mode = (controller.cam_mode + 1) % 4
|
| 271 |
+
cam_names = [
|
| 272 |
+
"๐๏ธ 3์ธ์นญ ์ ์ฒด ๋ทฐ (์ธ๋ถ์์ ๋ก๋ด์ ๋ชจ์ต ๊ด์ฐฐ)",
|
| 273 |
+
"๐ท [1์ธ์นญ FPV] ๋ก๋ด์ ๋จธ๋ฆฌ ์ ์ค์ ์์ ",
|
| 274 |
+
"๐๏ธ [์ข์ธก ๋ ์ง์ ์์ ] ๋ก๋ด์์ ์ผ์ชฝ ๋์์์ ์ธ์์ ์ง์ ๋ฐ๋ผ๋ณด๋ ์์ !",
|
| 275 |
+
"๐๏ธ [์ฐ์ธก ๋ ์ง์ ์์ ] ๋ก๋ด์์ ์ค๋ฅธ์ชฝ ๋์์์ ์ธ์์ ์ง์ ๋ฐ๋ผ๋ณด๋ ์์ !"
|
| 276 |
+
]
|
| 277 |
+
print(f"\n[๐ท ์์ ์ ํ] ํ์ฌ ์์ : {cam_names[controller.cam_mode]}")
|
| 278 |
+
elif keycode in [ord('V'), ord('v')]:
|
| 279 |
+
controller.toggle_look_down()
|
| 280 |
+
elif keycode in [ord('B'), ord('b')]:
|
| 281 |
+
controller.look_front()
|
| 282 |
+
elif keycode in [ord('X'), ord('x')]:
|
| 283 |
+
recorder.toggle(controller.cam_mode)
|
| 284 |
+
elif keycode in [ord('K'), ord('k')]:
|
| 285 |
+
print("\n[๐ฅ ์ฌ์ฉ์ ๊ฐ์
] ์ฌ์ฉ์๊ฐ ๋ก๋ด์ ํญ ์ณ์ ๋์ด๋จ๋ ธ์ต๋๋ค!")
|
| 286 |
+
data.qvel[0] = 0.35
|
| 287 |
+
data.qvel[3] = 4.5
|
| 288 |
+
data.qvel[4] = 2.0
|
| 289 |
+
controller.set_state(RobotBirdFSM.STATE_KNOCKDOWN)
|
| 290 |
+
|
| 291 |
+
with mujoco.viewer.launch_passive(model, data, key_callback=viewer_key_callback, show_left_ui=False, show_right_ui=False) as viewer:
|
| 292 |
+
try:
|
| 293 |
+
viewer.cam.distance = 1.05
|
| 294 |
+
viewer.cam.elevation = -10
|
| 295 |
+
viewer.cam.azimuth = -80
|
| 296 |
+
|
| 297 |
+
dt = model.opt.timestep if model.opt.timestep > 0 else 0.002
|
| 298 |
+
|
| 299 |
+
while viewer.is_running():
|
| 300 |
+
step_start = time.time()
|
| 301 |
+
|
| 302 |
+
# ๐ฌ ๋ํ๋์ ์คํ ๋ฆฌ๋ณด๋ ๋๋ ํฐ ๊ตฌ๋
|
| 303 |
+
director.update(dt)
|
| 304 |
+
|
| 305 |
+
# ๐ฆ ๋ฌผ๋ฆฌ ์ ์ด๊ธฐ ์
๋ฐ์ดํธ ๋ฐ ๋ฌผ๋ฆฌ ์คํ
์ ์ง
|
| 306 |
+
controller.update(dt)
|
| 307 |
+
mujoco.mj_step(model, data)
|
| 308 |
+
|
| 309 |
+
# ๐ [ํ ํ์์ /๋ฒ๊ฐ์ ์ฐจ๋จ & ๋ฐ๋ฅ ํ
์ค์ฒ/๊ทธ๋ฆฌ๋ 100% ์ ๋ช
ํ๊ฒ ๋ณด์กด]
|
| 310 |
+
try:
|
| 311 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_PERTFORCE] = 0
|
| 312 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = 0
|
| 313 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONSTRAINT] = 0
|
| 314 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = 0
|
| 315 |
+
except Exception:
|
| 316 |
+
pass
|
| 317 |
+
|
| 318 |
+
# ๐ฅ ์ค๋งํธ ์นด๋ฉ๋ผ ์์ ๋๊ธฐํ
|
| 319 |
+
try:
|
| 320 |
+
if controller.cam_mode == 0:
|
| 321 |
+
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
|
| 322 |
+
viewer.cam.lookat[0] = data.qpos[0]
|
| 323 |
+
viewer.cam.lookat[1] = data.qpos[1]
|
| 324 |
+
viewer.cam.lookat[2] = data.qpos[2]
|
| 325 |
+
elif controller.cam_mode == 1:
|
| 326 |
+
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
|
| 327 |
+
viewer.cam.fixedcamid = model.camera("head_fpv_cam").id
|
| 328 |
+
elif controller.cam_mode == 2:
|
| 329 |
+
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
|
| 330 |
+
viewer.cam.fixedcamid = model.camera("left_eye_cam").id
|
| 331 |
+
elif controller.cam_mode == 3:
|
| 332 |
+
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
|
| 333 |
+
viewer.cam.fixedcamid = model.camera("right_eye_cam").id
|
| 334 |
+
except Exception:
|
| 335 |
+
pass
|
| 336 |
+
|
| 337 |
+
# ๐ด ์ค์๊ฐ ๋น๋์ค ํ๋ ์ ์บก์ฒ (๋
นํ ์ค์ผ ๋ ์๋ ๊ธฐ๋ก)
|
| 338 |
+
if recorder.is_recording:
|
| 339 |
+
recorder.capture_frame(controller.cam_mode)
|
| 340 |
+
|
| 341 |
+
viewer.sync()
|
| 342 |
+
|
| 343 |
+
elapsed = time.time() - step_start
|
| 344 |
+
sleep_time = dt - elapsed
|
| 345 |
+
if sleep_time > 0:
|
| 346 |
+
time.sleep(sleep_time)
|
| 347 |
+
|
| 348 |
+
except KeyboardInterrupt:
|
| 349 |
+
print("\n์ฌ์ฉ์์ ์ํด AI ์๋ฎฌ๋ ์ดํฐ๊ฐ ์ข
๋ฃ๋์์ต๋๋ค.")
|
| 350 |
+
finally:
|
| 351 |
+
if recorder.is_recording:
|
| 352 |
+
recorder.stop()
|
| 353 |
+
|
| 354 |
+
if __name__ == "__main__":
|
| 355 |
+
main()
|
run_robot_bird.py
ADDED
|
@@ -0,0 +1,348 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import os
|
| 2 |
+
import sys
|
| 3 |
+
import time
|
| 4 |
+
import math
|
| 5 |
+
import msvcrt
|
| 6 |
+
import mujoco
|
| 7 |
+
import mujoco.viewer
|
| 8 |
+
import numpy as np
|
| 9 |
+
|
| 10 |
+
# UTF-8 ์
์ถ๋ ฅ ๋ณด์ฅ
|
| 11 |
+
if sys.platform == "win32":
|
| 12 |
+
try:
|
| 13 |
+
sys.stdout.reconfigure(encoding='utf-8', errors='replace')
|
| 14 |
+
sys.stderr.reconfigure(encoding='utf-8', errors='replace')
|
| 15 |
+
except Exception:
|
| 16 |
+
pass
|
| 17 |
+
|
| 18 |
+
from robot_bird.controller import RobotBirdFSM
|
| 19 |
+
from robot_bird.recorder import ScreenRecorder
|
| 20 |
+
|
| 21 |
+
def print_banner():
|
| 22 |
+
print("=" * 80)
|
| 23 |
+
print(" ๐ฎ Control Guide (๋ฐ๋ ค ๋ก๋ด์ ์กฐ์ข
ํค ์๋ด)")
|
| 24 |
+
print("=" * 80)
|
| 25 |
+
print(" ๐ [ ์ ํ ํ์ (Yaw Turn) ] : A / D โ ๋จธ๋ฆฌ ์ผ์ชฝ / ์ค๋ฅธ์ชฝ ํ์ (๋นํ ๋ฑ
ํฌํด / ๋ณดํ ํ์ )")
|
| 26 |
+
print(" ๐ [ ์ ์ง / ํ์ง ] : โ / โ (W/S) โ ์ง์: ์ ์ง/ํ์ง ๊ฑท๊ธฐ | ๊ณต์ค: ๋ฐ๋ผ๋ณด๋ ๋ฐฉํฅ ์ ์ง/ํ์ง ๋นํ")
|
| 27 |
+
print(" ๐ [ ํํ ์ด๋ (Strafe) ] : โ / โ โ ํ์ ์์ด ์ข / ์ฐ ํํ ์ฌ๋ผ์ด๋ ์ด๋")
|
| 28 |
+
print(" ๐ [ ๊ณ ๋ ์ ์ด ] : R / F โ ๐ ๊ณ ๋ ์์น (+15cm) | ๐ฌ ๊ณ ๋ ํ๊ฐ (-15cm)")
|
| 29 |
+
print(" ๐ [ ์ ์๋ฆฌ ์ ์ง ] : SPACE โ ๐ ์ง์: ๊ธฐ๋ฆฝ ์ ์ง | ๊ณต์ค: ์นผ ๊ฐ์ ์ ์ง ํธ๋ฒ๋ง")
|
| 30 |
+
print(" ๐ [ ์ํฐ์น ๋นํ ] : 1 โ 2 โ 3 โ ๐ชฝ ๋ ๊ฐ ํผ์น๊ธฐ โ ๐ ์ด๋ฅ/ํธ๋ฒ๋ง โ ๐ฌ ์ฐฉ๋ฅ ๋ฐ ๋ ๊ฐ ์ ๊ธฐ")
|
| 31 |
+
print(" ๐ [ ๐ท ๋ ์นด๋ฉ๋ผ ์์ ] : C โ ๐๏ธ 3์ธ์นญ ๋ทฐ โ ์ค์ 1์ธ์นญ FPV โ ์ข์ธก ๋ โ ์ฐ์ธก ๋")
|
| 32 |
+
print(" ๐ [ ๐ ๋์ ์์ ์กฐ์ ] : V ๋๋ B โ ๐ ๋์ ์๋ 55ยฐ ๋ณด๊ธฐ(V) ๐ ๋ค์ ๋์ ์๋ ์์น๋ก ๋ณต๊ท(V/B)")
|
| 33 |
+
print(" ๐ [ ๐ด ์ค์๊ฐ ์์ ๋
นํ ] : X โ ๐ฌ ํ์ฌ ํ๋ฉด ์ค์๊ฐ ํ์ค H.264 MP4 ๋์์ ๋
นํ ON / OFF")
|
| 34 |
+
print(" ๐ [ ๊ฐ์ฑ ๋ชจ์
& ์ธํฐ๋์
] : G / Y / K / P โ ๐พ ๋ชจ์ด ์ชผ๊ธฐ(G) | ๐ช ์ชผ๊ทธ๋ ค ์๊ธฐ(Y) | ๐ฅ ๋์ด๋จ๋ฆฌ๊ธฐ(K) | ๐ ์ค๋์ด ๊ธฐ๋ฆฝ(P)")
|
| 35 |
+
print(" ๐ [ ์๋ฎฌ๋ ์ดํฐ ์ข
๋ฃ ] : Q / ESC")
|
| 36 |
+
print("=" * 80)
|
| 37 |
+
print("\n์๋ฎฌ๋ ์ดํฐ๋ฅผ ์์ํฉ๋๋ค... (3D ์ฐฝ์ ํด๋ฆญํ๊ณ ๋ฉ์ง๊ฒ ์กฐ์ข
ํด๋ณด์ธ์!)\n")
|
| 38 |
+
|
| 39 |
+
def handle_key_action(keycode, controller, recorder=None):
|
| 40 |
+
"""
|
| 41 |
+
Control Guide ํ์ 100% ์ผ์นํ๋ ์ง๊ด์ ์ธ ํค ํธ๋ค๋ฌ
|
| 42 |
+
"""
|
| 43 |
+
is_flying = controller.state in [
|
| 44 |
+
RobotBirdFSM.STATE_FLIGHT,
|
| 45 |
+
RobotBirdFSM.STATE_TAKEOFF_CLIMB,
|
| 46 |
+
RobotBirdFSM.STATE_TAKEOFF_SPOOL,
|
| 47 |
+
RobotBirdFSM.STATE_LANDING
|
| 48 |
+
]
|
| 49 |
+
|
| 50 |
+
# [ Q / ESC ] : ์๋ฎฌ๋ ์ดํฐ ์ข
๋ฃ
|
| 51 |
+
if keycode in [ord('Q'), ord('q'), 256, 27]:
|
| 52 |
+
print("\n์๋ฎฌ๋ ์ดํฐ๋ฅผ ์ข
๋ฃํฉ๋๋ค. ์๋
ํ ๊ฐ์ธ์!")
|
| 53 |
+
sys.exit(0)
|
| 54 |
+
|
| 55 |
+
# [ 1 ] : ๋ ๊ฐ ํผ์น๊ธฐ / ์ ๊ธฐ
|
| 56 |
+
elif keycode in [ord('1'), 49, 321, ord('E'), ord('e')]:
|
| 57 |
+
controller.toggle_wings()
|
| 58 |
+
|
| 59 |
+
# [ 2 ] : ์ด๋ฅ ๋ฐ 1.0m ์ ์ง ํธ๋ฒ๋ง
|
| 60 |
+
elif keycode in [ord('2'), 50, 322, 257, ord('T'), ord('t')]:
|
| 61 |
+
controller.start_flight_sequence()
|
| 62 |
+
|
| 63 |
+
# [ 3 ] : ์ฐฉ๋ฅ ๋ฐ ๋ ๊ฐ ์ ๊ธฐ
|
| 64 |
+
elif keycode in [ord('3'), 51, 323, ord('H'), ord('h'), 259]:
|
| 65 |
+
controller.set_state(RobotBirdFSM.STATE_LANDING)
|
| 66 |
+
|
| 67 |
+
# [ โ ] / [ W ] : ์ ์ง (์ง์: ์ ์ง ๊ฑท๊ธฐ / ๊ณต์ค: ์ ๋ฐฉ ๋นํ)
|
| 68 |
+
elif keycode in [265, 38, ord('W'), ord('w')]:
|
| 69 |
+
if is_flying:
|
| 70 |
+
yaw = getattr(controller, 'target_yaw', 0.0)
|
| 71 |
+
controller.target_x += 0.30 * (-math.sin(yaw))
|
| 72 |
+
controller.target_y += 0.30 * math.cos(yaw)
|
| 73 |
+
print(f"\n[๐ ์ ์ง ๋นํ] ๋ฐ๋ผ๋ณด๋ ๋ฐฉํฅ์ผ๋ก 30cm ์ ์ง! (X:{controller.target_x:.2f}m, Y:{controller.target_y:.2f}m)")
|
| 74 |
+
else:
|
| 75 |
+
print(f"\n[๐ถ ๋ณดํ ๋ชจ๋] โฌ๏ธ ์์ผ๋ก ์์ฅ์์ฅ ๊ฑท๊ธฐ ์์!")
|
| 76 |
+
controller.walk_speed = 1.0
|
| 77 |
+
controller.walk_turn = 0.0
|
| 78 |
+
controller.set_state(RobotBirdFSM.STATE_WALK)
|
| 79 |
+
|
| 80 |
+
# [ โ ] / [ S ] : ํ์ง (์ง์: ํ์ง ๊ฑท๊ธฐ / ๊ณต์ค: ํ๋ฐฉ ๋นํ)
|
| 81 |
+
elif keycode in [264, 40, ord('S'), ord('s')]:
|
| 82 |
+
if is_flying:
|
| 83 |
+
yaw = getattr(controller, 'target_yaw', 0.0)
|
| 84 |
+
controller.target_x -= 0.30 * (-math.sin(yaw))
|
| 85 |
+
controller.target_y -= 0.30 * math.cos(yaw)
|
| 86 |
+
print(f"\n[๐ฌ ํ์ง ๋นํ] ๋ฐ๋ผ๋ณด๋ ๋ฐ๋๋ก 30cm ํ์ง! (X:{controller.target_x:.2f}m, Y:{controller.target_y:.2f}m)")
|
| 87 |
+
else:
|
| 88 |
+
print(f"\n[๐ถ ๋ณดํ ๋ชจ๋] โฌ๏ธ ๋ค๋ก ์์ฅ์์ฅ ๊ฑท๊ธฐ ์์!")
|
| 89 |
+
controller.walk_speed = -0.7
|
| 90 |
+
controller.walk_turn = 0.0
|
| 91 |
+
controller.set_state(RobotBirdFSM.STATE_WALK)
|
| 92 |
+
|
| 93 |
+
# [ โ ] : ์ข์ธก ํํ ์ด๋ (๊ณต์ค: ์ข ์ฌ๋ผ์ด๋ / ์ง์: ์ขํ์ ๊ฑท๊ธฐ)
|
| 94 |
+
elif keycode in [263, 37]:
|
| 95 |
+
if is_flying:
|
| 96 |
+
yaw = getattr(controller, 'target_yaw', 0.0)
|
| 97 |
+
controller.target_x += 0.30 * math.cos(yaw)
|
| 98 |
+
controller.target_y += 0.30 * math.sin(yaw)
|
| 99 |
+
print(f"\n[โฌ
๏ธ ํํ ์ฌ๋ผ์ด๋] ์ผ์ชฝ์ผ๋ก 30cm ์ด๋! (X:{controller.target_x:.2f}m, Y:{controller.target_y:.2f}m)")
|
| 100 |
+
else:
|
| 101 |
+
print(f"\n[๐ถ ๋ณดํ ๋ชจ๋] โฌ
๏ธ ์ผ์ชฝ์ผ๋ก ๋ฐฉํฅ์ ํ๋ฉฐ ๊ฑท๊ธฐ")
|
| 102 |
+
controller.walk_speed = 0.5
|
| 103 |
+
controller.walk_turn = 0.7
|
| 104 |
+
controller.set_state(RobotBirdFSM.STATE_WALK)
|
| 105 |
+
|
| 106 |
+
# [ โ ] : ์ฐ์ธก ํํ ์ด๋ (๊ณต์ค: ์ฐ ์ฌ๋ผ์ด๋ / ์ง์: ์ฐํ์ ๊ฑท๊ธฐ)
|
| 107 |
+
elif keycode in [262, 39]:
|
| 108 |
+
if is_flying:
|
| 109 |
+
yaw = getattr(controller, 'target_yaw', 0.0)
|
| 110 |
+
controller.target_x -= 0.30 * math.cos(yaw)
|
| 111 |
+
controller.target_y -= 0.30 * math.sin(yaw)
|
| 112 |
+
print(f"\n[โก๏ธ ํํ ์ฌ๋ผ์ด๋] ์ค๋ฅธ์ชฝ์ผ๋ก 30cm ์ด๋! (X:{controller.target_x:.2f}m, Y:{controller.target_y:.2f}m)")
|
| 113 |
+
else:
|
| 114 |
+
print(f"\n[๐ถ ๋ณดํ ๋ชจ๋] โก๏ธ ์ค๋ฅธ์ชฝ์ผ๋ก ๋ฐฉํฅ์ ํ๋ฉฐ ๊ฑท๊ธฐ")
|
| 115 |
+
controller.walk_speed = 0.5
|
| 116 |
+
controller.walk_turn = -0.7
|
| 117 |
+
controller.set_state(RobotBirdFSM.STATE_WALK)
|
| 118 |
+
|
| 119 |
+
# [ A ] : ๋จธ๋ฆฌ ์ผ์ชฝ ํ์ (๊ณต์ค: ์ข์ ํ ํด / ์ง์: ์ขํ์ ๊ฑท๊ธฐ)
|
| 120 |
+
elif keycode in [ord('A'), ord('a')]:
|
| 121 |
+
if is_flying:
|
| 122 |
+
controller.target_yaw = getattr(controller, 'target_yaw', 0.0) - 0.25
|
| 123 |
+
deg = math.degrees(controller.target_yaw) % 360
|
| 124 |
+
print(f"\n[๐ ์ ํ ํ์ ] ๋จธ๋ฆฌ ์ผ์ชฝ์ผ๋ก ํ์ ! (๋ชฉํ ๊ฐ๋: {deg:.1f}ยฐ)")
|
| 125 |
+
else:
|
| 126 |
+
print(f"\n[๐ถ ๋ณดํ ๋ชจ๋] ๐ ์ผ์ชฝ์ผ๋ก ๋ฐฉํฅ์ ํ๋ฉฐ ๊ฑท๊ธฐ")
|
| 127 |
+
controller.walk_speed = 0.5
|
| 128 |
+
controller.walk_turn = 0.7
|
| 129 |
+
controller.set_state(RobotBirdFSM.STATE_WALK)
|
| 130 |
+
|
| 131 |
+
# [ D ] : ๋จธ๋ฆฌ ์ค๋ฅธ์ชฝ ํ์ (๊ณต์ค: ์ฐ์ ํ ํด / ์ง์: ์ฐํ์ ๊ฑท๊ธฐ)
|
| 132 |
+
elif keycode in [ord('D'), ord('d')]:
|
| 133 |
+
if is_flying:
|
| 134 |
+
controller.target_yaw = getattr(controller, 'target_yaw', 0.0) + 0.25
|
| 135 |
+
deg = math.degrees(controller.target_yaw) % 360
|
| 136 |
+
print(f"\n[๐ ์ ํ ํ์ ] ๋จธ๋ฆฌ ์ค๋ฅธ์ชฝ์ผ๋ก ํ์ ! (๋ชฉํ ๊ฐ๋: {deg:.1f}ยฐ)")
|
| 137 |
+
else:
|
| 138 |
+
print(f"\n[๐ถ ๋ณดํ ๋ชจ๋] ๐ ์ค๋ฅธ์ชฝ์ผ๋ก ๋ฐฉํฅ์ ํ๋ฉฐ ๊ฑท๊ธฐ")
|
| 139 |
+
controller.walk_speed = 0.5
|
| 140 |
+
controller.walk_turn = -0.7
|
| 141 |
+
controller.set_state(RobotBirdFSM.STATE_WALK)
|
| 142 |
+
|
| 143 |
+
# [ R ] / [ + ] : ๊ณ ๋ ์์น (+15cm)
|
| 144 |
+
elif keycode in [ord('R'), ord('r'), ord('+'), ord('='), 266, 33, 334]:
|
| 145 |
+
if is_flying:
|
| 146 |
+
controller.target_altitude += 0.15
|
| 147 |
+
print(f"\n[๐ ๊ณ ๋ ์์น] ์๋ก ์์น! (๋ชฉํ ๊ณ ๋: {controller.target_altitude:.2f}m)")
|
| 148 |
+
|
| 149 |
+
# [ F ] / [ - ] : ๊ณ ๋ ํ๊ฐ (-15cm)
|
| 150 |
+
elif keycode in [ord('F'), ord('f'), ord('-'), ord('_'), 267, 34, 333]:
|
| 151 |
+
if is_flying:
|
| 152 |
+
controller.target_altitude = max(0.20, controller.target_altitude - 0.15)
|
| 153 |
+
print(f"\n[๐ฌ ๊ณ ๋ ํ๊ฐ] ์๋๋ก ํ๊ฐ! (๋ชฉํ ๊ณ ๋: {controller.target_altitude:.2f}m)")
|
| 154 |
+
|
| 155 |
+
# [ SPACE ] : ์ ์๋ฆฌ ์ ์ง (์ง์: ๊ธฐ๋ฆฝ ์ ์ง / ๊ณต์ค: ์นผ ๊ฐ์ ์ ์ง ํธ๋ฒ๋ง)
|
| 156 |
+
elif keycode in [ord(' '), 32]:
|
| 157 |
+
if is_flying:
|
| 158 |
+
print(f"\n[๐ ์ ์ง ํธ๋ฒ๋ง] ํ์ฌ ์์น์์ ์นผ ๊ฐ์ด ์ ์ง ํธ๋ฒ๋งํฉ๋๋ค.")
|
| 159 |
+
controller.target_x = controller.data.qpos[0]
|
| 160 |
+
controller.target_y = controller.data.qpos[1]
|
| 161 |
+
else:
|
| 162 |
+
print(f"\n[๐ฆ ๊ธฐ๋ฆฝ ์ ์ง] ๊ฑท๊ธฐ๋ฅผ ๋ฉ์ถ๊ณ ๋ฐ๋ฏํ๊ฒ ์ ์๋ฆฌ์ ์ญ๋๋ค.")
|
| 163 |
+
controller.walk_speed = 0.0
|
| 164 |
+
controller.walk_turn = 0.0
|
| 165 |
+
controller.set_state(RobotBirdFSM.STATE_STAND)
|
| 166 |
+
|
| 167 |
+
# [ C ] : ๋์์ ์ง์ ๋ณด๋ ์์ ์ ํ (3์ธ์นญ ๊ด์ฐฐ โ 1์ธ์นญ ์ผ๊ตด ์ค์ฌ โ ์ข์ธก ๋ ์ง์ ๋ทฐ โ ์ฐ์ธก ๋ ์ง์ ๋ทฐ)
|
| 168 |
+
elif keycode in [ord('C'), ord('c')]:
|
| 169 |
+
if not hasattr(controller, 'cam_mode'):
|
| 170 |
+
controller.cam_mode = 0
|
| 171 |
+
controller.cam_mode = (controller.cam_mode + 1) % 4
|
| 172 |
+
cam_names = [
|
| 173 |
+
"๐๏ธ 3์ธ์นญ ์ ์ฒด ๋ทฐ (์ธ๋ถ์์ ๋ก๋ด์ ๋ชจ์ต ๊ด์ฐฐ)",
|
| 174 |
+
"๐ท [1์ธ์นญ FPV] ๋ก๋ด์ ๋จธ๋ฆฌ ์ ์ค์ ์์ ",
|
| 175 |
+
"๐๏ธ [์ข์ธก ๋ ์ง์ ์์ ] ๋ก๋ด์์ ์ผ์ชฝ ๋์์์ ์ธ์์ ์ง์ ๋ฐ๋ผ๋ณด๋ ์์ !",
|
| 176 |
+
"๐๏ธ [์ฐ์ธก ๋ ์ง์ ์์ ] ๋ก๋ด์์ ์ค๋ฅธ์ชฝ ๋์์์ ์ธ์์ ์ง์ ๋ฐ๋ผ๋ณด๋ ์์ !"
|
| 177 |
+
]
|
| 178 |
+
print(f"\n[๐ท ์์ ์ ํ] ํ์ฌ ์์ : {cam_names[controller.cam_mode]}")
|
| 179 |
+
|
| 180 |
+
# [ V ] : ๋จธ๋ฆฌ/๋์ ํ๋ฐฉ ์์ ํ ๊ธ (์๋ 55ยฐ ๋ณด๊ธฐ <-> ์ ๋ฉด ๋๋ฐ๋ก ๋ณด๊ธฐ)
|
| 181 |
+
elif keycode in [ord('V'), ord('v')]:
|
| 182 |
+
controller.toggle_look_down()
|
| 183 |
+
|
| 184 |
+
# [ B ] : ๋จธ๋ฆฌ/๋์ ์ฆ์ ์ ๋ฉด ์ํ ์์ ์ผ๋ก ๋ณต๊ท
|
| 185 |
+
elif keycode in [ord('B'), ord('b')]:
|
| 186 |
+
controller.look_front()
|
| 187 |
+
|
| 188 |
+
# [ X ] : ์ค์๊ฐ ํ์ค H.264 MP4 ๋น๋์ค ๋
นํ ON / OFF ํ ๊ธ
|
| 189 |
+
elif keycode in [ord('X'), ord('x')]:
|
| 190 |
+
if recorder is not None:
|
| 191 |
+
cam_mode = getattr(controller, 'cam_mode', 0)
|
| 192 |
+
recorder.toggle(cam_mode)
|
| 193 |
+
|
| 194 |
+
# [ G ] : ๋ค๋ฆฌ ์
ํฌ๋ฆผ 3๋จ ๋ชจ์ด ์ชผ๊ธฐ
|
| 195 |
+
elif keycode in [ord('G'), ord('g')]:
|
| 196 |
+
print(f"\n[๐พ ๊ฐ์ฑ ๋ชจ์
] ๋ค๋ฆฌ๋ฅผ ์ ์ ์ผ๋ฉฐ ๋ฐ๋ฅ์ ์ฝ! ์ฝ! ์ฝ! 3๋จ ๋ชจ์ด ์ชผ๊ธฐ!")
|
| 197 |
+
controller.set_state(RobotBirdFSM.STATE_GROUND_PICK)
|
| 198 |
+
|
| 199 |
+
# [ Y ] : ์ชผ๊ทธ๋ ค ์๊ธฐ / ์ผ์ด์๊ธฐ ํ ๊ธ
|
| 200 |
+
elif keycode in [ord('Y'), ord('y')]:
|
| 201 |
+
if controller.state == RobotBirdFSM.STATE_SIT:
|
| 202 |
+
print(f"\n[๐ช ๊ฐ์ฑ ๋ชจ์
] ์ชผ๊ทธ๋ ค ์๊ธฐ์์ ๋ค์ ์ผ์ด์ญ๋๋ค.")
|
| 203 |
+
controller.set_state(RobotBirdFSM.STATE_STAND)
|
| 204 |
+
else:
|
| 205 |
+
print(f"\n[๐ช ๊ฐ์ฑ ๋ชจ์
] ๋ค๋ฆฌ๋ฅผ ์ ์ ๊ณ ์์ ํ ์ชผ๊ทธ๋ ค ์์ต๋๋ค.")
|
| 206 |
+
controller.set_state(RobotBirdFSM.STATE_SIT)
|
| 207 |
+
|
| 208 |
+
# [ K ] : ๊ฝ๋น ๋์ด๋จ๋ฆฌ๊ธฐ ํ
์คํธ
|
| 209 |
+
elif keycode in [ord('K'), ord('k'), ord('O'), ord('o')]:
|
| 210 |
+
print(f"\n[๐ฅ ๋์ด๋จ๋ฆฌ๊ธฐ] ๊ฝ๋น! ๋ก๋ด์ด ๋์ ์ต๋๋ค. ('P' ํค๋ฅผ ๋๋ฅด๋ฉด ๋ฒ๋ก ์ผ์ด๋ฉ๋๋ค!)")
|
| 211 |
+
controller.data.qvel[0] = 0.35
|
| 212 |
+
controller.data.qvel[3] = 4.5
|
| 213 |
+
controller.data.qvel[4] = 2.0
|
| 214 |
+
controller.set_state(RobotBirdFSM.STATE_KNOCKDOWN)
|
| 215 |
+
|
| 216 |
+
# [ P ] : 3D ์ค๋์ด ๋ฒ๋ก ๊ธฐ๋ฆฝ (๋์์์ ๋: ๊ธฐ๋ฆฝ / ์ ์์ ๋: ์ฌ๋กฑ ์ ํ ๋์ค)
|
| 217 |
+
elif keycode in [ord('P'), ord('p')]:
|
| 218 |
+
print(f"\n[๐ ์ค๋์ด ๊ธฐ๋ฆฝ] ๋ฒ๋ก ์ผ์ด์๊ธฐ & ๊ฐธ์ฐ๋ฑ ์ฌ๋กฑ ๋์ค!")
|
| 219 |
+
controller.set_state(RobotBirdFSM.STATE_RECOVER)
|
| 220 |
+
|
| 221 |
+
def main():
|
| 222 |
+
print_banner()
|
| 223 |
+
|
| 224 |
+
root_dir = os.path.dirname(os.path.abspath(__file__))
|
| 225 |
+
model_path = os.path.join(root_dir, "robot_bird", "models", "robot_bird.xml")
|
| 226 |
+
|
| 227 |
+
if not os.path.exists(model_path):
|
| 228 |
+
print(f"[์ค๋ฅ] ๋ชจ๋ธ ํ์ผ์ ์ฐพ์ ์ ์์ต๋๋ค: {model_path}")
|
| 229 |
+
return
|
| 230 |
+
|
| 231 |
+
with open(model_path, "r", encoding="utf-8") as f:
|
| 232 |
+
xml_string = f.read()
|
| 233 |
+
model = mujoco.MjModel.from_xml_string(xml_string)
|
| 234 |
+
data = mujoco.MjData(model)
|
| 235 |
+
|
| 236 |
+
# ๐ ๊ด์ ์ด๋ฆ ๊ธฐ๋ฐ์ ์์ ํ๊ณ ์ ํํ ์ด๊ธฐ ์์ธ ์ค์ (๋ณดํ ๋ฐ ๋ ๊ฐ ์์ธ 100% ๋ณด์ฅ)
|
| 237 |
+
def set_init_jnt(jnt_name, val):
|
| 238 |
+
adr = model.joint(jnt_name).qposadr[0]
|
| 239 |
+
data.qpos[adr] = val
|
| 240 |
+
|
| 241 |
+
data.qpos[2] = 0.122 # z ๋์ด (์ง๋ฉด ์๋ฒฝ ์์ฐฉ)
|
| 242 |
+
set_init_jnt("left_wing_fold", 0.0)
|
| 243 |
+
set_init_jnt("left_tilt", 1.57)
|
| 244 |
+
set_init_jnt("right_wing_fold", 0.0)
|
| 245 |
+
set_init_jnt("right_tilt", 1.57)
|
| 246 |
+
set_init_jnt("neck_pitch", 0.0)
|
| 247 |
+
for j in ["left_hip_pitch", "left_knee_pitch", "left_ankle_pitch",
|
| 248 |
+
"right_hip_pitch", "right_knee_pitch", "right_ankle_pitch"]:
|
| 249 |
+
set_init_jnt(j, 0.0)
|
| 250 |
+
|
| 251 |
+
data.ctrl[:] = 0.0
|
| 252 |
+
data.ctrl[model.actuator("act_l_tilt").id] = 1.57
|
| 253 |
+
data.ctrl[model.actuator("act_r_tilt").id] = 1.57
|
| 254 |
+
mujoco.mj_forward(model, data)
|
| 255 |
+
|
| 256 |
+
controller = RobotBirdFSM(model, data)
|
| 257 |
+
recorder = ScreenRecorder(model, data)
|
| 258 |
+
|
| 259 |
+
def viewer_key_callback(keycode):
|
| 260 |
+
handle_key_action(keycode, controller, recorder)
|
| 261 |
+
|
| 262 |
+
with mujoco.viewer.launch_passive(model, data, key_callback=viewer_key_callback, show_left_ui=False, show_right_ui=False) as viewer:
|
| 263 |
+
try:
|
| 264 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = 0
|
| 265 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = 0
|
| 266 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONSTRAINT] = 0
|
| 267 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_PERTFORCE] = 0
|
| 268 |
+
except Exception:
|
| 269 |
+
pass
|
| 270 |
+
|
| 271 |
+
while viewer.is_running():
|
| 272 |
+
step_start = time.time()
|
| 273 |
+
|
| 274 |
+
if msvcrt.kbhit():
|
| 275 |
+
key = msvcrt.getch()
|
| 276 |
+
if key in [b'\x00', b'\xe0']:
|
| 277 |
+
sub = msvcrt.getch()
|
| 278 |
+
if sub == b'H': handle_key_action(265, controller, recorder) # UP Arrow
|
| 279 |
+
elif sub == b'P': handle_key_action(264, controller, recorder) # DOWN Arrow
|
| 280 |
+
elif sub == b'K': handle_key_action(263, controller, recorder) # LEFT Arrow
|
| 281 |
+
elif sub == b'M': handle_key_action(262, controller, recorder) # RIGHT Arrow
|
| 282 |
+
else:
|
| 283 |
+
try:
|
| 284 |
+
k_ord = ord(key.decode('utf-8', errors='ignore'))
|
| 285 |
+
handle_key_action(k_ord, controller, recorder)
|
| 286 |
+
if k_ord in [ord('q'), ord('Q'), 27]:
|
| 287 |
+
break
|
| 288 |
+
except Exception:
|
| 289 |
+
pass
|
| 290 |
+
|
| 291 |
+
controller.cmd_pitch *= 0.95
|
| 292 |
+
controller.cmd_roll *= 0.95
|
| 293 |
+
controller.cmd_yaw *= 0.95
|
| 294 |
+
|
| 295 |
+
dt = model.opt.timestep
|
| 296 |
+
controller.update(dt)
|
| 297 |
+
mujoco.mj_step(model, data)
|
| 298 |
+
|
| 299 |
+
# ๐ [ํ ํ์์ /๋ฒ๊ฐ์ ์ฐจ๋จ & ๋ฐ๋ฅ ํ
์ค์ฒ/๊ทธ๋ฆฌ๋ 100% ์ ๋ช
ํ๊ฒ ๋ณด์กด]
|
| 300 |
+
try:
|
| 301 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_PERTFORCE] = 0
|
| 302 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = 0
|
| 303 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONSTRAINT] = 0
|
| 304 |
+
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = 0
|
| 305 |
+
except Exception:
|
| 306 |
+
pass
|
| 307 |
+
|
| 308 |
+
# ๐ [์ค๋งํธ ์นด๋ฉ๋ผ ์์ ์ ์ด] (C ํค๋ก 3์ธ์นญ <-> 1์ธ์นญ FPV / ๋ ์นด๋ฉ๋ผ ์ ํ)
|
| 309 |
+
cam_mode = getattr(controller, 'cam_mode', 0)
|
| 310 |
+
try:
|
| 311 |
+
if cam_mode == 0:
|
| 312 |
+
# 3์ธ์นญ ์ค๋งํธ ์ถ์ ๋ทฐ
|
| 313 |
+
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
|
| 314 |
+
viewer.cam.lookat[0] = data.qpos[0]
|
| 315 |
+
viewer.cam.lookat[1] = data.qpos[1]
|
| 316 |
+
viewer.cam.lookat[2] = data.qpos[2]
|
| 317 |
+
elif cam_mode == 1:
|
| 318 |
+
# ๐ท ์ค์ FPV ์นด๋ฉ๋ผ
|
| 319 |
+
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
|
| 320 |
+
viewer.cam.fixedcamid = model.camera("head_fpv_cam").id
|
| 321 |
+
elif cam_mode == 2:
|
| 322 |
+
# ๐๏ธ ์ข์ธก ๋ ์นด๋ฉ๋ผ
|
| 323 |
+
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
|
| 324 |
+
viewer.cam.fixedcamid = model.camera("left_eye_cam").id
|
| 325 |
+
elif cam_mode == 3:
|
| 326 |
+
# ๐๏ธ ์ฐ์ธก ๋ ์นด๋ฉ๋ผ
|
| 327 |
+
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
|
| 328 |
+
viewer.cam.fixedcamid = model.camera("right_eye_cam").id
|
| 329 |
+
except Exception:
|
| 330 |
+
pass
|
| 331 |
+
|
| 332 |
+
# ๐ด ์ค์๊ฐ ๋น๋์ค ํ๋ ์ ์บก์ฒ (๋
นํ ์ค์ผ ๋ ์๋ ๊ธฐ๋ก)
|
| 333 |
+
if recorder.is_recording:
|
| 334 |
+
recorder.capture_frame(cam_mode)
|
| 335 |
+
|
| 336 |
+
viewer.sync()
|
| 337 |
+
|
| 338 |
+
elapsed = time.time() - step_start
|
| 339 |
+
sleep_time = max(0.0, 0.004 - elapsed)
|
| 340 |
+
if sleep_time > 0:
|
| 341 |
+
time.sleep(sleep_time)
|
| 342 |
+
|
| 343 |
+
# ํ๋ก๊ทธ๋จ ์ข
๋ฃ ์ ๋
นํ ์๋ ๋ง๋ฌด๋ฆฌ ์ ์ฅ
|
| 344 |
+
if recorder.is_recording:
|
| 345 |
+
recorder.stop()
|
| 346 |
+
|
| 347 |
+
if __name__ == "__main__":
|
| 348 |
+
main()
|
videos/eval_video.gif
ADDED
|
Git LFS Details
|
videos/eval_video.mp4
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
version https://git-lfs.github.com/spec/v1
|
| 2 |
+
oid sha256:5d2b237b35896aa215bd1cae710dffbbb603544e28496698b8f389daf8750454
|
| 3 |
+
size 1109526
|
๋ฏธ๋ฆฌ๋ณด๊ธฐ_๋ฐ๋ชจ.bat
ADDED
|
@@ -0,0 +1,10 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
@echo off
|
| 2 |
+
cd /d "%~dp0"
|
| 3 |
+
if exist "..\duckenv\Scripts\python.exe" (
|
| 4 |
+
..\duckenv\Scripts\python.exe run_ai_robot_bird.py
|
| 5 |
+
) else if exist ".\duckenv\Scripts\python.exe" (
|
| 6 |
+
.\duckenv\Scripts\python.exe run_ai_robot_bird.py
|
| 7 |
+
) else (
|
| 8 |
+
python run_ai_robot_bird.py
|
| 9 |
+
)
|
| 10 |
+
if errorlevel 1 pause
|