IsaacLab ✕ SO-ARM101 で学ぶ Physical AI
〜キューブ持ち上げタスクをゼロから実装してみた〜

2025年12月24日掲載

キービジュアル

この記事は、ソフトバンク アドベントカレンダー 2025 、24日目の記事です。

近年の大規模基盤モデルなどの生成AIの成功と並行して、現実世界のモノを動かす Physical AI という分野も、改めて存在感を増してきていると感じています。とくに NVIDIA がこの領域のプラットフォームや開発環境を精力的に整備していることもあり、Physical AI はここ最近、ますます注目度が高まっている分野です。

Physical AI とは、カメラ・ロボット・自動運転車のような自律システムが、物理世界の中で 「知覚し・理解し・推論し・複雑な行動を実行する」 ことを可能にする、いわば 「物理世界で行動する AI」 の総称ですNVIDIA What is Physical AI?)。

本記事では、この Physical AI の流れに乗って、NVIDIA が提供するロボティクス特化シミュレーター Isaac Sim と、ロボット学習フレームワーク Isaac Lab を用い、SO-ARM101 というロボットアームに「キューブを持ち上げる」タスクを強化学習で習得させる ことに挑戦してみました。また、Isaac Lab をできるだけ中身が見える形で理解することを目標としました。

目次

想定読者:Isaac Sim や強化学習を活用したロボティクス開発に関心のある AI エンジニアや実務者を想定しています。特に、物理シミュレーションと機械学習を組み合わせた応用に取り組みたい技術者に向けた内容です。

想定技術レベル:Python や Linux 環境に習熟しており、強化学習やロボットシミュレーションに関する基本的な知識を持つ中級者以上を前提としています。Gym、Hydra、rsl_rl などのフレームワークやツールにもある程度の理解が求められます。

得られること:IsaacLab と SO-ARM101 を用いた強化学習環境の構築方法から、報酬設計・テンプレートのカスタマイズ・学習結果の検証までの実践的なノウハウが得られます。また、Sim2Real に向けた課題意識や、今後の発展に役立つ視点も得ることができます。

環境構築

今回は、Google Cloud Platform(GCP)上で NVIDIA T4 GPU を搭載したインスタンスを用いて作業を行いました。OS は Ubuntu 24.04 LTS を使用しています。Isaac Sim 5.1.0 を pip でインストールする場合、GLIBC 2.35 以上 が必要になるためバージョンに注意してください(NVIDIA公式ドキュメント)。また、Isaac Sim の pip 版は Python 3.11 系 が前提となるので、こちらも事前に用意しておきます。

IsaacSim のインストール

Isaac Sim 自体は pip 経由でインストールできます。公式ドキュメントの手順に沿って環境を整えたうえで、以下のコマンドを実行します。

pip install "isaacsim[all,extscache]==5.1.0" --extra-index-url https://pypi.nvidia.com
インストールが成功すると isaacsim コマンドが使えるようになります。
isaacsim

を実行してみて、Isaac Sim の GUI ウィンドウが立ち上がればセットアップ完了です。

IsaacLab のインストール

次に、強化学習用のフレームワークである IsaacLab をインストールします。こちらはソースコードからセットアップします。まず IsaacLab のリポジトリをクローンします。

git clone https://github.com/isaac-sim/IsaacLab.git
cd IsaacLab

続いて、付属のセットアップスクリプトを実行します。

./isaaclab.sh --install

依存ライブラリのインストールやパッケージ設定が一通りここで行われます。インストールが完了したら、簡単なサンプルスクリプトを動かして動作確認をします。

./isaaclab.sh -p scripts/tutorials/00_sim/create_empty.py

このコマンドで Isaac Sim 5.1.0 のウィンドウが起動し、空のシーンが立ち上がれば、Isaac Sim + Isaac Lab の環境構築は完了 です(NVIDIA公式ドキュメント)。

テンプレートの作成

Isaac Lab には、強化学習タスク用のテンプレートプロジェクトを自動生成してくれる機能があります。今回はこれを使って、SO-ARM101 向けの学習用プロジェクトディレクトリを用意しました。

