【Pybulletサンプル解説】蛇ロボットを生成・操作する【snake.py】

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

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

本コードを実行すると、キーボードにより操作可能な蛇ロボットが生成されます。

使用している機能と仕組み

本コードでは以下の機能を使用して、ヘビ型ロボットの運動シミュレーションを実現しています。

createMultiBody によるリンク構成ロボットの生成

createMultiBody 関数を使用して、複数のリンクと関節からなるロボットを生成します。

p.createMultiBody(
    baseMass,
    baseCollisionShapeIndex,
    baseVisualShapeIndex,
    basePosition,
    baseOrientation,
    linkMasses=...,
    linkCollisionShapeIndices=...,
    linkVisualShapeIndices=...,
    linkPositions=...,
    linkOrientations=...,
    linkInertialFramePositions=...,
    linkInertialFrameOrientations=...,
    linkParentIndices=...,
    linkJointTypes=...,
    linkJointAxis=...,
    useMaximalCoordinates=...
)

• baseMass：ベースリンクの質量（kg）。
• baseCollisionShapeIndex：ベースリンクの衝突形状のID。
• baseVisualShapeIndex：ベースリンクの視覚形状のID。
• basePosition：ベースリンクの初期位置（[x, y, z]）。
• baseOrientation：ベースリンクの初期姿勢（クォータニオン [x, y, z, w]）。
• linkMasses：各リンクの質量のリスト。
• linkCollisionShapeIndices：各リンクの衝突形状のIDのリスト。
• linkVisualShapeIndices：各リンクの視覚形状のIDのリスト。
• linkPositions：各リンクの親リンクからの相対位置のリスト。
• linkOrientations：各リンクの親リンクからの相対姿勢のリスト（クォータニオン）。
• linkInertialFramePositions：各リンクの慣性フレームの位置のリスト。
• linkInertialFrameOrientations：各リンクの慣性フレームの姿勢のリスト（クォータニオン）。
• linkParentIndices：各リンクの親リンクのインデックスのリスト。
• linkJointTypes：各リンクの関節タイプのリスト（例：p.JOINT_REVOLUTE）。
• linkJointAxis：各リンクの関節軸のリスト。
• useMaximalCoordinates：最大座標系を使用するかどうかのフラグ（True または False）。

setJointMotorControl2 による関節駆動制御（POSITION_CONTROL）

setJointMotorControl2 関数を使用して、各関節に目標角度を設定し、モーターで制御します。

p.setJointMotorControl2(
    bodyUniqueId,
    jointIndex,
    controlMode,
    targetPosition,
    force
)

• bodyUniqueId：対象のボディの一意なID。
• jointIndex：制御対象の関節のインデックス。
• controlMode：制御モード（例：p.POSITION_CONTROL）。
• targetPosition：目標とする関節角度（ラジアン）。
• force：モーターが出力できる最大トルク。

異方性摩擦の設定

changeDynamics 関数を使用して、各リンクに異方性摩擦を設定します。

p.changeDynamics(
    bodyUniqueId,
    linkIndex,
    lateralFriction=...,
    anisotropicFriction=...
)

• bodyUniqueId：対象のボディの一意なID。
• linkIndex：対象のリンクのインデックス（ベースリンクは -1）。
• lateralFriction：接触面における横方向の摩擦係数。
• anisotropicFriction：異方性摩擦係数のリスト（[x, y, z]）。

これにより、進行方向への滑りやすさを変えることで、ヘビの前進運動を実現しています。

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

以下に元コードにコメントを追記した完全版を掲載します。

import pybullet as p
import time
import math
import pybullet_data

# PyBulletとGUIを接続
p.connect(p.GUI)

# 追加のURDFファイルなどのパスを設定
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# 地面を生成
plane = p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, plane)

# 各リンクを剛体として扱う設定（TrueでMaximalCoordinates使用）
useMaximalCoordinates = True
sphereRadius = 0.25

