Isaac SimをGazeboみたいに使うには？

## はじめに
本記事はROS 2アドベントカレンダー3日目の記事です．

https://qiita.com/advent-calendar/2024/ros2

本記事では、Nvidiaが提供しているロボット開発プラットフォームであるIsaac Simを使って、ROS 2で作成した自作の移動ロボットをシミュレーションするまでの手順について説明します。

本記事では、自作のツールを用いてGazeboのようにROS 2コマンドを利用してシミュレーションをおこないます。

https://github.com/hijimasa/isaac_ros2_utils

わかりにくい部分や間違っている部分などありましたら、気軽にコメントいただけると助かります。

## 本記事の対象読者

- ROS 2の基本的なプログラムに触れたことがある人
- 上記に加えて、気軽に現実的な見た目を表現できるシミュレータを使いたい人

## Isaac Simとは

上でも書きましたが、Isaac Simは、Nvidiaが提供しているロボット開発プラットフォームです。GPUを開発しているNvidiaが提供しているツールなだけあって、レイトレーシング技術などを活用した**現実に近い見た目でのシミュレーション**を行うことができるほか、ゲームエンジンで培われた物理エンジンを利用しておりGazeboと比較して**オブジェクトが多い環境でも安定**してシミュレーションが出来ます。

一方で、ロボットモデルの記述方法にUSDというピクサーが開発した記述方法が採用されていたり、Omni Graphというビジュアルプログラミングツールでロボットのコントローラやセンサの設定を行わなければならなかったりするため、ROSユーザにとってとっつきにくい部分がありました。また、URDF Importerというツールが存在しますが、単純にインポートするだけでは摩擦係数やセンサといったGazeboなら自動で設定された項目が設定されません。

本記事では、自作のユーティリティツールを活用することで、上記のIsaac Sim特有の部分に直接触れることなくシミュレーションを行う方法について解説します。


## 開発環境の導入

個人的に開発環境にはDockerを使用することを強く推奨します。
Docker環境を使用することで、PC内の環境をできるだけクリーンな状態に保つことができます。

### Dockerのセットアップ
1. Nvidia GPUドライバのインストール
   ```bash
   sudo apt-get update
   sudo apt install build-essential -y
   wget https://us.download.nvidia.com/XFree86/Linux-x86_64/535.129.03/NVIDIA-Linux-x86_64-535.129.03.run
   chmod +x NVIDIA-Linux-x86_64-535.129.03.run
   sudo ./NVIDIA-Linux-x86_64-535.129.03.run
   ```

2. Dockerのインストール
   ```bash
   curl -fsSL https://get.docker.com -o get-docker.sh
   sudo sh get-docker.sh
   sudo groupadd docker
   sudo usermod -aG docker $USER
   ```

3. Nvidia コンテナツールキットのインストール
   ```bash
   curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \
   && curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \
   sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
   sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list \
   && \
   sudo apt-get update

   sudo apt-get install -y nvidia-container-toolkit
   sudo systemctl restart docker

   sudo nvidia-ctk runtime configure --runtime=docker
   sudo systemctl restart docker
   ```

### Isaac SimのDocker環境のインストール
1. NGCのウェブサイトにサインイン
   ブラウザから https://ngc.nvidia.com/signin にアクセスして、eメールとパスワードを入力する

2. 右上に表示されるユーザアイコンをクリックして、メニューから**Setup**をクリック

3. **Generate API Key**をクリックして、APIキーのページを開きます。 
   APIキーは、NGCコンテナレジストリへのアクセスするためのパスワード的なものです。

4. APIキーページで、**+ Generate API Key**をクリックしてAPIキーを生成
   新しいAPIキーを作成すると、古いAPIキーは無効になるという警告メッセージが表示されます

5. **confirm**をクリックしてキーを生成
   APIキーは繰り返し使えるので、表示されたAPIキーをコピーして手元で大事に保管してください
   APIキーを紛失した場合は、NGCウェブサイトから新しいキーを生成できます。新しいAPIキーを生成すると、古いキーは無効になります

