はじめに

今回はpybulletのGitHubにあるHumanoidの仕様、動かし方を解説します。

Racecarよりもジョイントが多く、人型であるためにバランスをとりづらいので制御は難しいですが、動かせると面白いロボットだと思います。

(実行環境はGoogle Colaboratoryです)

Humanoid「NAO」

pybulletのdataには人型ロボットのサンプルとして人型ロボットの「NAO」のURDFモデルが提供されています。
人型ロボット「NAO」はALDEBARAN社(Pepperなども手がけている)が開発したロボットです。
世界の70カ国以上で教育や研究に用いられる非常に人気のあるロボットです。

Humanoid(NAO)の仕様

Humanoid(NAO)には全部で78個ジョイントがあります。
ロボットを制御するときはジョイントのタイプが「JOINT_REVOLUTE」のジョイントを指定して動かします。

jointIndex	名称	タイプ	最小値	最大値
0	base_link_fixedjoint	JOINT_FIXED	0.0	-1.0
1	HeadYaw	JOINT_REVOLUTE	-2.08567	2.08567
2	HeadPitch	JOINT_REVOLUTE	-0.671952	0.514872
3	gaze_joint	JOINT_FIXED	0.0	-1.0
4	Head/Touch/Front_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
5	CameraBottom_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
6	CameraBottom_optical_frame_fixedjoint	JOINT_FIXED	0.0	-1.0
7	CameraTop_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
8	CameraTop_optical_frame_fixedjoint	JOINT_FIXED	0.0	-1.0
9	InfraredL_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
10	Head/Touch/Rear_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
11	Head/Touch/Middle_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
12	InfraredR_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
13	LHipYawPitch	JOINT_REVOLUTE	-1.14529	0.740718
14	LHipRoll	JOINT_REVOLUTE	-0.379435	0.79046
15	LHipPitch	JOINT_REVOLUTE	-1.53589	0.48398
16	LKneePitch	JOINT_REVOLUTE	-0.0923279	2.11255
17	LAnklePitch	JOINT_REVOLUTE	-1.18944	0.922581
18	LAnkleRoll	JOINT_REVOLUTE	-0.397761	0.768992
19	LLeg_effector_fixedjoint	JOINT_FIXED	0.0	-1.0
20	LFoot/Bumper/Right_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
21	LFoot/FSR/FrontRight_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
22	LFoot/FSR/RearRight_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
23	LFoot/FSR/FrontLeft_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
24	LFoot/FSR/RearLeft_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
25	LFoot/Bumper/Left_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
26	RHipYawPitch	JOINT_REVOLUTE	-1.14529	0.740718
27	RHipRoll	JOINT_REVOLUTE	-0.79046	0.379435
28	RHipPitch	JOINT_REVOLUTE	-1.53589	0.48398
29	RKneePitch	JOINT_REVOLUTE	-0.0923279	2.11255
30	RAnklePitch	JOINT_REVOLUTE	-1.1863	0.932006
31	RAnkleRoll	JOINT_REVOLUTE	-0.768992	0.397761
32	RLeg_effector_fixedjoint	JOINT_FIXED	0.0	-1.0
33	RFoot/FSR/RearLeft_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
34	RFoot/FSR/RearRight_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
35	RFoot/Bumper/Right_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
36	RFoot/FSR/FrontRight_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
37	RFoot/Bumper/Left_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
38	RFoot/FSR/FrontLeft_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
39	LShoulderPitch	JOINT_REVOLUTE	-2.08567	2.08567
40	LShoulderRoll	JOINT_REVOLUTE	-0.314159	1.32645
41	LElbowYaw	JOINT_REVOLUTE	-2.08567	2.08567
42	LElbowRoll	JOINT_REVOLUTE	-1.54462	-0.0349066
43	LWristYaw	JOINT_REVOLUTE	-1.82387	1.82387
44	LHand	JOINT_REVOLUTE	0.0	1.0
45	LHand/Touch/Left_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
46	LHand/Touch/Back_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
47	LHand/Touch/Right_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
48	LFinger21	JOINT_REVOLUTE	0.0	-1.0
49	LFinger22	JOINT_REVOLUTE	0.0	-1.0
50	LFinger23	JOINT_REVOLUTE	0.0	-1.0
51	LFinger11	JOINT_REVOLUTE	0.0	-1.0
52	LFinger12	JOINT_REVOLUTE	0.0	-1.0
53	LFinger13	JOINT_REVOLUTE	0.0	-1.0
54	LThumb1	JOINT_REVOLUTE	0.0	-1.0
55	LThumb2	JOINT_REVOLUTE	0.0	-1.0
56	RShoulderPitch	JOINT_REVOLUTE	-2.08567	2.08567
57	RShoulderRoll	JOINT_REVOLUTE	-1.32645	0.314159
58	RElbowYaw	JOINT_REVOLUTE	-2.08567	2.08567
59	RElbowRoll	JOINT_REVOLUTE	0.0349066	1.54462
60	RWristYaw	JOINT_REVOLUTE	-1.82387	1.82387
61	RHand	JOINT_REVOLUTE	0.0	1.0
62	RHand/Touch/Right_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
63	RHand/Touch/Back_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
64	RHand/Touch/Left_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
65	RFinger21	JOINT_REVOLUTE	0.0	-1.0
66	RFinger22	JOINT_REVOLUTE	0.0	-1.0
67	RFinger23	JOINT_REVOLUTE	0.0	-1.0
68	RFinger11	JOINT_REVOLUTE	0.0	-1.0
69	RFinger12	JOINT_REVOLUTE	0.0	-1.0
70	RFinger13	JOINT_REVOLUTE	0.0	-1.0
71	RThumb1	JOINT_REVOLUTE	0.0	-1.0
72	RThumb2	JOINT_REVOLUTE	0.0	-1.0
73	Accelerometer_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
74	Sonar/Right_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
75	Sonar/Left_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
76	Gyrometer_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0
77	ChestBoard/Button_sensor_fixedjoint	JOINT_FIXED	0.0	-1.0