まず、IsaacLab のルートディレクトリで次のコマンドを実行します。

./isaaclab.sh --new

すると対話形式でいくつか質問(出力先ディレクトリ、ワークフローの種類、RL ライブラリなど)が聞かれるので、順番に回答していきます。質問への回答を終えると、指定したパス配下に 強化学習タスク用のテンプレート一式 が自動生成されます。このテンプレートには、

  • 環境クラス(Env)

  • 環境設定クラス(EnvCfg)

  • RL ライブラリ向けの設定ファイル

  • Gym 登録用の __init__.py

など、一通り必要な構成要素が含まれており、ここから SO-ARM101 用にカスタマイズしていく、という流れになります。

Task type について

Task type は、テンプレートプロジェクトをどこに作るかを決める項目です。

  • Internal:IsaacLab リポジトリ内にタスクを作成します

  • External:IsaacLab リポジトリの外(任意のディレクトリ)にタスクを作成します

基本的には External モードで作業を進めるのが良いと思います。このモードでは、IsaacLab 本体とは別の場所に自分用のプロジェクトディレクトリを作成でき、カスタマイズしたソースコードを IsaacLab 本体と分離して管理できるメリットがあります。External を選択すると、その後に Project path(テンプレート一式を生成するパス名)、Project name(その配下に作られるディレクトリ名)を指定することが可能です。好みに合わせて設定してください。

workflow について

次に、学習環境のロジックをどう構成するかを決める workflow を選びます。選択肢は 2 つあります。

  • Direct

    • 観測・行動・報酬・リセットといった処理を、1 つのクラスの中に直接書いていくスタイル です。単一ファイルにまとまるので、最初に全体像を掴みやすい反面、大きくなってくると肥大化しがちです。

  • Manager-based

    • 観測・報酬・イベントなどを「マネージャ」単位に分割し、モジュール構造として組み立てるスタイル です。拡張性・再利用性は高いですが、最初は構造を把握するのに少し慣れが必要です。

今回は IsaacLab を用いた強化学習がその中で何をしているのかをできるだけ隠蔽なく理解したかったので Direct workflow を選択しました。Direct workflow では 1クラスの中に

  • シーン構築

  • 観測生成

  • 行動適用

  • 報酬計算

  • リセット条件

を実装していくことになるので保守性は低いと思いますが、今回の検証においてはどのタイミングで何が起きているかを理解しやすかったので用途に合っていました。

RLライブラリについて

Isaac Lab 自体は高速に大量の並列環境を回すシミュレータ/フレームワークであり、強化 学習アルゴリズムそのものは外部ライブラリを使用する設計になっています。テンプレート作成時には、以下の RL ライブラリから選択できます。

  • rl_games

  • rsl_rl

  • skrl

  • sb3

から選択することが可能です。今回はとりあえず rsl_rl を使用しました。別のアルゴリズムを試したくなったら、ここを変えてテンプレートを作り直したり、設定を切り替えたりすることも可能です。

作成結果について

テンプレート作成時の QA に回答し終えてスクリプトが完了すると、指定したパス配下に カスタムテンプレートプロジェクトが生成されます。  実際にコマンドを実行した直後の様子がこちらです。また、先ほど指定した  Project path / Project name の配下を見ると、環境クラスや設定ファイル、RL ライブラリ用の設定など、  強化学習タスクに必要なファイル一式が自動的に作成されていることが分かります。

テンプレートの作成直後の段階では、まずはおなじみのCartPole タスクが動くようになっています。  そのまま次のコマンドを実行すると、テンプレート環境を使った学習をすぐに試すことができます。

python scripts/rsl_rl/train.py --task Template-Isaac-Lab-So101-Direct-v0

実行すると、以下のように Isaac Sim のGUI が起動して学習が走り始め、ログや学習の進捗がコンソールに表示されます。

ここまでは、あくまでテンプレートですが、このプロジェクトをベースに環境クラスや設定を書き換えていくことで、SO-ARM101 用のカスタムタスクへと発展させていきます。

カスタマイズ

