【Pybulletサンプル解説】ロボットアームの先端にtipと表示する【debugDrawItems.py】

お疲れ様です。秋並です。

Pybullet公式gitリポジトリのサンプルコードを解説するシリーズです（一覧はこちら）。

今回は、debugDrawItems.pyを解説します。（コードのリンクはこちら）

本コードを実行すると、「ロボットアームの先端に"tip"と表示され、各関節の角度が変化」します。

使用している機能

本コードは、addUserDebugText 関数を使用することで「ロボットアームの先端にtipを表示」しています。

pybullet.addUserDebugText(text, textPosition,
                           textColorRGB,
                           textSize,
                           parentObjectUniqueId,
                           parentLinkIndex)

• text：表示するテキスト
• textPosition：表示するテキストの位置（[x,y,z]）
• textColorRGB：テキストの色（[R,G,B]）
• parentObjectUniqueId：テキストを表示するオブジェクトのID
• parentLinkIndex：テキストを表示するリンクのインデックス

今回の場合、下記コードのようにkukaロボットアームの先端リンクにtipというテキストを表示しています。

p.addUserDebugText("tip", [0, 0, 0.1],
                   textColorRGB=[1, 0, 0],
                   textSize=1.5,
                   parentObjectUniqueId=kuka,
                   parentLinkIndex=6)

コメントをつけたサンプルコード

サンプルコードにコメントをつけたものが以下になります（もともとあった不要と思われるコメント等については削除しています）

import pybullet as p
import time
import pybullet_data

# Pybulletを共有メモリモードで接続
cid = p.connect(p.SHARED_MEMORY)

# 共有メモリモードが失敗したら、GUIモードで接続
if (cid < 0):
  p.connect(p.GUI)

# Pybullet用のデータパスを取得
p.setAdditionalSearchPath(pybullet_data.getDataPath())  

# 床のオブジェクトを生成
p.loadURDF("plane.urdf")

# kukaロボットアームを生成
kuka = p.loadURDF("kuka_iiwa/model.urdf")

# kukaロボットアームの先端に赤色のテキストで「tip」と表示
p.addUserDebugText("tip", [0, 0, 0.1],
                   textColorRGB=[1, 0, 0],
                   textSize=1.5,
                   parentObjectUniqueId=kuka,
                   parentLinkIndex=6)

# リアルタイムシミュレーションを有効化
p.setRealTimeSimulation(0)

# 角度を0度で初期化
angle = 0

while (True):
  time.sleep(0.01)

  # 2, 3番目の関節をangleに設定
  p.resetJointState(kuka, 2, angle)
  p.resetJointState(kuka, 3, angle)

  # angleを+0.01
  angle += 0.01