はじめに
今回は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の詳しい使い方やその他のロボットの動かし方は以下の記事で紹介しています。