ここからは、テンプレートとして生成された学習環境をカスタマイズして、  SO-ARM101 がキューブを持ち上げるタスクの学習を進めていきます。

SO-ARM 101 の紹介

SO-ARM101 は、Hugging Face LeRobot プロジェクト向けのオープンソース 6 軸ロボットアームキットで、教育・研究・プロトタイピング向けの安価な実機アームとして人気です。

ロボット構造や関節パラメータ、リンク形状などがURDF(so101_new_calib.urdf)形式で提供されており、概要は次の通りです。

  • shoulder_pan(肩、ベース回転)
  • shoulder_lift(肩、上下)
  • elbow_flex(肘)
  • wrist_flex(手首、上下)
  • wrist_roll(手首、ひねり)
  • gripper(グリッパーの指)

今回の検証では、SO-ARM101 を Isaac Lab 上のロボットアセットとして取り込み、シミュレーション環境内での動作検証に利用します。

IsaacLab 構成の紹介

具体的に話を進める前に、IsaacLab が自動生成してくれたテンプレートには、開発を楽にするための便利機能がいくつか入っているので、軽く整理しておきます。

環境の登録

まず、テンプレートを使った学習実行時に指定していた次のコマンドをもう一度見てみます。

python scripts/rsl_rl/train.py --task Template-Isaac-Lab-So101-Direct-v0
ここで --task に渡している Template-Isaac-Lab-So101-Direct-v0 という文字列は、テンプレート作成時に生成された
source/isaac_lab_so101/tasks/direct/isaac_lab_so101/__init__.py

の中で、タスクのID(id フィールド)として定義されています。

この __init__.py では、おおまかに次のような情報が紐づいています。

  • id … Gym に登録されるタスク名(Template-Isaac-Lab-So101-Direct-v0 など)

  • entry_point … 実際の環境クラス(Env)へのパス

  • env_cfg_entry_point … 環境設定クラス(EnvCfg)へのパス

  • rsl_rl_cfg_entry_point … 使用する RL ライブラリ(今回だと rsl_rl)向けの設定へのパス

学習スクリプト(scripts/rsl_rl/train.py)は、この task ID をもとに

  • どの環境クラスをインスタンス化するか

  • どの設定クラスを読み込むか

を解決してくれているという構造になっています。

学習環境の設定

ロボットをどこに置くか・どんな観測を出すか・どう報酬を計算するか、といった環境固有のロジック は、テンプレートで生成された次の 2 ファイルにまとまっています。

  • isaac_lab_so101_env_cfg.py(下図の①)

  • isaac_lab_so101_env.py(下図の②)

今回の試行錯誤では、ほぼこの 2 ファイルをひたすら触っていました。

DirectRLEnvCfg

スクリーンショット中の① isaac_lab_so101_env_cfg.py では、DirectRLEnvCfg を継承したカスタムクラスが定義されています。これは dataclass ベースの設定クラスで、環境の構造とパラメータをここに集約して管理していきます。

DirectRLEnv

スクリーンショット中の② isaac_lab_so101_env.py では、DirectEnv を継承したカスタムクラスが定義されています。強化学習環境の本体に当たる部分です。このクラスはおおよそ次のようなメソッドを実装しています。

  • _setup_scene()

    • ロボットなどのオブジェクト、地面、障害物、センサーなどを Isaac Sim 上に spawn します

  • _apply_action(actions)

    • ポリシーが出力したアクション(ロボット関節へのトルク/目標角度/速度)を Isaac Sim のアクチュエータへ流し込みます

  • _get_observations()

    • 関節角度・速度、ベース姿勢、IMU や LiDAR の値などから、観測ベクトルを組み立てて返します。観測空間の設計に関わる部分です

  • _get_rewards()

    • タスク固有の報酬を計算する部分です。今回は主にこのメソッドをどのように設計するかについて試行錯誤しました

  • _get_dones()

    • 転倒やタイムアウトなどの条件でエピソード終了フラグを立てます。

 

