PyBulletでスクリプト化した把持により立方体をピックアンドプレースする

作るもの

本プロジェクトでは、シミュレーション上のアームを動かすピックアンドプレースのプログラムを一から書きます。モデルライブラリからは何も借りません。ロボットは自分で定義します。2本指の平行グリッパを備えた5関節のアームを、直方体と円柱からなるURDF文字列として記述します。逆運動学（IK）は、グリッパが常に真下を向くように閉形式で解きます。そのうえで、接近、閉じる、持ち上げ、移動、配置、解放、退避という定番のシーケンスをスクリプトにします。立方体はロボットの軸に対して17度回転した状態から始まります。そのため、指を立方体の面にそろえるには手首を回す必要があります。

PyBulletはヘッドレス（p.DIRECT）で動作し、床には付属のplane.urdfを使います。プログラムはフェーズごとに1行を出力します。最後に立方体の最終位置と、10 mmの許容誤差に対する合否を表示します。筆者らの実行では、シミュレーション時間で10.8 s経過した後、立方体は目標から0.6 mmの位置に直立した状態で置かれました。

先に2リンクアームのプロジェクトを済ませておくと理解しやすくなります。このアームのIKは、鉛直面内で同じ余弦定理の解を再利用するためです。モデル形式はURDFとMJCFのチートシートで扱っています。スクリプト化した把持の位置づけは、マニピュレーションと把持のハブにまとめています。

必要なもの

• Python 3.10以降とpybullet・numpy（pip install pybullet numpy）。筆者らはApple SiliconのMac mini上で、PyBullet 3.2.5とNumPy 2.5.4を使って実行しました。
• pybullet_data。pybulletパッケージに同梱されています。使うのはplane.urdfのみです。
• GPUもディスプレイも不要です。実行にかかるCPU時間は数秒です。

設計

ロボット

このアームは、小型マニピュレータでよく使われる構成です。ベースのヨー関節が、アーム全体を鉛直軸まわりに回します。ターレットのy軸まわりに回る3つのピッチ関節（肩、肘、手首）により、アームは1つの鉛直面内にとどまります。手先自身の軸まわりの手首ロールにより、任意のヨー角の物体に指をそろえられます。グリッパには、手先のy軸に沿ってスライドする2本の直動の指があります。固定関節により、質量がないに等しいtcpリンクを手先プレートの55 mm下、指パッドの間に配置します。このリンクの質量は1 gです。PyBulletが勝手に慣性を設定しないように質量を与えています。

URDF内のすべての可動関節には、下限・上限、最大力（effort）、速度を指定した<limit>があります。URDFのリファレンスパーサであるurdfdomは、これのない回転関節や直動関節を受け付けません。衝突形状を持つすべてのリンクには<inertial>ブロックがあります。PyBulletは慣性データのないリンクに質量1 kgと単位慣性を与え、警告を出すためです。形状は衝突形状のみです。p.DIRECTでは何も描画されないので、視覚形状は読み込み時間を増やすだけです。

グリッパを真下に向けた閉形式の逆運動学

上方からのピックアンドプレースでは、tcpを位置(x, y, z)に置き、手先の軸を真下に向け、指を指定したヨー角にそろえる必要があります。5関節に対して拘束が5つなので、解は閉形式で求まります。

1. ベースのヨー。 q1 = atan2(y, x)により、アームの平面が目標点を通るように回します。
2. 手首の位置。 手先は下を向いているので、手首のピッチ軸はtcpの真上L_hand = 0.095 mの位置にあります。アームの平面内で、手首は肩に対して水平距離r = √(x² + y²)、高さh = z + 0.095 - 0.20にあります。
3. 肩と肘。 これも2リンクの問題で、リンク長は0.30 mと0.25 mです。アームはゼロ角度で真上を向くので、角度は鉛直から測ります。肘の角度は余弦定理から、肩の角度は2回のatan2呼び出しから求めます。コードは常に、肘が手首より上にある解の枝を選びます。
4. 手首のピッチ。 3つのピッチ角の和が、手先の鉛直からの角度になります。手先を下に向けるにはこの和がπでなければならないので、q4 = π - q2 - q3です。
5. 手首のロール。 手先を下に向けると、手先は裏返しになります。裏返った状態では、ワールド座標系での手先のヨーはq1 + q5ではなくq1 - q5になります。この符号は間違えやすい箇所です。筆者らも最初の版で間違えました。指は誤った角度で閉じ、立方体の角付近に当たって、つかむ際に立方体をねじってしまいました。立方体は90度ごとに同じ見た目になります。そこでコードはq5を±45度の範囲に折り返し、必要以上に手首を回さないようにしています。

