Pythonで物理演算：PyBulletライブラリの使い方

アームロボット（３節リンク）を例に挙げて，物理シミュレーションのサンプルを示します．

（注意）Pythonのバージョン：3.10を使う．

右上のスライダーを動かすことで，３節アームロボットを駆動させることができる．

いきなり複雑なピッキングロボットを作成するのではなく，まずはこのような非常に単純なモデルで，
　・ピン位置（アームの長さ）
　・各ボディの重量重心
　・結合部分のパラメータ（damping，friction）
を変更したら，どういった挙動を示すのかを体感する．

pybullet_slider.py

import pybullet as p
import pybullet_data
import time
import math

# --- PyBullet 初期化 ---
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.8)

# --- 平面 ---
plane = p.loadURDF("plane.urdf")

# --- 3リンクアームのパラメータ ---
link_length = 0.3

# リンクの原点を「根元」に置き、形状を +X に伸ばす
col_id = p.createCollisionShape(
    p.GEOM_BOX,
    halfExtents=[link_length/2, 0.02, 0.02],
    collisionFramePosition=[link_length/2, 0, 0]   # ← 形状を +X にずらす
)

vis_id = p.createVisualShape(
    p.GEOM_BOX,
    halfExtents=[link_length/2, 0.02, 0.02],
    visualFramePosition=[link_length/2, 0, 0],      # ← 見た目も +X にずらす
    rgbaColor=[0.8, 0.3, 0.3, 1]
)

links = 3
link_masses = [1] * links
link_collision = [col_id] * links
link_visual = [vis_id] * links

# 子リンクは親リンクの先端 (x = link_length) に接続
link_positions = [[link_length, 0, 0] for _ in range(links)]
link_orientations = [[0, 0, 0, 1] for _ in range(links)]

# XZ 平面で動かすため、Y軸回転
joint_types = [p.JOINT_REVOLUTE] * links
joint_axes = [[0, 1, 0] for _ in range(links)]

# 親リンクの指定（0→1→2 の順で接続）
parents = [0, 1, 2]

arm = p.createMultiBody(
    baseMass=0,
    baseCollisionShapeIndex=-1,
    baseVisualShapeIndex=-1,
    basePosition=[0, 0, 0.1],
    linkMasses=link_masses,
    linkCollisionShapeIndices=link_collision,
    linkVisualShapeIndices=link_visual,
    linkPositions=link_positions,
    linkOrientations=link_orientations,
    linkInertialFramePositions=[[0, 0, 0]] * links,
    linkInertialFrameOrientations=[[0, 0, 0, 1]] * links,
    linkParentIndices=parents,
    linkJointTypes=joint_types,
    linkJointAxis=joint_axes
)

# --- スライダー作成 ---
slider_ids = []
for i in range(3):
    slider = p.addUserDebugParameter(f"Joint {i+1} Angle (deg)", -180, 180, 0)
    slider_ids.append(slider)

# --- メインループ ---
while p.isConnected():
    for j in range(3):
        angle_deg = p.readUserDebugParameter(slider_ids[j])
        angle_rad = math.radians(angle_deg)

        p.setJointMotorControl2(
            bodyUniqueId=arm,
            jointIndex=j,
            controlMode=p.POSITION_CONTROL,
            targetPosition=angle_rad,
            force=200,
            positionGain=0.5,
            velocityGain=1.0,
            maxVelocity=0
        )

    p.stepSimulation()
    time.sleep(1/240)