DirectRLEnv は gymnasium.Env を継承しているので(リンク)、DirectRLEnv.step() が呼ばれるたびに、内部では次のような処理が進みます

  1. アクションの前処理の実行

    1.  まず最初に _pre_physics_step() が呼ばれ、ポリシーから渡されたアクションに対してクリッピング・スケーリング・内部バッファへの展開など、物理シミュレーションに渡すための前処理が行われます。

  2. アクション適用と物理シミュレーションの実行

    1. つづいて _apply_action() を通じてロボットにコマンドを送りつつ、 decimation で指定した回数だけ物理シミュレーションをステップ実行します。ここで使われるのは「物理タイムステップ」で、physics_dt × decimation が 1 環境ステップ分の時間になります。

  3. 報酬と終了判定の計算

    1. 物理ステップが終わったら、_get_rewards() でタスク固有の報酬を計算し、  _get_dones() で terminated / truncated などの終了フラグを求めます。

  4. 終了した環境のみリセット

    1.  並列環境のうち「終わったものだけ」が個別に初期化されるイメージです

  5. interval event の適用

    1.  一定ステップごとに実行したい処理(ログ出力やノイズ付与など)が有効になっている場合は、ここで適用されます。

  6. 観測の計算

    1. 最後に _get_observations() で新しい観測を組み立て、  次のステップでポリシーに入力する観測ベクトルとして用意します。

この一連の処理の結果として、

obs, reward, terminated, truncated, info

が返され、RL ライブラリ側から見ると通常の gymnasium 環境と同じインターフェースで扱えるようになっています。

train.py について

ここまで train.py を

python scripts/rsl_rl/train.py --task Template-Isaac-Lab-So101-Direct-v0

のように呼び出してきましたが、このスクリプトには Hydra を使って設定を上書きできる仕組み が入っています。

train.py では main 関数に isaaclab_tasks.utils.hydra.hydra_task_config のデコレーターがついていて、先程紹介した __init__.py 内でGym レジストリに登録していた 

  • env_cfg_entry_point 

  • rsl_rl_cfg_entry_point

を Hydra の設定ツリーとして読み込むようになっていて、env / agent というキーでコマンドラインから設定を上書きする機能が実装されています。これにより、スクリプトを編集しなくてもコマンドライン引数を用いることで各種パラメーターを変更して学習スクリプトを回すことができます。

例えば、rsl_rl 側の学習設定(rsl_rl_ppo_cfg.py)にある num_steps_per_env を変えたい場合には、

python scripts/rsl_rl/train.py --task Template-Isaac-Lab-So101-Direct-v0 agent.num_steps_per_env=12

のように、agent のキーワードを介してパラメーターを上書きすることができます。同様に、環境側のパラメーター(isaac_lab_so101_env_cfg.py)を変えたい場合は、

python scripts/rsl_rl/train.py --task Template-Isaac-Lab-So101-Direct-v0 env.decimation=10

のように、env のキーワードを介してパラメーターを上書きすることができます。このように、IsaacLab の train.py は Hydra と連携しているおかげで、スクリプト本体を編集せずに

env.xxx=… / agent.xxx=…

という形で、その場でいろいろなハイパーパラメータや環境設定を試せる実験スタイルを採ることができます。

実際に学習を回してみる

だいぶと前置きが長くなってしまいましたが、早速SO-ARM101でのキューブ持ち上げタスクを実装してみたいと思います。

環境のカスタマイズ

今回扱うロボットアーム SO-ARM101 は、so101_new_calib.urdf として URDF 形式で配布されていました。  IsaacLab では USD 形式のアセットが扱いやすく、また他のシーンと組み合わせる際にも取り回しがよいので、付属のツールスクリプトを使って URDF → USD 変換を行いました。IsaacLab におけるURDFやUSDの取り扱いについては NVIDIA公式ドキュメントが参考になるかと思います。

変換は次のコマンドで実行できます。

./isaaclab.sh -p scripts/tools/convert_urdf.py \
    so101_new_calib.urdf \
    ./so101_new_calib.usd \
    --merge-joints \
    --joint-stiffness 0.0 \
    --joint-damping 0.0 \
    --joint-target-type none \
    --headless