move_tcpは、始点と終点で滑らかに加減速する補間（3s² - 2s³）により、tcpを直交座標空間の直線に沿って動かします。1/240 sの各ステップで逆運動学を解き、関節角をPyBulletの位置コントローラに送ります。直線移動は下降時と上昇時に重要です。ホバー姿勢と把持姿勢の間を関節空間で移動すると、指が横に振れて立方体の中を通り抜けてしまいます。

把持シーケンス

立方体は一辺40 mm、質量0.1 kgです。初期位置は(0.42, -0.12) m、ヨー角は0.3 radで、目標位置は(0.25, 0.35) mです。tcpは高さ0.15 mでホバーし、0.03 mで把持します。このとき指先は床から5 mm上にあります。閉じる際、プログラムは各指を中心線まで動かすよう指令し、力の上限を20 Nとします。指は立方体の面で止まり、その上限の力で挟み続けます。指と立方体の摩擦係数は1.0なので、得られる摩擦力は立方体の重さ0.98 Nを大きく上回ります。解放時のtcpは把持時より5 mm高い位置にあります。そのため立方体は床に押し付けられず、約5 mm落下して床に着きます。

連動する指

力の上限をそれぞれ独立に持つ2本の指でも、立方体を挟むことはできます。しかし、中央で挟む保証はありません。一方の指が先に当たり、もう一方が接触するまで立方体を横に押してしまいます。さらに移動中、各指は力を保っているだけなので、2本の指がそろって横にずれることがあります。筆者らの最初の版はまさにこうなりました。立方体は持ち上げ後に中心から7 mm、配置位置では15 mmずれ、10 mmの許容誤差を満たせませんでした。

実際の平行グリッパは、ラックアンドピニオンやリンク機構など、指同士を結びつける機構でこれを防いでいます。コードでも同様に、2つの指関節の間にギア比-1のPyBulletのギア拘束を置いています。両関節の軸は外向きなので、この拘束により2本の指の開き量が等しくなります。この変更後、持ち上げ後の立方体の中心ずれは0.4 mmとなり、目標から1 mm以内に着地します。

コード

プログラム全体は1ファイルです。pick_place.pyとして保存してください。

# pick_place.py: a 5-joint arm with a parallel gripper picks a cube and places it, in PyBullet (DIRECT mode).
import math
import os
import tempfile

import numpy as np
import pybullet as p
import pybullet_data

H_SHOULDER = 0.20     # m, shoulder axis height above the floor
L_UPPER, L_FORE = 0.30, 0.25
L_HAND = 0.095        # m, wrist pitch axis to the tool centre point (between the finger pads)