6. 以下のコマンドで手元のPCのDockerデーモンからNGCコンテナレジストリへアクセスできるようにします
   ```
   docker login nvcr.io
   Username: $oauthtoken
   Password: <Your NGC API Key>
   ```

7. Isaac SimのDockerイメージをプル
   ```bash
   docker pull nvcr.io/nvidia/isaac-sim:4.2.0
   ```
   正常にイメージがプルできれば完了です。プル出来ない場合、NGCコンテナレジストリに正しくログインできていない場合やプロキシの問題などが考えられます。

### Isaac Sim + ROS 2のDockerイメージ作成
1. colcon_wsとdockerディレクトリの作成
   任意のディレクトリにcolcon_wsとdockerディレクトリを作成します。こうすることで、後述のシェルスクリプトを用いて簡単にDockerコンテナを立ち上げることが出来ます

2. Dockerfileおよびシェルスクリプト、設定ファイルのコピー
   以下のURLに置かれたDockerfileおよびシェルスクリプト、設定ファイルを作成したdockerディレクトリにコピーします
   https://github.com/hijimasa/isaac-ros2-control-sample/tree/main/docker
   
3. Dockerイメージの作成
   端末を使用して作成したdockerディレクトリ内でシェルスクリプトを実行し、Dockerイメージを作成します
   ```bash
   cd docker
   ./build_docker_image.sh
   ```   

### ROS 2パッケージの導入
1. 上記の手順で作成したcolcon_wsディレクトリにsrcディレクトリを作成
2. 作成したsrcディレクトリ内でgit clone（gitで管理されたディレクトリ内ではgit submodule add）で必要なパッケージをダウンロード
   ```bash
   git clone https://github.com/hijimasa/isaac_ros2_utils.git
   cd isaac_ros2_utils
   git submodule update --init --recursive
   ```

## ロボットモデルの作成

URDFの基本的な記述内容は変わらないので、すでに手元にシミュレーションしたいロボットモデルがある場合には、そのURDFを含むパッケージをcolcon_ws/srcにコピーしてください。
新しくパッケージを作成する場合には、以下の手順を参考にしてください。
```bash
cd docker
./launch_docker.sh
# 立ち上がったコンテナ内で
cd src
ros2 pkg create diffbot_description --build-type ament_cmake
# 作成されたパッケージはroot所有になるので、別の端末から以下のコマンドで所有権を移す
sudo chown -R $USER src/hoge_description
```

### 基本的なロボットモデルの記述

基本的なURDFの記述方法はGazeboのときと一切変わらないです。
ただ、Isaac Simではgazeboタグの記述が無視されるので摩擦係数やセンサ情報が設定できなかったり、Gazebo向けのros2_controlプラグインが利用できなかったりするので、その部分を補間する方法について以下で説明します。

### ジョイントの剛性、ダンパ係数の設定

Isaac Simでは各ジョイントに剛性とダンパ係数と摩擦を設定できます。
剛性とダンパ係数はそれぞれ現在位置・速度と目標位置・速度の差分に対して、どれだけの力をジョイントに出力させるかを決定する係数となっています。式で表すと以下のとおりです。
```math
F=S*(P－P_{target})+D*(V－V_{target})
```
ここで、 $F$はジョイントの出力、$S$,$D$はジョイントの剛性、ダンパ係数、$P$, $P_{target}$は現在位置と目標位置、$V$,$V_{target}$は速度と目標速度を表します。

基本的に位置制御では剛性を高くしてダンパ係数をゼロに、速度制御では剛性をゼロでダンパ係数を高く設定する感じです。
今回作成したツールではデフォルトでフリージョイントになるように、剛性とダンパ係数は設定されていません。

