URDFを表示するだけのpython

ほんとにちょっと表示するだけのメモ

やること

URDFファイルのロボットをpythonで表示だけプレビューします。
pybulletを使います。
ROS環境は不要です。
コードは適当ですが動くメモです。

コード

urdf_preview.py

import os
import pybullet as p
import pybullet_data
import time
import tkinter as tk
from tkinter import filedialog
import re


def find_directory(root_dir, target_dirs):
    """ルートディレクトリ内で特定のディレクトリを検索"""
    for dir_name in target_dirs:
        target_dir = os.path.join(root_dir, dir_name)
        if os.path.isdir(target_dir):
            return target_dir
    return None


def find_urdf_file(urdf_dir):
    """urdfディレクトリ内のURDFファイルを検索"""
    for file_name in os.listdir(urdf_dir):
        if file_name.endswith('.urdf'):
            return os.path.join(urdf_dir, file_name)
    return None


def add_default_material(urdf_content):
    """マテリアルが指定されていない<visual>タグにデフォルトのグレーを追加"""
    def add_material_to_visual(match):
        visual_tag = match.group(0)
        if '<material' not in visual_tag:
            # マテリアル指定がない場合、デフォルトのグレーのマテリアルを追加
            material_tag = '<material name="default_gray"><color rgba="0.5 0.5 0.5 1"/></material>'
            visual_tag = re.sub(r'(</geometry>)', r'\1' +
                                material_tag, visual_tag)
        return visual_tag

    # <visual>タグを検索して、マテリアルがない場合はグレーのマテリアルを追加
    urdf_content = re.sub(r'<visual>.*?</visual>',
                          add_material_to_visual, urdf_content, flags=re.DOTALL)
    return urdf_content


# 親ディレクトリの選択ダイアログを1回だけ表示
print("Starting script...")
print("Opening directory selection dialog...")
root = tk.Tk()
root.withdraw()
root_dir = filedialog.askdirectory(
    title="Please select the parent directory of urdf and meshes files.")
print(f"Directory selected: {root_dir}")

# URDFディレクトリを探す
print("Searching for URDF directory...")
urdf_dir = find_directory(root_dir, ['urdf', 'URDF', 'Urdf'])
if urdf_dir is None:
    print("URDF directory not found.")
    exit()
print(f"URDF directory found: {urdf_dir}")

# URDFファイルを探す
print("Searching for URDF file...")
urdf_file_path = find_urdf_file(urdf_dir)
if urdf_file_path is None:
    print("URDF file not found in the URDF directory.")
    exit()
print(f"URDF file found: {urdf_file_path}")

# メッシュディレクトリを探す
print("Searching for meshes directory...")
meshes_dir = find_directory(root_dir, ['meshes', 'mesh'])
if meshes_dir is None:
    print("Mesh directory not found.")
    exit()
print(f"Meshes directory found: {meshes_dir}")

# PyBulletシミュレータを開始
p.connect(p.GUI)

# URDFファイルの内容を読み込んで、package://からmeshesまでをメッシュディレクトリに置き換える
with open(urdf_file_path, 'r') as file:
    urdf_content = file.read()

# "package://"から"meshes"までをメッシュディレクトリに置き換える
urdf_content = re.sub(r'package://.*?/meshes', meshes_dir, urdf_content)

# マテリアルが指定されていない<visual>タグにグレーを追加
urdf_content = add_default_material(urdf_content)

# 一時的な修正後のURDFファイルを保存
temp_urdf_path = os.path.join(urdf_dir, "temp_cassie.urdf")
with open(temp_urdf_path, 'w') as file:
    file.write(urdf_content)

# メッシュディレクトリとURDFファイルのディレクトリを検索パスに追加
p.setAdditionalSearchPath(meshes_dir)
p.setAdditionalSearchPath(urdf_dir)
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# 修正したURDFファイルを読み込み
try:
    robot_id = p.loadURDF(temp_urdf_path, useFixedBase=True)
except Exception as e:
    print(f"Failed to load URDF file: {e}")
    p.disconnect()
    exit()

# 空中に固定するために、ロボットのベースリンクの位置を上に移動させる
base_position = [0, 0, 1.5]  # 1.5メートル空中に固定
p.resetBasePositionAndOrientation(robot_id, base_position, [0, 0, 0, 1])

# 継続的にシミュレーションを実行し、ウィンドウが閉じられるまで保持
try:
    while True:
        p.stepSimulation()
        time.sleep(1./240.)  # 実時間でのシミュレーションを行うためのウェイト
except KeyboardInterrupt:
    print("Simulation interrupted by user.")

# シミュレーションを終了
p.disconnect()

# 一時的なURDFファイルを削除
os.remove(temp_urdf_path)

使い方

python urdf_preview.py 

とします。
起動するとルードティレクトリを聞かれるので、urdfとmeshesの親ディレクトリを指定してください。

階層は以下のようになっている必要があります。（一般的なファイルはこのようになっています。）

[ルートディレクトリ(名前はファイルによる)] ←ここを指定する。
 │
 ├─ meshes 
 │   └ **.stl
 └─ urdf 
     └ **.urdf

ファイルは中空に固定された状態で表示されます。
マウスで視点を回転させたり、パーツを掴んで少し動かしたりできます。

URDFファイルのリンク集