ARM_URDF = f"""<?xml version="1.0"?>
<robot name="pick_arm">
  <link name="base_link">
    <inertial><origin xyz="0 0 0.05"/><mass value="3.0"/><inertia ixx="0.01" iyy="0.01" izz="0.01" ixy="0" ixz="0" iyz="0"/></inertial>
    <visual><origin xyz="0 0 0.05"/><geometry><cylinder radius="0.08" length="0.1"/></geometry></visual>
    <collision><origin xyz="0 0 0.05"/><geometry><cylinder radius="0.08" length="0.1"/></geometry></collision>
  </link>
  <link name="turret">
    <inertial><origin xyz="0 0 0.05"/><mass value="0.5"/><inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/></inertial>
    <collision><origin xyz="0 0 0.05"/><geometry><cylinder radius="0.05" length="0.1"/></geometry></collision>
  </link>
  <link name="upper_arm">
    <inertial><origin xyz="0 0 0.15"/><mass value="0.5"/><inertia ixx="0.004" iyy="0.004" izz="0.0002" ixy="0" ixz="0" iyz="0"/></inertial>
    <collision><origin xyz="0 0 0.15"/><geometry><box size="0.04 0.04 {L_UPPER}"/></geometry></collision>
  </link>
  <link name="forearm">
    <inertial><origin xyz="0 0 0.125"/><mass value="0.4"/><inertia ixx="0.002" iyy="0.002" izz="0.0001" ixy="0" ixz="0" iyz="0"/></inertial>
    <collision><origin xyz="0 0 0.125"/><geometry><box size="0.035 0.035 {L_FORE}"/></geometry></collision>
  </link>
  <link name="wrist">
    <inertial><origin xyz="0 0 0.02"/><mass value="0.1"/><inertia ixx="0.0001" iyy="0.0001" izz="0.0001" ixy="0" ixz="0" iyz="0"/></inertial>
    <collision><origin xyz="0 0 0.02"/><geometry><cylinder radius="0.025" length="0.04"/></geometry></collision>
  </link>
  <link name="hand">
    <inertial><origin xyz="0 0 0.01"/><mass value="0.1"/><inertia ixx="0.0001" iyy="0.0001" izz="0.0001" ixy="0" ixz="0" iyz="0"/></inertial>
    <collision><origin xyz="0 0 0.01"/><geometry><box size="0.03 0.12 0.02"/></geometry></collision>
  </link>
  <link name="finger_left">
    <inertial><origin xyz="0 0.005 0.03"/><mass value="0.02"/><inertia ixx="0.00001" iyy="0.00001" izz="0.00001" ixy="0" ixz="0" iyz="0"/></inertial>
    <collision><origin xyz="0 0.005 0.03"/><geometry><box size="0.02 0.01 0.06"/></geometry></collision>
  </link>
  <link name="finger_right">
    <inertial><origin xyz="0 -0.005 0.03"/><mass value="0.02"/><inertia ixx="0.00001" iyy="0.00001" izz="0.00001" ixy="0" ixz="0" iyz="0"/></inertial>
    <collision><origin xyz="0 -0.005 0.03"/><geometry><box size="0.02 0.01 0.06"/></geometry></collision>
  </link>
  <link name="tcp">
    <inertial><mass value="0.001"/><inertia ixx="1e-7" iyy="1e-7" izz="1e-7" ixy="0" ixz="0" iyz="0"/></inertial>
  </link>

  <joint name="base_yaw" type="revolute">
    <parent link="base_link"/><child link="turret"/><origin xyz="0 0 0.1"/><axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="50" velocity="2.0"/>
  </joint>
  <joint name="shoulder" type="revolute">
    <parent link="turret"/><child link="upper_arm"/><origin xyz="0 0 0.1"/><axis xyz="0 1 0"/>
    <limit lower="-1.6" upper="1.6" effort="50" velocity="2.0"/>
  </joint>
  <joint name="elbow" type="revolute">
    <parent link="upper_arm"/><child link="forearm"/><origin xyz="0 0 {L_UPPER}"/><axis xyz="0 1 0"/>
    <limit lower="-2.6" upper="2.6" effort="30" velocity="2.0"/>
  </joint>
  <joint name="wrist_pitch" type="revolute">
    <parent link="forearm"/><child link="wrist"/><origin xyz="0 0 {L_FORE}"/><axis xyz="0 1 0"/>
    <limit lower="-2.6" upper="2.6" effort="10" velocity="3.0"/>
  </joint>
  <joint name="wrist_roll" type="revolute">
    <parent link="wrist"/><child link="hand"/><origin xyz="0 0 0.04"/><axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="5" velocity="3.0"/>
  </joint>
  <joint name="finger_left_joint" type="prismatic">
    <parent link="hand"/><child link="finger_left"/><origin xyz="0 0 0.02"/><axis xyz="0 1 0"/>
    <limit lower="0.0" upper="0.05" effort="20" velocity="0.1"/>
  </joint>
  <joint name="finger_right_joint" type="prismatic">
    <parent link="hand"/><child link="finger_right"/><origin xyz="0 0 0.02"/><axis xyz="0 -1 0"/>
    <limit lower="0.0" upper="0.05" effort="20" velocity="0.1"/>
  </joint>
  <joint name="tcp_joint" type="fixed">
    <parent link="hand"/><child link="tcp"/><origin xyz="0 0 0.055"/>
  </joint>
</robot>
"""

