【Pybulletサンプル解説】物体間の制約の生成と更新【constraint.py】

お疲れ様です。秋並です。

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

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

本コードを実行すると、箱状のオブジェクトが時間とともに制約が変更されていくことで、位置や角度が変化していく様子を確認できます。

使用している機能

本コードは、以下の機能を使用して「制約の作成、更新」を実現できます

• 制約の作成
• 制約の更新

制約の作成

createConstraint関数を使用することで、指定したボディ（オブジェクト）間に制約を設けることができます。

constraintId = pybullet.createConstraint(
    parentBodyId, 
    parentLinkIdx, 
    childBodyId, 
    childLinkIdx, 
    jointType, 
    jointAxis, 
    parentFramePosition, 
    childFramePosition)

• parentBodyId：制約する親ボディのID
• parentLinkIdx：親ボディのリンクのインデックス（ベースリンクの場合、-1を指定）
• childBodyId：制約する子ボディのID（ワールドに対して制約する場合、-1を指定）
• childLinkIdx：子ボディのリンクのインデックス（ベースリンクの場合、-1を指定）
• jointType：ジョイントの種類
• jointAxis：ジョイントの軸（[x, y, z]、jointTypeが固定軸の場合、[0, 0, 0]でもよい）
• parentFramePosition：親フレームの位置（[x, y, z]）
• childFramePosition：子フレームの位置（[x, y, z]）

サンプルコートでは、以下のように「箱状のオブジェクト」をワールド座標の[0, 0, 1]の位置に対して固定ジョイントで制約をかけています。

cid = p.createConstraint(
    cubeId, 
    -1, 
    -1, 
    -1, 
    p.JOINT_FIXED, 
    [0, 0, 0], 
    [0, 0, 0], 
    [0, 0, 1])

制約の更新

changeConstraint関数を使用することで、すでに作成された制約を更新することができます。

pybullet.changeConstraint(
    constraintId,
    pivot,
    jointChildFrameOrientation,
    maxFroce
)

• constraintId：制約のID
• pivot：（更新後の）子ボディのピボット位置（[x, y, z]）
• jointChildFrameOrientaion：（更新後の）子ボディのフレーム（[qx, qy, qz, qw]）

サンプルコードでは、箱状のオブジェクトの「位置（[a, 0, 1]）」と「向き（オイラー角[a, 0, 1]をクォータニオンに変換したもの）」が時間とともに変化していきます。

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

サンプルコードにコメントをつけたものが以下になります（もともとあった不要と思われるコメント等については削除しています）

import pybullet as p
import time
import math
import pybullet_data

# GUIモードでpybulletを接続
p.connect(p.GUI)

# Pybulletに関するデータのパスを取得
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# 床を生成
p.loadURDF("plane.urdf")

# 箱状のオブジェクトを生成
cubeId = p.loadURDF("cube_small.urdf", 0, 0, 1)

# 重力を設定
p.setGravity(0, 0, -10)

# リアルタイムシミュレーションを有効化
p.setRealTimeSimulation(1)

# キューブを[0, 0, 1]の位置に固定
cid = p.createConstraint(cubeId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1])

print(cid)
print(p.getConstraintUniqueId(0))

# 立方体の回転角度を-πに設定
a = -math.pi

while 1:
  # 回転角度を+0.01する
  a = a + 0.01
  
  if (a > math.pi):
    a = -math.pi
  
  time.sleep(.01)
  
  p.setGravity(0, 0, -10)
  
  # 箱の位置を「角度a」に応じて更新
  pivot = [a, 0, 1]

  # オイラー角からクォータニオンを計算
  orn = p.getQuaternionFromEuler([a, 0, 0])

  # 立方体の新しい位置と回転を設定
  p.changeConstraint(cid, pivot, jointChildFrameOrientation=orn, maxForce=50)

# 制約を解除（↑が無限ループなので、実際はたどり着かない）
p.removeConstraint(cid)