Genesisで小型二足歩行ロボットを学習させてみる

![SS 192.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/565043/a41cd967-f549-8677-1efa-2e99bf69518d.png) 一体どうすれば自作ロボがうごくんだよう

# やること

物理シミュレータの[Genesis](https://genesis-embodied-ai.github.io/)を使って、自作のURDFを導入し、学習させてみます。
小型二足歩行ロボットを試します。
[holypong氏](https://x.com/automo_emo)に聞いた方法を記事にまとめたものです。

# Genesisの導入

下記の記事などをご参考にGenesisが動作するところまでご準備ください。

https://note.com/npaka/n/nbee82a5d5374

https://qiita.com/tamashu/items/53b07627bef29eb5dd02

https://qiita.com/hEnka/items/cc5fd872eb0bf7cd3abc

# オリジナルモデルを動かす

## STEP 1: ディレクトリ構造を確認する

```
Genesis
 ├ CONTRIBUTINE.md
 ├ ...
 ├ examples
 ├ genesis
 │　　├　...
 │　　└ assets
 │　　   ├ meshes
 │　　   ├ textures
 │　　   ├ urdf
 │　　   └ xml
 └....
```

Genesisをインストールすると、そのディレクトリは上記のような構造になっていると思います。
独自のURDFモデルは下記の位置に配置するようにします。

```
Genesis
 └ genesis
  　　└ assets
  　　   └ urdf
  　　      └ xxxx_description #自作URDFのディレクトリ
              ├ meshes  
              │   └ xxx.stl   #自作のmeshファイル達
              └ urdf    
                  └ xxx.urdf      #自作の.urdfファイル
```

## STEP 2: .urdfファイルを確認、調整する

xxx.urdfファイル内のjointのパラメータのlimitについて、effortやvelocityの値が含まれるようにします。この項目が未設定や0であるとモデルが歩きません。

```xml:例
<joint name="l_shoulder_pitch" type="revolute">
    <origin xyz="0.01145 0.04885 0.0454" rpy="0.0 0.0 0.0"/>
    <axis xyz="0 1 0"/>
    <parent link="c_chest"/>
    <child link="l_shoulder"/>
    <limit lower="-3.14159" upper="3.14159" effort="1.372" velocity="7.4762"/>
</joint>
```


- **effort**（力またはトルク）
単位: ニュートン (N) または ニュートン・メートル (N·m)
直線関節（prismatic）: 最大推力（N）
回転関節（revolute）: 最大トルク（N·m）

  
- **velocity**（速度）
    単位: メートル毎秒 (m/s) または ラジアン毎秒 (rad/s)
        直線関節（prismatic）: 最大速度（m/s）
        回転関節（revolute）: 最大角速度（rad/s）

たとえば、KRS-2552R2HV ICSサーボであれば、カタログスペックで最大トルク14.0kgf・cm、最高スピード：0.14s/60°とあるので、変換された値は、
- effort = 1.372938（N·m）
- velocity = 7.48 (rad/s)
となります。
 


トルク変換

https://www.unipulse.tokyo/techinfo/toruquhenkan/

回転速度変換（参考）

https://corelinks.co.jp/useful/unit-conversion/rotate/

また、Jointは後の機械学習設定で、学習に使用すると指定すると回転しますが、学習の対象にせずrevoluteのままの場合はぶらんとなります。
動きの範囲を絞るために学習させず固定にする場合などは、URDF上でtype="fixed"に指定するとよいです。

## STEP 3: 機械学習用のファイルを準備する

```shell:例
Genesis
 ├ ...
 ├ examples
 │　　├ locomotions
 │　　└ warking          # 任意のディレクトリ名
 │       ├ go2_env.py   #locomotions内のファイルをコピペ
 │       ├ go2_eval.py  #locomotions内のファイルをコピペ
 │       └ go2_train.py #locomotions内のファイルをコピペ 

```

locomotionsの中にあるgo2_env.py, go2_eval.py, go2_train.py の3つのファイルを、同じ階層に作成したディレクトリにコピペします。

go2_env.pyが条件を設定するファイルです。その設定内容に従って、go2_train.pyで機械学習を実行、go2_eval.pyで評価（表示）をおこなっているようです。


## STEP 4: go2_env.pyを修正する

```python
class Go2Env:
    def __init__(self, num_envs, env_cfg, obs_cfg, reward_cfg, 
                 command_cfg, show_viewer=True, device="cuda"):
```
↑ 修正① show_viewerをTrueにすると学習中にレンダリング表示が行われます。動作状況が見てわかりやすくなります。



```python
        # add robot
        self.base_init_pos = torch.tensor(self.env_cfg["base_init_pos"], device=self.device)
        self.base_init_quat = torch.tensor(self.env_cfg["base_init_quat"], device=self.device)
        self.inv_base_init_quat = inv_quat(self.base_init_quat)
        self.robot = self.scene.add_entity(
            gs.morphs.URDF(
                #.urdfファイルの場所をフルパスでいれます
                file="/home/xxx/Genesis/genesis/assets/urdf/xxx_description/urdf/xxx.urdf",
                pos=self.base_init_pos.cpu().numpy(),
                quat=self.base_init_quat.cpu().numpy(),
            ),
```
↑ 修正② .urdfファイルの置き場所をフルパスで指定します。


## STEP 5: go2_train.pyを修正する

def get_cfgs(): のところを設定します。
とりあえず関数全文を載せておきますので、コメントを参考に必要な箇所を書き換えてください。

```python
def get_cfgs():
    env_cfg = {
        "num_actions": 12, #強化学習する関節の数は12と決まっているらしい
        # joint/link names
        "default_joint_angles": {  # [rad]
            "FL_hip_joint": 0.0, #それぞれURDFで設定したjoint名にする
            "FR_hip_joint": 0.0,
            "RL_hip_joint": 0.0,
            "RR_hip_joint": 0.0,
            "FL_thigh_joint": 0.8, #この0.8という数字は,初期位置
            "FR_thigh_joint": 0.8,
            "RL_thigh_joint": 1.0,
            "RR_thigh_joint": 1.0,
            "FL_calf_joint": -1.5,
            "FR_calf_joint": -1.5,
            "RL_calf_joint": -1.5,
            "RR_calf_joint": -1.5,
        },
        "dof_names": [
            "FR_hip_joint", #default_joint_anglesの名前に合わせて変更
            "FR_thigh_joint",
            "FR_calf_joint",
            "FL_hip_joint",
            "FL_thigh_joint",
            "FL_calf_joint",
            "RR_hip_joint",
            "RR_thigh_joint",
            "RR_calf_joint",
            "RL_hip_joint",
            "RL_thigh_joint",
            "RL_calf_joint",
        ],
        # PD
        "kp": 20.0,
        "kd": 0.5,
        # termination
        "termination_if_roll_greater_than": 10,  # degree
        "termination_if_pitch_greater_than": 10,
        # base pose
        "base_init_pos": [0.0, 0.0, 0.42],      #本体の初期高さ
        "base_init_quat": [1.0, 0.0, 0.0, 0.0], #本体の初期角度
        "episode_length_s": 20.0,
        "resampling_time_s": 4.0,
        "action_scale": 0.25,
        "simulate_action_latency": True,
        "clip_actions": 100.0,
    }
    obs_cfg = {
        "num_obs": 45,
        "obs_scales": {
            "lin_vel": 2.0,
            "ang_vel": 0.25,
            "dof_pos": 1.0,
            "dof_vel": 0.05,
        },
    }
    reward_cfg = {
        "tracking_sigma": 0.25,
        "base_height_target": 0.3,
        "feet_height_target": 0.075,
        "reward_scales": {
            "tracking_lin_vel": 1.0,
            "tracking_ang_vel": 0.2,
            "lin_vel_z": -1.0,
            "base_height": -50.0,
            "action_rate": -0.005,
            "similar_to_default": -0.1,
        },
    }
    command_cfg = {
        "num_commands": 3,
        "lin_vel_x_range": [0.5, 0.5],
        "lin_vel_y_range": [0, 0],
        "ang_vel_range": [0, 0],
    }

    return env_cfg, obs_cfg, reward_cfg, command_cfg
```
- 使用するjoint名をURDFで指定したものに合わせて書き換えます。  
- dof_namesも上記に合わせます
- base_init_posで本体の初期高さを調整します（高すぎる場所から落ちぬよう）


## STEP 6: 学習を実行する
```shell
$ cd (修正したgo2_train.pyを含むディレクトリ)
$ python go2_train.py
```

初回の起動はある程度の時間がかかりますので少し不安になるかもしれません。
待ち続けるとコンソールのカーソルの点滅が止まり、やがて描画が始まります。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/565043/8532362f-2dbc-d12a-9588-7ed2bb778033.png" width = 500>

学習が完了したら、
```shell
$ python go2_eval.py
```

で学習結果のアニメーションを表示することができます。

## これから

他にもパラメータを設定できる箇所があるので、なるべく実機に近づけられればと思います。
報酬についてもさまざまな調整ができそうですが、そのあたりはまだ全くわかりません。
（たとえば胴体の角度が垂直であるほど報酬が高いなど）