ARM_JOINTS = [0, 1, 2, 3, 4]       # base_yaw, shoulder, elbow, wrist_pitch, wrist_roll
FINGERS = [5, 6]
TCP = 7
DT = 1.0 / 240.0


def arm_ik(x: float, y: float, z: float, grasp_yaw: float) -> list[float]:
    """Analytic IK with the gripper pointing straight down and its fingers closing across grasp_yaw + 90 deg."""
    q1 = math.atan2(y, x)
    r = math.hypot(x, y)                       # horizontal distance in the arm's vertical plane
    h = z + L_HAND - H_SHOULDER                # wrist pitch axis height relative to the shoulder
    c3 = (r * r + h * h - L_UPPER ** 2 - L_FORE ** 2) / (2 * L_UPPER * L_FORE)
    if abs(c3) > 1:
        raise ValueError("target out of reach")
    q3 = math.acos(c3)                         # elbow bent forward, elbow above the wrist
    q2 = math.atan2(r, h) - math.atan2(L_FORE * math.sin(q3), L_UPPER + L_FORE * math.cos(q3))
    q4 = math.pi - q2 - q3                     # hand axis points down
    # With the hand flipped to point down, its yaw in the world is q1 - q5.
    # A cube looks the same every 90 deg, so pick the smallest roll that lines the fingers up with its faces.
    q5 = q1 - grasp_yaw
    q5 = (q5 + math.pi / 4) % (math.pi / 2) - math.pi / 4
    return [q1, q2, q3, q4, q5]


sim_steps = 0


def step(n: int = 1) -> None:
    global sim_steps
    for _ in range(n):
        p.stepSimulation()
        sim_steps += 1


def set_gripper(robot: int, opening: float, force: float) -> None:
    """opening is the distance of each finger from the centre line, in metres."""
    p.setJointMotorControlArray(robot, FINGERS, p.POSITION_CONTROL, targetPositions=[opening] * 2, forces=[force] * 2)


def move_tcp(robot: int, start: np.ndarray, goal: np.ndarray, yaw: float, seconds: float) -> None:
    """Straight-line Cartesian move of the tool centre point, solving IK at every control step."""
    n = int(seconds / DT)
    for i in range(1, n + 1):
        s = i / n
        s = 3 * s * s - 2 * s ** 3              # smooth start and stop
        q = arm_ik(*((1 - s) * start + s * goal), yaw)
        p.setJointMotorControlArray(robot, ARM_JOINTS, p.POSITION_CONTROL, targetPositions=q,
                                    forces=[50, 50, 30, 10, 5], positionGains=[0.3] * 5)
        step()
    step(60)                                   # 0.25 s to settle


def tcp_pos(robot: int) -> np.ndarray:
    return np.array(p.getLinkState(robot, TCP, computeForwardKinematics=True)[4])


p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
p.setTimeStep(DT)
p.loadURDF("plane.urdf")

urdf_path = os.path.join(tempfile.mkdtemp(), "pick_arm.urdf")
with open(urdf_path, "w") as f:
    f.write(ARM_URDF)
robot = p.loadURDF(urdf_path, useFixedBase=True)
# Couple the fingers like the linkage in a real parallel gripper, so the cube is always centred between them.
gear = p.createConstraint(robot, FINGERS[0], robot, FINGERS[1], p.JOINT_GEAR, [0, 1, 0], [0, 0, 0], [0, 0, 0])
p.changeConstraint(gear, gearRatio=-1, erp=0.1, maxForce=50)