URDFでの記述では以下のサンプルのようにjointタグの中にisaac_drive_apiタグで設定します。
```xml
    <joint name="${prefix}_joint" type="continuous">
      <origin xyz="${xyz}" rpy="${radians(-90)} 0 0"/>
      <parent link="${parent}"/>
      <child  link="${prefix}_link"/>
      <axis xyz="0 0 1" />
      <isaac_drive_api stiffness="0" damping="30000" joint_friction="0"/>
    </joint>
```
joint_frictionパラメータはジョイントの摩擦に関するパラメータで、Isaac Simで設定できるパラメータ一覧にあったので実装しています。おそらくジョイントに加わる最大摩擦力を設定できるものだと思います。

### 摩擦係数の設定

最初の方で述べたように、Isaac SimではGazeboのときの摩擦係数の記述は無視されます。
また、Isaac SimではGazeboのようにリンクごとに摩擦係数を設定するわけではなく、マテリアルに紐付ける形で摩擦係数を設定します。
そのため、今回作成したツールでは、materialタグ内で摩擦係数の設定に関する項目を設定するようにしました。
```xml
  <material name="gray">
    <color rgba="0.5 0.5 0.5 0.2"/>
    <isaac_rigid_body static_friction="0.8" dynamic_friction="0.5"/>
  </material>
  <material name="ball">
    <color rgba="0.5 0.5 0.5 0.2"/>
    <isaac_rigid_body static_friction="0.0" dynamic_friction="0.0"/>
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
    <isaac_rigid_body static_friction="1.0" dynamic_friction="1.0"/>
  </material>
```
これによって、滑ってほしいボールキャスタ部分は摩擦をゼロにしつつ、車輪部分の摩擦は高く設定できます。

### ros2_controlの設定

Gazeboでは専用のプラグインを使用してロボットを制御していましたが、今回のツールではtopic_based_ros2_contorolパッケージを使用しています。これによって、各ジョイントの指令や状態をIsaac Simとトピックでやり取りすることが出来ます。もともとのtopic_based_ros2_contorolパッケージだとIsaac Simとの連携に不具合があったため、修正版を作成した上でそれを使用するようにしています。修正版は2024年11月18日時点でプルリクエストを依頼している状態です。

以下がros2_controlタグのサンプルです。
```xml
    <ros2_control name="diffbot" type="system">
      <hardware>
        <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
        <param name="joint_commands_topic">/diffbot/joint_command</param>
        <param name="joint_states_topic">/diffbot/joint_states</param>
        <param name="sum_wrapped_joint_states">true</param>
      </hardware>
      <joint name="left_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
      </joint>
      <joint name="right_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
      </joint>
    </ros2_control>
```

### センサ（と追加機能）の設定

Isaac Simでのセンサの設定はGazeboとは異なるパラメータを持つため、isaacタグで記述することにしました。
以下がサンプルです。
```xml
    <isaac>
      <sensor name="lidar_link" type="lidar">
        <topic>scan</topic>
        <sensor_dimension_num>3</sensor_dimension_num> <!-- 2 or 3 -->
        <config>hokuyo/UST-30LX</config>
        <!-- Config Example
        <config>Example_Rotary</config>
        <config>SLAMTEC/RPLIDAR_S2E</config> LaserScan do not work Why?
        <config>hokuyo/UST-30LX</config> LaserScan do not work Why?
        -->
      </sensor>

      <sensor name="camera_link" type="camera">
        <topic>image_raw</topic>
        <horizontal_fov_rad>1.3962634</horizontal_fov_rad>
        <horizontal_focal_length>30</horizontal_focal_length> <!-- optical parameter -->
        <vertical_focal_length>30</vertical_focal_length> <!-- optical parameter -->
        <focus_distance>400</focus_distance> <!-- distance for clear image -->
        <projection>perspective</projection> <!-- perspective or orthgonal -->
        <image>
           <width>600</width>
           <height>600</height>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <update_rate>10.0</update_rate>
      </sensor>

      <sensor name="depth_camera_link" type="depth_camera">
        <topic>image_raw</topic>
        <horizontal_fov_rad>1.3962634</horizontal_fov_rad>
        <horizontal_focal_length>30</horizontal_focal_length> <!-- optical parameter -->
        <vertical_focal_length>30</vertical_focal_length> <!-- optical parameter -->
        <focus_distance>400</focus_distance> <!-- distance for clear image -->
        <projection>perspective</projection> <!-- perspective or orthgonal -->
        <image>
          <width>320</width>
          <height>240</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <update_rate>10</update_rate>
      </sensor>
      
      <surface_gripper name="body_link">
        <offset_x>1.0</offset_x>
        <offset_y>0</offset_y>
        <offset_z>0</offset_z>
        <axis>1 0 0</axis>
        <grip_threshold>0.1</grip_threshold>
        <force_limit>1.0e2</force_limit>
        <torque_limit>1.0e3</torque_limit>
        <bend_angle>0.7854</bend_angle>
        <stiffness>1.0e4</stiffness>
        <damping>1.0e3</damping>
        <retry_close>False</retry_close>
      </surface_gripper>

      <thruster name="body_link">
      </thruster>

    </isaac>
```
各センサの設定方法について説明します。
- LiDARセンサ
  LiDARセンサは現状では設定されているconfigファイルを選択する形で使用できます。センサの次元を選択することで、scanメッセージを出力するかpointcloudメッセージを出力するかを選択できます。