IsaacLab には SO-ARM101 に相当するアセットがもともと用意されていますが、「手元の URDF から自前で USD を生成して使ってみる」 という一連の流れも理解しておきたかったので、あえてこの手順を踏んでいます。机とキューブについては、IsaacLab が用意している標準アセット(テーブル、単純な立方体)をそのまま利用しました。

実際の配置は、DirectRLEnv 派生クラスの _setup_scene() を編集して行いました。配置部分は次のようなコードになります。

 

ソースコードを表示
def _setup_scene(self):
        # 1) ロボット
        self.robot = Articulation(self.cfg.robot_cfg)

        # 2) キューブ
        self.object = RigidObject(self.cfg.object_cfg)

        # 3) 地面
        spawn_ground_plane(prim_path="/World/GroundPlane", cfg=self.cfg.plane_cfg)

        # 4) テーブル
        spawn_from_usd(
            prim_path="/World/envs/env_.*/Table",
            cfg=self.cfg.table_usd,
            translation=self.cfg.table_translation,
            orientation=self.cfg.table_orientation,
        )

        # 5) env_0 を複製して num_envs 分の環境を作る
        self.scene.clone_environments(copy_from_source=False)

        # 6) シーンに登録
        self.scene.articulations["robot"] = self.robot
        self.scene.rigid_objects["object"] = self.object

        # 7) ライト追加
        light_cfg = sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
        light_cfg.func("/World/Light", light_cfg)

報酬のカスタマイズ

強化学習では、エージェント(ここでは SO-ARM101 の制御ポリシー)が「いまの状態で、この行動をとったらどれくらい嬉しいか」を数値として受け取ります。この1ステップごとのフィードバックの指標が報酬(reward)です。

SO-ARM101 がキューブを持ち上げるタスクを学習できるようにするためには、DirectRLEnv の _get_rewards() 内で、ロボットとキューブの状態からこの「報酬」を計算し、RL ライブラリに渡す処理を実装する必要があり、_get_rewards() がタスク設計の中核となる部分です。

強化学習に今回始めて挑戦してみたのですが、この部分のカスタマイズにそれなりに時間が掛かってしまいました。雑多な試行錯誤の全てを書くと際限がないので、ここではハマった部分と意識した設計ステップを中心に整理しておきます。

そもそも

とりあえず始めに「キューブがある一定の高さまで持ち上がったら大きな報酬を与える」という実装で雑にやってみたのですが、案の定うまく行きませんでした。キューブを持ち上げるタスクを段階的に分解して報酬を設計していくことが(何事についても当たり前ですが、、、)重要でした。

結局、だいたい以下の順で段階的に報酬を修正していきました。

1. EE(指先)がキューブに近づく

2. “掴めそうな姿勢” が作る

3. 近い状態でグリッパーを閉じる

4. キューブを机から浮かせる

5. キューブを目標の高さまで安定して持ち上げる

このように、「ここまではできている」「この先で失敗している」という分かれ目が見えるような段階構造にすることで、  ログを見ながらどのフェーズの報酬がボトルネックになっているのかが追いやすくなりました。

SO-ARM101 の URDF の把握ミス

IsaacLab では、使用しているURDF の中でどの link が何に該当するかを正しく把握しておくことが重要です。特に今回は、エンドエフェクター(End Effector; EE)にしたい link がどれかを正しく特定しておく必要があります。ここが曖昧なまま進めてしまっていたのも反省で、報酬が「見当違いの点への誘導」になって学習が崩れていました。

EE だと思っていたフレームが、実はグリッパの付け根だったという勘違いが一番時間をくってしまった部分でした。IsaacSim の GUI では、読み込んだ URDF のリンク構造やフレーム名をツリーとして確認できるので、

  • どの link がベースで、どこからどこまでがアームか

  • グリッパまわりの link / joint がどう接続されているか

  • EE として扱いたいフレームはどれか(報酬や観測で使う座標系)

といった点を、一度しっかり目視で確認してから実装に入る必要がありました。

掴むまでのお膳立て