CUBE = 0.04                                    # m, edge length
pick = np.array([0.42, -0.12, CUBE / 2])
place = np.array([0.25, 0.35, CUBE / 2])
cube_yaw = 0.3                                 # rad, the cube is not square to the robot
col = p.createCollisionShape(p.GEOM_BOX, halfExtents=[CUBE / 2] * 3)
cube = p.createMultiBody(baseMass=0.1, baseCollisionShapeIndex=col, basePosition=pick.tolist(),
                         baseOrientation=p.getQuaternionFromEuler([0, 0, cube_yaw]))
for body, link in [(cube, -1), (robot, FINGERS[0]), (robot, FINGERS[1])]:
    p.changeDynamics(body, link, lateralFriction=1.0)

# Start above the cube with the gripper open.
GRASP_Z = 0.03                                 # TCP height when grasping: finger tips 5 mm above the floor
HOVER_Z = 0.15
home = np.array([pick[0], pick[1], HOVER_Z])
for j, q in zip(ARM_JOINTS, arm_ik(*home, cube_yaw)):
    p.resetJointState(robot, j, q)
set_gripper(robot, 0.035, 20)
step(120)
print(f"start: tcp at {np.round(tcp_pos(robot), 4)}, cube at {np.round(p.getBasePositionAndOrientation(cube)[0], 4)}")

# Approach, close, lift, move, place, release, retreat.
above_pick, at_pick = home, np.array([pick[0], pick[1], GRASP_Z])
above_place, at_place = np.array([place[0], place[1], HOVER_Z]), np.array([place[0], place[1], GRASP_Z + 0.005])
move_tcp(robot, above_pick, at_pick, cube_yaw, 1.0)
print(f"approach: tcp error {np.linalg.norm(tcp_pos(robot) - at_pick) * 1000:.1f} mm")
set_gripper(robot, 0.0, 20)                    # aim past the cube faces so the fingers squeeze with 20 N
step(240)
grip = p.getJointState(robot, FINGERS[0])[0] + p.getJointState(robot, FINGERS[1])[0]
contacts = len(p.getContactPoints(robot, cube))
print(f"close: finger gap {grip * 1000:.1f} mm (cube is {CUBE * 1000:.0f} mm), contact points {contacts}")
move_tcp(robot, at_pick, above_pick, cube_yaw, 1.0)
offset = np.array(p.getBasePositionAndOrientation(cube)[0]) - tcp_pos(robot)
print(f"lift: cube centre {-offset[2] * 1000:.1f} mm below the tcp, {np.linalg.norm(offset[:2]) * 1000:.1f} mm off-centre sideways")
move_tcp(robot, above_pick, above_place, cube_yaw, 2.0)
move_tcp(robot, above_place, at_place, cube_yaw, 1.0)
set_gripper(robot, 0.035, 20)
step(240)
print("release: gripper open")
move_tcp(robot, at_place, above_place, cube_yaw, 1.0)
step(240)

pos, orn = p.getBasePositionAndOrientation(cube)
pos = np.array(pos)
roll, pitch, yaw = p.getEulerFromQuaternion(orn)
xy_err = np.linalg.norm(pos[:2] - place[:2])
TOL = 0.01
print(f"cube final position: x={pos[0]:.4f} y={pos[1]:.4f} z={pos[2]:.4f} m")
print(f"cube final tilt: roll {math.degrees(roll):.2f} deg, pitch {math.degrees(pitch):.2f} deg, yaw {math.degrees(yaw):.1f} deg")
print(f"xy error {xy_err * 1000:.1f} mm, tolerance {TOL * 1000:.0f} mm: {'PASS' if xy_err < TOL and abs(pos[2] - CUBE / 2) < 0.005 else 'FAIL'}")
print(f"simulated {sim_steps * DT:.1f} s in {sim_steps} steps")
p.disconnect()