- カメラおよびデプスカメラ
  視野角や解像度に加えて、焦点距離といったパラメータを設定できます。

Isaac Simではセンサに加えて吸着ハンドやスラスターのシミュレーションを行えるので、その機能をURDFで設定できるようにしています。

- 吸着ハンド(surface gripper)
  所定の位置にあるものをくっつけるように把持することができる機能です。対象のリンク原点に対して実際の吸着力を発生させる位置と向き、力の限界などを設定できます。方向は0,1,-1で指定する形です。（例：+Z方向``0 0 1``、-Y方向``0 -1 0``）基本的にIsaac Sim側で設定できる項目をそのまま設定できるようにした形なので、bend_angleがどのようなものなのか正直わかっていません。

- スラスター
  航空宇宙向けの機能です。リンクの原点にz軸プラス方向に力を発生させることができるようにしています。もともとはリンクの原点に対する任意の位置に任意の方向に力を発生できますが、簡単に使用するためにオミットしました。要望に応じて仕様変更予定です。

## シミュレーション上でのロボットモデルの生成・テスト

この節では、これまでの手順で作成したURDFファイルをIsaac Sim上に実際に生成する方法について説明します。

### launchファイルの作成

作成したURDFモデルをIsaac Simに生成するlaunchファイルのサンプルを以下に示します。

```diffbot_spawn.launch.py
import os
import pathlib

from ament_index_python.packages import get_package_share_directory


from launch import LaunchDescription
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

import xacro

def generate_launch_description():
    isaac_diffbot_description_path = os.path.join(
        get_package_share_directory('diffbot_description'))

    xacro_file = os.path.join(isaac_diffbot_description_path,
                              'robots',
                              'diffbot.urdf.xacro')
    urdf_path = os.path.join(isaac_diffbot_description_path, 'robots', 'diffbot.urdf')
    # xacroをロード
    doc = xacro.process_file(xacro_file, mappings={'use_sim' : 'true'})
    # xacroを展開してURDFを生成
    robot_desc = doc.toprettyxml(indent='  ')
    f = open(urdf_path, 'w')
    f.write(robot_desc)
    f.close()
    relative_urdf_path = pathlib.Path(urdf_path).relative_to(os.getcwd())

    params = {'robot_description': robot_desc}

    robot_controllers = PathJoinSubstitution(
        [
            FindPackageShare("isaac_diffbot_sim"),
            "config",
            "isaac_diffbot.yaml",
        ]
    )

    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[params, robot_controllers],
        output={
            "stdout": "screen",
            "stderr": "screen",
        },
    )
    
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params]
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
    )
    
    diff_drive_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["diff_drive_controller", "--controller-manager", "/controller_manager"],
    )

    velocity_converter = Node(
        package='velocity_pub',
        name='velocity_pub',
        executable='velocity_pub',
       remappings=[
            ('cmd_vel_stamped', '/diff_drive_controller/cmd_vel'),
        ],
    )
        
    isaac_spawn_robot = Node(
        package="isaac_ros2_scripts",
        executable="spawn_robot",
        parameters=[{'urdf_path': str(relative_urdf_path),
                    'x' : 0.0,
                    'y' : 0.0,
                    'z' : 0.0,
                    'R' : 0.0,
                    'P' : 0.0,
                    'Y' : 1.57,
                    }],
    )

    isaac_prepare_sensors = Node(
        package="isaac_ros2_scripts",
        executable="prepare_sensors",
        parameters=[{'urdf_path': str(relative_urdf_path)}],
    )

    isaac_prepare_robot_controller = Node(
        package="isaac_ros2_scripts",
        executable="prepare_robot_controller",
        parameters=[{'urdf_path': str(relative_urdf_path)}],
    )

    return LaunchDescription([
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=isaac_spawn_robot,
                on_exit=[isaac_prepare_sensors],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=isaac_prepare_sensors,
                on_exit=[isaac_prepare_robot_controller],
            )
        ),
        control_node,
        node_robot_state_publisher,
        joint_state_broadcaster_spawner,
        diff_drive_controller_spawner,
        velocity_converter,
        isaac_spawn_robot,
    ])
```