頭文字の「L」が左半身の関節、「R」が右半身の関節です。
また、関節の名称には「Pitch」「Roll」「Yaw」の表現が使われています。
ただ、実際に動きがイメージしづらいところもあると思うので後でGIFと共に解説します。

読み込み

pybulletのインストールとpybulletのgitのクローン

!pip install pybullet
!git clone https://github.com/bulletphysics/bullet3.git

必要なライブラリのインポート

import pybullet as p
import pybullet_data
import matplotlib.pyplot as plt
import numpy as np
from PIL import Image
physicsClient = p.connect(p.DIRECT)#pybulletの物理エンジンに接続

Humanoidの読み込みと表示

p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
floor = p.loadURDF("plane.urdf")

#初期位置の指定
StartPos = [0,0,0.35]
StartOrientation = p.getQuaternionFromEuler([0,0,3.14])


#humanoidのロード
naoId = p.loadURDF("/content/bullet3/data/humanoid/nao.urdf",StartPos, StartOrientation)

#カメラの設定
viewMatrix = p.computeViewMatrix(cameraEyePosition=[-2, 0, 1],cameraTargetPosition=[0, 0, 0.3],cameraUpVector=[0, 0, 1])
projectionMatrix = p.computeProjectionMatrixFOV(fov=20.0,aspect=1.5,nearVal=0.1,farVal=50)

#画像の取得
width, height, rgbImg, depthImg, segImg = p.getCameraImage(3600,2400,viewMatrix,projectionMatrix)
#p.getCameraImage(360,240,viewMatrix,projectionMatrix)だと少し画像が荒いので今回は変えてます

#画像の描画
plt.imshow(rgbImg)
plt.show()

動かし方

今回はHumanoidの関節の位置を指定して動かします。
p.setJointMotorControl2(動かしたいロボットのID,動かしたい関節,コントロール方法,目的の位置)

p.setJointMotorControl2(naoId, JointIndex, p.POSITION_CONTROL, targetPosition)

以下が実際に動かして結果をGIFファイルで保存するプログラムです。

p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-9.8)
timestep = 1. / 240.
p.setTimeStep(timestep)
floor = p.loadURDF("plane.urdf")

StartPos = [0,0,0.35]
StartOrientation = p.getQuaternionFromEuler([0,0,3.14])


naoId = p.loadURDF("/content/bullet3/data/humanoid/nao.urdf",StartPos, StartOrientation)

viewMatrix = p.computeViewMatrix(cameraEyePosition=[-2, 0, 1],cameraTargetPosition=[0, 0, 0.3],cameraUpVector=[0, 0, 1])
projectionMatrix = p.computeProjectionMatrixFOV(fov=20.0,aspect=1.5,nearVal=0.1,farVal=50)


frame = []
for t in range (400):
  p.setJointMotorControl2(naoId,1, p.POSITION_CONTROL, targetPosition = 1)
  p.stepSimulation()
  if t % 8 == 0:
    width, height, rgbImg, depthImg, segImg = p.getCameraImage(360,240,viewMatrix,projectionMatrix)
    frame.append(rgbImg)

images =[]
for im in frame:
  img = Image.fromarray(im)
  images.append(img)
images[0].save('HeadYaw1.gif',save_all=True, append_images=images[:], optimize=False, duration=40, loop=0)

実行結果

それぞれの関節を動かしたときの様子を紹介します。

頭

HeadYaw(1番)を1に設定したとき
頭を左右に動かす関節です。

HeadPitch(2番)を-0.6に設定したとき(ここでは上を向いています)
頭を前後に動かす関節です。

股関節

HipYawPitch(左:13,右:26)を共に-1に設定したとき
脚を広げたり閉じたりしながら、股関節を前後に曲げる関節です。

HipPitch(左:14,右:27)を共に-1に設定したとき
股関節を前後に曲げる関節です。

膝

KneePitch(左:16,右:29)を共に1に設定したとき
膝を曲げる関節です。

くるぶし

AnklePitch(左:17,右:30)を共に1に設定したとき
くるぶしを前後にまげる動かすときに使う関節

AnkleRoll(左:18,右:31)を左:1,右:-1に設定したとき
くるぶしを左右に傾けるときに使う関節

肩

ShoulderPitch(左:39,右:56番)をそれぞれ1.8に設定したとき
腕を体と並行に上げ下げするときに使う関節です。

ShoulderRoll(左:40,右:57)を左:1,右:-1に設定したとき
腕を広げる、閉じるときに使う関節です。

肘

ElbowYaw(左:41,右:58)を左:-1,右:1に設定したとき
肘を内旋、外旋させるときに使う関節です。

ElbowRoll(左:42,右:59)を左:-1,右:1に設定したとき
肘を曲げるときに使う関節です。

手首

WristYaw(左:43,右:60)を左:1,右:-1に設定したとき
手首を内旋、外旋させるときに使う関節です。

指

左手の指の関節全て(48~55)を-1に設定したとき
３本の指それぞれに２つずつ関節がついています。

最後に

今回はpybullet上でHumanoid「NAO」を動かす方法を紹介しました。
不明な点やコードに不備があったらご遠慮なくコメントでこ指摘ください。
pybulletの詳しい使い方やその他のロボットの動かし方は以下の記事で紹介しています。