MyoSuite Tutorial No.6

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

はじめに

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

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

本記事では6_Inverse_Dynamics.ipynbをGoogle Colabで実行した際の手順と結果に関してまとめる．

MyoSuiteとは

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

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

Tutorial Notebook

実施事項

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

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

!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と関数設定

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と一般化力の計算

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)

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