# リンクに使うボックス形状（ヘビの1節）
colBoxId = p.createCollisionShape(p.GEOM_BOX,
                                  halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1
visualShapeId = -1

# ヘビの構成要素（36節のリンク）を定義
link_Masses = []
linkCollisionShapeIndices = []
linkVisualShapeIndices = []
linkPositions = []
linkOrientations = []
linkInertialFramePositions = []
linkInertialFrameOrientations = []
indices = []
jointTypes = []
axis = []

# 36個のリンクを作成し、Y方向につなげていく
for i in range(36):
    link_Masses.append(1)
    linkCollisionShapeIndices.append(colBoxId)
    linkVisualShapeIndices.append(-1)
    linkPositions.append([0, sphereRadius * 2.0 + 0.01, 0])  # Y方向にずらす
    linkOrientations.append([0, 0, 0, 1])
    linkInertialFramePositions.append([0, 0, 0])
    linkInertialFrameOrientations.append([0, 0, 0, 1])
    indices.append(i)
    jointTypes.append(p.JOINT_REVOLUTE)  # 回転関節
    axis.append([0, 0, 1])  # Z軸回転

# ベース（最初のリンク）の初期位置と姿勢
basePosition = [0, 0, 1]
baseOrientation = [0, 0, 0, 1]

# ヘビ型ロボット（マルチボディ）を作成
sphereUid = p.createMultiBody(
    mass,
    colBoxId,
    visualShapeId,
    basePosition,
    baseOrientation,
    linkMasses=link_Masses,
    linkCollisionShapeIndices=linkCollisionShapeIndices,
    linkVisualShapeIndices=linkVisualShapeIndices,
    linkPositions=linkPositions,
    linkOrientations=linkOrientations,
    linkInertialFramePositions=linkInertialFramePositions,
    linkInertialFrameOrientations=linkInertialFrameOrientations,
    linkParentIndices=indices,
    linkJointTypes=jointTypes,
    linkJointAxis=axis,
    useMaximalCoordinates=useMaximalCoordinates
)

# 重力を設定
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)  # ステップ実行

# 各リンクに異方性摩擦を設定（進行方向への滑りやすさを変える）
anistropicFriction = [1, 0.01, 0.01]
p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction)
for i in range(p.getNumJoints(sphereUid)):
    p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction)

# 各種定数の初期化
dt = 1. / 240.
m_wavePeriod = 1.5
m_waveLength = 4
m_waveAmplitude = 0.4
m_waveFront = 0.0
m_steering = 0.0
m_segmentLength = sphereRadius * 2.0

# メインループ
while True:
    keys = p.getKeyboardEvents()
    # 左右キーでステアリング操作
    for k, v in keys.items():
        if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
            m_steering = -0.2
        if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
            m_steering = 0
        if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
            m_steering = 0.2
        if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
            m_steering = 0

    # 波の振幅スケールを先頭だけ弱くする
    scaleStart = 1.0
    if (m_waveFront < m_segmentLength * 4.0):
        scaleStart = m_waveFront / (m_segmentLength * 4.0)

    # 各関節に波を伝える
    for joint in range(p.getNumJoints(sphereUid)):
        segment = joint
        # 各リンクの波の位相を計算
        phase = (m_waveFront - (segment + 1) * m_segmentLength) / m_waveLength
        phase -= math.floor(phase)
        phase *= math.pi * 2.0
        targetPos = math.sin(phase) * scaleStart * m_waveAmplitude

        # ステアリング効果を適用（片側の波を弱める）
        if (m_steering > 0 and targetPos < 0):
            targetPos *= 1.0 / (1.0 + m_steering)
        if (m_steering < 0 and targetPos > 0):
            targetPos *= 1.0 / (1.0 - m_steering)

        # 関節モーターに目標角度を指令
        p.setJointMotorControl2(
            sphereUid,
            joint,
            p.POSITION_CONTROL,
            targetPosition=targetPos + m_steering,
            force=30
        )

    # 波を時間で前進させる
    m_waveFront += dt / m_wavePeriod * m_waveLength
    p.stepSimulation()
    time.sleep(dt)