単に EE がキューブに近づくだけだと、

  • 上から押さえつけるだけで、まったく掴めない  

  • キューブを突いて動かすだけになってしまう

といった失敗パターンがかなり多く発生しました。 今回は次のような要素を盛り込みました。

  • グリッパの開閉方向ベクトルが、キューブの面法線と揃うような項  

  • EE の Z 高さが、キューブ中心付近に来るような項  

  • EE の XY 位置誤差が十分に小さいときに、追加で報酬を与える項  

きちんとした ablation study まではできていないので、どの項目がどれくらい効いているか、そもそも本当に効いているのかは定量的には言い切れませんが、 これらの項を報酬に付け加えることで改善の方向には学習が進んでいました。 

条件付きの報酬項とした

もうひとつ試した事項として、報酬項を条件付きで有効化するという設計です。すべてのステップで全報酬項が常に有効になっていたので、まだキューブに近づいてもいない段階で「持ち上げ」向けの報酬が入ってしまったりと、意図しない方向に学習が進む状況になりがちでした。

そこで、次のような条件付きの構造にしました。

  • EE とキューブの距離が十分近いときだけ「グリッパを閉じるときの報酬」を計算する  

  • キューブが机からある程度浮いているときだけ「高さ」や「姿勢安定性」に関する報酬を有効にする  

こうすることで、

1. 最初のうちは「まず近づく」ことに集中させる  

2. 近づけるようになってきたら「正しい構えで掴む」に意識をシフトする  

3. 掴めるようになってきたら「持ち上げて安定させる」フェーズに移行する  

という流れを、報酬設計に落とし込むことができました。最終的に使用していた報酬は以下のようなものです。

 