URDFは一時ファイルに書き出します。p.loadURDFがディスクから読み込むためです。useFixedBase=Trueはbase_linkをワールドに固定します。PyBulletでは、子リンクのリンクインデックスは、そのリンクを生成する関節のインデックスと同じです。このモデルではp.getJointInfoがbase_yawを関節0、tcp_jointを関節7として列挙します。そのためTCP = 7です。モデルを変更したときは、必ずp.getJointInfoでインデックスを確認してください。

実行

python -m pip install pybullet numpy
python pick_place.py

筆者らのMacでは、実行に約0.13 sかかります。PyBulletは最初に、ビルド日時を標準エラーに出力します。この行はエラーではありません。日付は使用しているPyBulletのビルドによって変わります。

テスト

以下は筆者らの実行での出力です。DIRECTモードのPyBulletは同一マシン上では決定的なので、同じ行が表示されるはずです。別のプラットフォームでは、一部の値の最後の桁が異なることがあります。

pybullet build time: Oct 21 2025 16:30:45
start: tcp at [ 0.42 -0.12  0.15], cube at [ 0.42 -0.12  0.02]
approach: tcp error 0.0 mm
close: finger gap 39.9 mm (cube is 40 mm), contact points 6
lift: cube centre 10.5 mm below the tcp, 0.4 mm off-centre sideways
release: gripper open
cube final position: x=0.2506 y=0.3503 z=0.0200 m
cube final tilt: roll 0.00 deg, pitch -0.00 deg, yaw 17.2 deg
xy error 0.6 mm, tolerance 10 mm: PASS
simulated 10.8 s in 2580 steps

1行ずつ見ていきます。

• start。 アームは立方体の上方の逆運動学解にリセットされます。tcpは逆運動学が示したとおりの位置にあります。
• approach。 1 sの下降と0.25 sの整定の後、tcpの誤差は丸めて0.0 mmです。
• close。 40 mmの立方体に対して指の間隔が39.9 mmなので、指が面を押していることがわかります。PyBulletは接触部でわずかなめり込みを許容します。接触点の数から、ロボットが立方体に触れていることを確認できます。
• lift。 立方体は手先と一緒に持ち上がりました。中心はtcpの10.5 mm下（把持高さ30 mm、立方体中心20 mm）にあります。横方向のずれは0.4 mmだけなので、滑っていません。
• final positionとfinal tilt。 立方体は床の上にあり（z = 0.0200 mは一辺の半分）、ロールとピッチはゼロです。ヨーは17.2度のままで、初期値の0.3 radと変わりません。
• PASS。 水平誤差は0.6 mmで、10 mmの許容誤差に十分収まっています。

FAILになった場合は、まずliftの行を確認してください。横方向のずれが大きい場合は、指が連動していません。立方体がtcpの下にまったくない場合は、把持に失敗しています。原因はたいてい、手首ロールの符号の誤りか摩擦の不足です。

実機では

実機でもシーケンスは同じで、シーケンス自体は簡単な部分です。変わるのは次の点です。

• 立方体の位置がわからない。 シミュレーションでは、立方体の姿勢は変数です。実機ではカメラと認識処理から得られ、数mmと1〜2度程度の誤差を含みます。この誤差を許容できるようにグリッパを十分に開いてください。持ち上げる前に、指の位置や力の読み値で把持を確認してください。
• アームは完全には追従しない。 実際の関節には、バックラッシ、コンプライアンス、キャリブレーション誤差があります。データシートに記載される繰り返し精度は、教示した点にアームがどれだけ正確に戻るかを表す値です。モデルから計算した点にどれだけ正確に到達するかについては何も示しません。作業が許すなら、ピック点とプレース点は実機で教示してください。
• 把持力は選ぶものである。 20 Nは0.1 kgの立方体を大きな余裕を持って保持できますが、紙コップなら潰してしまいます。多くの電動グリッパは把持力を設定できます。移動中の加速度がかかっても部品を保持できる、最も小さい力を選んでください。
• 衝突は現実に起こる。 毎回の移動の前に、テーブルや治具との衝突判定を加えてください。作業領域の境界付近では速度を落としてください。初めてプログラムを動かすときは、非常停止ボタンをすぐ押せるようにしておいてください。

