MyoSuite Tutorial No.6

:::note alert
未解決問題あり
原因、対処法わかる方いればコメント欄にて教えていただけますと助かります
:::


# はじめに
本記事は以下の人を対象としています．

- 筋骨格シミュレータに興味がある人
- MuJoCoに興味がある人

:::note warn
本記事では[6_Inverse_Dynamics.ipynb](https://github.com/MyoHub/myosuite/blob/main/docs/source/tutorials/6_Inverse_Dynamics.ipynb)をGoogle Colabで実行した際の手順と結果に関してまとめる．
:::

# MyoSuiteとは

https://sites.google.com/view/myosuite

MuJoCoを学習モジュールとして組み込んだ筋骨格シミュレータ

:::note info
2024年にICRAにてworkshopを展開
https://sites.google.com/view/myosuite/myosymposium/icra24
:::

# Tutorial Notebook

https://github.com/MyoHub/myosuite/tree/main/docs/source/tutorials

# 実施事項
Tutorialに記載のコードを1つずつ試して, Errorになったところは都度修正版を記載する.

## ライブラリのinstallと環境変数設定

```bash
!pip install -U myosuite
!pip install tabulate matplotlib torch gym==0.13 git+https://github.com/aravindr93/mjrl.git@pvr_beta_1vk
!pip install scikit-learn

%env MUJOCO_GL=egl
```

## ライブラリのimportと関数設定

```python
from myosuite.simhive.myo_sim.test_sims import TestSims as loader
from IPython.display import HTML
import matplotlib.pyplot as plt
from base64 import b64encode
import scipy.sparse as spa
from copy import deepcopy
from tqdm import tqdm
import pandas as pd
import numpy as np
import skvideo.io
import mujoco
import osqp
import os

def show_video(video_path, video_width = 400):
    """
    Display a video within the notebook.
    """
    video_file = open(video_path, "r+b").read()
    video_url = f"data:video/mp4;base64,{b64encode(video_file).decode()}"
    return HTML(f"""<video autoplay width={video_width} controls><source src="{video_url}"></video>""")

def solve_qp(P, q, lb, ub, x0):
    """
    Solve a quadratic program.
    """
    P = spa.csc_matrix(P)
    A = spa.csc_matrix(spa.eye(q.shape[0]))
    m = osqp.OSQP()
    m.setup(P=P, q=q, A=A, l=lb, u=ub, verbose=False)
    m.warm_start(x=x0)
    res = m.solve()
    return res.x

def plot_qxxx(qxxx, joint_names, labels):
    """
    Plot generalized variables to be compared.
    qxxx[:,0,-1] = time axis
    qxxx[:,1:,n] = n-th sequence
    qxxx[:,1:,-1] = reference sequence
    """
    fig, axs = plt.subplots(4, 6, figsize=(12, 8))
    axs = axs.flatten()
    line_objects = []
    linestyle = ['-'] * qxxx.shape[2]
    linestyle[-1] = '--'
    for j in range(1, len(joint_names)+1):
        ax = axs[j-1]
        for i in range(qxxx.shape[2]):
            line, = ax.plot(qxxx[:, 0, -1], qxxx[:, j, i], linestyle[i])
            if j == 1: # add only one set of lines to the legend
                line_objects.append(line)
        ax.set_xlim([qxxx[:, 0].min(), qxxx[:, 0].max()])
        ax.set_ylim([qxxx[:, 1:, :].min(), qxxx[:, 1:, :].max()])
        ax.set_title(joint_names[j-1])
    legend_ax = axs[len(joint_names)] # create legend in the 24th subplot area
    legend_ax.axis('off')
    legend_ax.legend(line_objects, labels, loc='center')
    plt.tight_layout()
    plt.show()

def plot_uxxx(uxxx, muscle_names, labels):
    """
    Plot actuator variables to be compared.
    uxxx[:,0,-1] = time axis
    uxxx[:,1:,n] = n-th sequence
    """
    fig, axs = plt.subplots(5, 8, figsize=(12, 8))
    axs = axs.flatten()
    line_objects = []
    for j in range(1, len(muscle_names)+1):
        ax = axs[j-1]
        for i in range(uxxx.shape[2]):
            line, = ax.plot(uxxx[:, 0, -1], uxxx[:, j, i])
            if j == 1: # add only one set of lines to the legend
                line_objects.append(line)
        ax.set_xlim([uxxx[:, 0].min(), uxxx[:, 0].max()])
        ax.set_ylim([uxxx[:, 1:, :].min(), uxxx[:, 1:, :].max()])
        ax.set_title(muscle_names[j-1])
    legend_ax = axs[len(muscle_names)] # create legend in the 40th subplot area
    legend_ax.axis('off')
    legend_ax.legend(line_objects, labels, loc='center')
    plt.tight_layout()
    plt.show()
```

## trajectoryのloadと一般化力の計算

```python
traj = pd.read_csv('data/6_trajectory.csv').values

def get_qfrc(model, data, target_qpos):
    """
    Compute the generalized force needed to reach the target position in the next mujoco step.
    """
    data_copy = deepcopy(data)
    data_copy.qacc = (((target_qpos - data.qpos) / model.opt.timestep) - data.qvel) / model.opt.timestep
    model.opt.disableflags += mujoco.mjtDisableBit.mjDSBL_CONSTRAINT
    mujoco.mj_inverse(model, data_copy)
    model.opt.disableflags -= mujoco.mjtDisableBit.mjDSBL_CONSTRAINT
    return data_copy.qfrc_inverse

# --- ここのblockでセルがcrachする ---
model0 = loader.get_sim(None, 'hand/myohand.xml')
data0 = mujoco.MjData(model0)
qpos_eval = np.zeros((traj.shape[0], traj.shape[1], 2))
qpos_eval[:,:,-1] = traj
for idx in tqdm(range(traj.shape[0])):
    target_qpos = traj[idx, 1:]
    qfrc = get_qfrc(model0, data0, target_qpos)
    data0.qfrc_applied = qfrc
    mujoco.mj_step(model0, data0)
    qpos_eval[idx,:,0] = np.hstack((data0.time, data0.qpos))
error = ((qpos_eval[:,1:,0] - qpos_eval[:,1:,-1])**2).mean(axis=0)
print(f'error max (rad): {error.max()}')
joint_names = [model0.joint(i).name for i in range(model0.nq)]
plot_qxxx(qpos_eval, joint_names, ['Achieved qpos', 'Reference qpos'])
# ------

all_qfrc_scaler = [10, 100, 1000]
qpos_eval = np.zeros((traj.shape[0], traj.shape[1], len(all_qfrc_scaler)+1))
qpos_eval[:,:,-1] = traj
labels = []
for i_scaler, scaler in enumerate(all_qfrc_scaler):
    data0 = mujoco.MjData(model0)
    for idx in tqdm(range(traj.shape[0])):
        target_qpos = traj[idx, 1:]
        qfrc = get_qfrc(model0, data0, target_qpos)
        data0.qfrc_applied = qfrc/scaler
        mujoco.mj_step(model0, data0)
        qpos_eval[idx,:,i_scaler] = np.hstack((data0.time, data0.qpos))
    error = ((qpos_eval[:,1:,i_scaler] - qpos_eval[:,1:,-1])**2).mean(axis=0)
    print(f'qfrc scaler: {scaler} - error max (rad): {error.max()}')
    labels.append(f'Achieved qpos\nscaler:{scaler}')
labels.append('Reference qpos')
plot_qxxx(qpos_eval, joint_names, labels)
```

:::note alert
力の計算のところでgoogle colabではセルがcrashするため進めない
localにcloneしてきてjupyter notebookで起動しても同様
:::