ソースコードを表示
def _get_rewards(self) -> torch.Tensor:
        # ------------------------------------------------------------
        # 状態の取り出し
        # ------------------------------------------------------------
        # アーム DOF の joint velocity
        joint_vel_all = self.robot.data.joint_vel  # (N, dof)
        joint_vel = joint_vel_all[:, self._arm_dof_idx]  # (N, n_arm_dof)

        # オブジェクトのワールド座標
        object_pos_w = self.object.data.root_pos_w[:, :3]  # (N, 3)
        object_height_w = object_pos_w[:, 2]  # (N,)

        # ロボット root
        robot_root_state = self.robot.data.root_state_w  # (N, 13)
        robot_pos_w = robot_root_state[:, :3]
        robot_quat_w = robot_root_state[:, 3:7]

        # 全ボディのワールド姿勢
        body_pos_w = self.robot.data.body_pos_w  # (N, num_bodies, 3)
        body_quat_w = self.robot.data.body_quat_w  # (N, num_bodies, 4)

        # グリッパ中心(gripper_frame_link)のワールド姿勢
        gripper_center_pos = body_pos_w[:, self._gripper_frame_link_idx, :3]  # (N, 3)
        gripper_center_quat = body_quat_w[:, self._gripper_frame_link_idx, :4]  # (N, 4)

        # コマンド(ロボット root から見たキューブ目標位置)
        target_object_pos_b = self.command_object_pos_b  # (N, 3)

        # テーブル高さからのオブジェクト高さ
        object_height_from_table = torch.clamp(
            object_height_w - self.object_init_height, min=0.0
        )  # (N,)

        # ------------------------------------------------------------
        # スケール(重み)
        # ------------------------------------------------------------
        rew_scale_reach = 1.0
        rew_scale_lift = 15.0
        rew_scale_goal_coarse = 16.0
        rew_scale_goal_fine = 5.0
        rew_scale_action = -1e-4
        rew_scale_joint_vel = -2e-4
        rew_scale_close_good = 2.0  # 近くで閉じたときのボーナス
        rew_scale_close_bad = -0.1  # 遠くで閉じたときのペナルティ
        rew_scale_attach = 1.0  # 掴んでいる間の定常ボーナス
        rew_scale_lift_smooth = 30.0  # 高さ方向 shaping
        rew_scale_lift_vel = 6.0  # 上向き速度
        rew_scale_geo = 2.0  # グリッパ中心フレームでの幾何報酬
        rew_scale_hold = 5.0  # ホールドする

        # ------------------------------------------------------------
        # 1) reaching_object: 「グリッパ中心」とキューブの距離
        # ------------------------------------------------------------
        center_cube_distance = torch.norm(
            gripper_center_pos - object_pos_w, dim=-1
        )  # (N,)
        reach_std = 0.05
        rew_reach = rew_scale_reach * (
            1.0 - torch.tanh(center_cube_distance / reach_std)
        )

        # ------------------------------------------------------------
        # グリッパの開閉状態 → 「掴んでいる」判定(attach)
        # ------------------------------------------------------------
        joint_pos_all = self.robot.data.joint_pos
        q_gripper = joint_pos_all[:, self._gripper_dof_idx]
        q_gripper = q_gripper.mean(dim=-1)

        current_q = self.cfg.gripper_open_pos - q_gripper
        gripper_range = self.cfg.gripper_open_pos - self.cfg.gripper_close_pos
        gripper_closed_amount = torch.clamp(current_q / gripper_range, 0.0, 1.0)  # (N,)

        # "閉じている" フラグ
        closed_mask = (gripper_closed_amount > 0.6).float()

        # 「閉じた瞬間」を検出する
        prev_closed = self._prev_closed_mask
        closing_event = (closed_mask - prev_closed).clamp(min=0.0)

        # キューブ近傍かどうか(グリッパ中心との距離で判定)
        close_near = (center_cube_distance < 0.04).float()
        close_far = (center_cube_distance > 0.08).float()

        # 近くで閉じた瞬間にボーナス
        rew_close_good = rew_scale_close_good * closing_event * close_near

        # 遠くで閉じた瞬間にはペナルティ
        rew_close_bad = rew_scale_close_bad * closing_event * close_far

        # EE とキューブが十分近く、かつ閉じているとき「掴んでいる」とみなす
        attach_dist_thresh = 0.02
        attach_mask = (center_cube_distance < attach_dist_thresh).float() * closed_mask

        # 掴んでいる状態そのものへのボーナス
        rew_attach = rew_scale_attach * attach_mask

        # ------------------------------------------------------------
        # 2.5) グリッパ中心フレームで見たキューブ位置の幾何報酬
        #      - gripper_frame_link を「原点・軸を持つ座標系」として最大活用
        # ------------------------------------------------------------
        object_in_grip, _ = subtract_frame_transforms(
            gripper_center_pos, gripper_center_quat, object_pos_w
        )
        # Y–Z 平面での距離(指の左右+上下方向のズレ)
        obj_yz = object_in_grip[:, 1:3]  # (N, 2)
        yz_dist = torch.norm(obj_yz, dim=-1)  # (N,)

        geo_std = 0.02  # 2cm 程度のスケール(グリッパ幅/キューブサイズに応じて調整)
        geo_align = 1.0 - torch.tanh(yz_dist / geo_std)

        # 「掴んでいる」間に、できるだけ中心線上にキューブを保つ
        rew_geo = rew_scale_geo * geo_align * attach_mask

        # ------------------------------------------------------------
        # 3) リフト
        #   - 高さ方向の smooth なリフト報酬 / 上向き速度
        #   - lifting_object: キューブが一定以上の高さまで上がったら報酬
        # ------------------------------------------------------------
        # 密な報酬
        lift_h_stage1 = 0.04
        lift_smooth_gate = (object_height_from_table > 0.01).float()
        h1 = torch.clamp(object_height_from_table / lift_h_stage1, min=0.0, max=1.0)

        # さらに報酬
        lift_h_goal = 0.5
        h2 = torch.clamp(
            (object_height_from_table - lift_h_stage1) / (lift_h_goal - lift_h_stage1),
            min=0.0,
            max=1.0,
        )
        h2_gate = (object_height_from_table > lift_h_stage1).float()
        rew_lift_smooth = (
            rew_scale_lift_smooth
            * (h1 + 1.0 * h2 * h2_gate)
            * attach_mask
            * lift_smooth_gate
        )

        # 上方向の速度
        object_lin_vel_w = self.object.data.root_lin_vel_w  # (N, 3)
        object_vz = object_lin_vel_w[:, 2]
        rew_lift_vel = (
            rew_scale_lift_vel * torch.clamp(object_vz, min=0.0) * attach_mask
        )

        # 最後に現在の閉じ状態を保存
        self._prev_closed_mask = closed_mask

        # 疎な報酬
        minimal_height_lift = 0.02
        lifted_mask = (object_height_from_table > minimal_height_lift).float()  # (N,)

        rew_lift = rew_scale_lift * lifted_mask * attach_mask

        # ------------------------------------------------------------
        # ホールド
        # ------------------------------------------------------------
        rew_hold = rew_scale_hold * attach_mask * h1

        # ------------------------------------------------------------
        # 4) object_goal_distance: 目標位置への tracking(持ち上げ後のみ)
        # ------------------------------------------------------------
        des_pos_w, _ = combine_frame_transforms(
            robot_pos_w, robot_quat_w, target_object_pos_b
        )
        goal_dist = torch.norm(des_pos_w - object_pos_w, dim=-1)  # (N,)

        goal_std_coarse = 0.3
        goal_std_fine = 0.05

        base_goal_coarse = 1.0 - torch.tanh(goal_dist / goal_std_coarse)
        base_goal_fine = 1.0 - torch.tanh(goal_dist / goal_std_fine)

        rew_goal_coarse = rew_scale_goal_coarse * base_goal_coarse * lifted_mask
        rew_goal_fine = rew_scale_goal_fine * base_goal_fine * lifted_mask

        # ------------------------------------------------------------
        # 5) ペナルティ: action_rate_l2 / joint_vel_l2
        # ------------------------------------------------------------
        rew_action = rew_scale_action * torch.sum(self.last_actions**2, dim=-1)
        rew_joint_vel = rew_scale_joint_vel * torch.sum(joint_vel**2, dim=-1)

        # ------------------------------------------------------------
        # 6) 総和
        # ------------------------------------------------------------
        total_reward = (
            rew_reach
            + rew_lift
            + rew_goal_coarse
            + rew_goal_fine
            + rew_action
            + rew_joint_vel
            + rew_close_good
            + rew_close_bad
            + rew_attach
            + rew_lift_smooth
            + rew_lift_vel
            + rew_geo
            + rew_hold
        )

        return total_reward