下のブロックは、同じシーケンスを実機で行う場合の形を示します。実機のロボットが必要です。RobotArmとGripperは使用するロボットのドライバやSDKの代わりで、メソッド名は仮のものです。インターフェースはベンダーごとに異なるため、このままでは実行できません。

# needs-hardware
from my_robot_driver import Gripper, RobotArm  # placeholder for your vendor's SDK

HOVER_Z, GRASP_Z = 0.15, 0.03       # m
arm, gripper = RobotArm("192.168.1.10"), Gripper()


def pick_and_place(pick_xyz, place_xyz, yaw_rad):
    gripper.open()
    arm.move_linear(pick_xyz[0], pick_xyz[1], HOVER_Z, yaw_rad, speed=0.1)
    arm.move_linear(pick_xyz[0], pick_xyz[1], GRASP_Z, yaw_rad, speed=0.02)
    gripper.close(force=20.0)
    if not gripper.holding_object():
        raise RuntimeError("grasp failed, stopping before the lift")
    arm.move_linear(pick_xyz[0], pick_xyz[1], HOVER_Z, yaw_rad, speed=0.05)
    arm.move_linear(place_xyz[0], place_xyz[1], HOVER_Z, yaw_rad, speed=0.1)
    arm.move_linear(place_xyz[0], place_xyz[1], GRASP_Z + 0.005, yaw_rad, speed=0.02)
    gripper.open()
    arm.move_linear(place_xyz[0], place_xyz[1], HOVER_Z, yaw_rad, speed=0.05)

垂直方向の移動は、水平方向の移動より遅くしています。接触付近ではこれが一般的なやり方です。また、閉じる動作と持ち上げの間に把持の確認を入れています。

試してみること

1. 指の連動を外す。 ギア拘束の2行を削除して再実行してください。筆者らの実行では、立方体は目標から15.1 mmの位置に着地し、プログラムはFAILを出力しました。
2. 摩擦を減らす。 横方向の摩擦係数を0.1、次に0.05に設定し、持ち上げ中に立方体が滑り落ちる値を探してください。指1本あたり20 Nの力から予想される値と比べてください。
3. PyBulletの逆運動学を使う。 arm_ikを、下向きの四元数を与えたp.calculateInverseKinematics(robot, TCP, target, targetOrientation)に置き換えてください。PyBulletの四元数は(x, y, z, w)の順です。接近後のtcp誤差を比べてください。
4. 速く動かす。 すべての移動時間を半分にしてください。liftの行で滑りを、approachの行で追従誤差を確認してください。そのうえでpositionGainsを上げて補償してください。
5. 立方体を2つ積む。 2つ目の立方体を追加し、配置高さを40 mm上げて1つ目の上に置いてください。
6. 初期位置をランダムにする。 シードを固定した乱数生成器で、到達可能な領域内から立方体の初期位置とヨーを引いてください。50回の実行のうち何回成功するか数えてください。

まとめ

• 直方体と円柱からなるURDFで、すべての可動関節に<limit>、すべてのリンクに<inertial>を付ければ、PyBulletで動くアームとグリッパには十分です。
• グリッパを真下に向けると、5関節の逆運動学は閉形式で解けます。ベースのヨー、肩と肘の2リンク解、手首ピッチ = π - q2 - q3、そして裏返った手先を考慮する必要がある手首ロールです。
• 物体の近くではtcpを直交座標空間の直線で動かし、制御ステップごとに逆運動学を解きます。
• 平行グリッパの指は連動させます。シミュレーションではギア拘束で、実機では機構で実現します。力の上限を独立に持つ2本の指では、物体が横にずれます。
• 確認できる値を出力します。tcp誤差、指の間隔、持ち上げ後の滑り、許容誤差に対する最終姿勢です。

PyBullet Quickstart Guideは、ここで使ったloadURDF・setJointMotorControlArray・createConstraintと接触の問い合わせ関数を説明しています。モデル形式とシミュレータの選び方は、用語集のURDFの項とシミュレーションとデジタルツインのハブで扱っています。