ここで、Isaac Simにロボットモデルを生成するために重要なノードであるspawn_robot、prepare_sensors、prepare_robot_controllerについて説明します。

- spawn_robotは文字通りIsaac Sim上にロボットを生成するノードです。パラメータとして、生成するロボットのURDFのパスを必要とします。また、パラメータで**シミュレーション実行時の**ロボットの出現位置姿勢を設定できます。シミュレーション実行時というのは、シミュレーション停止時には世界原点になってしまうためです。現状、シミュレーションの実行に支障がないので放置しています。

- prepare_sensorsはURDFの記述を元にセンサを利用するためのOmniGraphをIsaac Sim上に生成するノードです。生成したロボットのURDFのパスを必要とします。

- prepare_robot_controllerはURDFの記述を元にros2_controlで制御するためのOmni GraphをIsaac Sim上に生成するノードです。これも生成したロボットのURDFのパスを必要とします。

本来は上記３つのノードを一つにまとめる予定でしたが、まとめて動かすと動作しなかったので、分けて実装しています。また、Isaac Simにすべてのノードを同時に処理させることは難しいので、それぞれのノードが終了したときに次のノードを実行できるようにlaunchファイルを記述しています。（詳しく書くとPython REPLという拡張機能でリモートでPythonを実行させているのですが、それが複数を同時に実行できないようです。）

上記のlaunchファイルでは、twistメッセージをtwist_stampedメッセージに変換するために、velocity_pubという自作ノードを使用しています。ノードの本体は以下にあるので適宜コピーして使ってください。

https://github.com/hijimasa/isaac-ros2-control-sample/tree/main/colcon_ws/src/velocity_pub

### launchファイルを立ち上げる手順

1. Isaac Simの立ち上げ
   ```bash
   ros2 run isaac_ros2_scripts launcher
   ```

2. ロボットモデルの立ち上げ
   ```bash
   ros2 launch test_launch_pkg diffbot_spawn.launch.py 
   #ここでパッケージ名はlaunchファイルが存在しているパッケージ名に適宜変更してください
   ```

3. （オプション）teleop_twist_keyboardの立ち上げ
   ```bash
   ros2 run teleop_twist_keyboard teleop_twist_keyboard
   ```

## おわりに
本記事では、URDFの記述を用いてIsaac Sim特有のツールに直接触れることなくシミュレーションを行うツールの導入と使用方法について解説しました。
今回作成したURDFのサンプルは以下のリポジトリに格納しているので、適宜参考にしてみてください。

https://github.com/hijimasa/isaac-ros2-control-sample

## 参考サイト
- [Container Installation](https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_container.html)
- [Generating API Key](https://docs.nvidia.com/ngc/gpu-cloud/ngc-user-guide/index.html#generating-api-key)