使用している機能

本コードは、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