学習結果

いろいろと試行錯誤をしまして、最終的な挙動は以下の動画をご覧ください。

ざっくりとした観察結果は次のような感じでした。

  • EE がキューブに向かって素直にアプローチするようになっている

  • グリッパの姿勢もそれなりに「掴めそうな構え」になっている

  • うまくいくときは、キューブをしっかり挟みこんでから、目標高さ付近まで持ち上げてくれる

完璧とは言い難いものの、「SO-ARM101 が自律的にキューブを掴んで持ち上げる」というところまではたどり着くことができました。  

最後に

今回は、IsaacSim / IsaacLab を用いて

  • SO-ARM101 用のカスタム環境を構築し

  • 観測・行動空間と報酬関数を設計し

  • rsl_rl(PPO)で実際に学習を回してみる

というところまでを、一通り試してみました。UDF/URDF などの考え方に触れることができたこと、報酬設計に試行錯誤が必要であること、IsaacLab のテンプレート構成について、理解を深めることができました。

一方で、シミュレーター上でロボットアームを動かせただけでは、Physical AI のスタートラインが見えただけで、まだまだ本当のスタートすら切れていないと感じています。実世界のタスクとして意味を持たせるための Sim2Real などの課題に取り組んでいく必要があると思います。今回の実験で「シミュレーション側の足場」はある程度固めることができたので、実機での簡易な再現実験、シミュレーション条件と実機条件の差分の洗い出し、などにも挑戦していきたいと思います。

以上、IsaacLab を用いた SO-ARM101 のキューブ持ち上げタスクの学習事例と、その過程で得られた気づきを紹介しました。

明日は、アドベントカレンダー最終日となる25日です!明日もお楽しみに!

おすすめの記事

 

条件に該当するページがございません