Zenn
🤖

pybulletでロボットシミュレーション

に公開
robot
PyBullet
tech

概要

対象読者

  • ロボットシミュレーションをやってみたい人

内容

  • pybulletのインストール方法
  • racecarをキーボードで操作する

本編

pyBulltとは

pythonでロボットシミュレーションができるライブラリで比較的簡単に行えるのが特徴です。
ロボットシミュレーションというとROSのgazeboとかがありますがgazeboだとROSの知識が必要でシミュレーションを行うまでに時間が掛かりそう。そのため、簡単にやりたい場合はpybulltがおすすめです。

インストール

ライブラリをインストール

pip install pybullet

gitのクローンを行う

git clone https://github.com/bulletphysics/bullet3.git

このgitに入っているracecarでロボットを操作します
windows環境で行う人はzipファイルでダウンロードして展開する方法が多分楽かも

プログラム

JointIDの確認

pybulletでロボットを動かすのはジョイントに速度や力を加えることで行います
その際に、ジョイント指定するためにIDを以下のコードで取得します。

import pybullet as p
import pybullet_data

def main():
    # --- PyBulletの初期設定 ---
    physicsClient = p.connect(p.GUI)

    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    file_path = "racecar/racecar.urdf"
    car_Id = p.loadURDF(file_path)

    num_joints = p.getNumJoints(car_Id)
    for i in range(num_joints):
        info_jonit = p.getJointInfo(car_Id,i)
        print(f"Joint ID: {info_jonit[0]}, Name: {info[1].decode('utf-8')}")
        
if __name__ == "__main__":
    main()

このコマンドを実行すると一瞬だけウィンドウが開かれて閉じますが問題ないです。（すぐに処理が終わってウィンドウが閉じるだけなので）
ここで画像のようなデバックが返って来ます

これはどのジョイントがどのIDあ振られているのか表しています。
例えば、base_link_jointを動かす際はid0を指定すればいいのです。

実際に動かしてみる

キーボードで操作するためのコードが以下のような感じです。
uで前進、dで後進、lで左折、rで右折します。

import pybullet as p
import pybullet_data
import time

class ControllRacecar():
    def __init__(self,setspeed):
        self.setspeed = setspeed
    
    def up(self):
        speedlist = [self.setspeed,self.setspeed,self.setspeed,self.setspeed]
        return speedlist
    
    
    def back(self):
        self.setspeed *= -1
        speedlist = [self.setspeed,self.setspeed,self.setspeed,self.setspeed]
        self.setspeed *= -1
        return speedlist
    
    def left(self):
        speedlist = [self.setspeed*0.5,self.setspeed*1.5,self.setspeed*0.5,self.setspeed*1.5]
        return speedlist
    
    def right(self):
        speedlist = [self.setspeed*1.5,self.setspeed*0.5,self.setspeed*1.5,self.setspeed*0.5]
        return speedlist
    
    def stop(self):
        speedlist = [0,0,0,0]
        return speedlist
    
def main():
    contrallrace = ControllRacecar(10)
    # --- PyBulletの初期設定 ---
    physicsClient = p.connect(p.GUI)

    p.resetSimulation()
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setGravity(0,0,-9.8)
    timestep = 1. / 240.
    p.setTimeStep(timestep)

    planeId = p.loadURDF("plane.urdf")

    StartPos = [0,0,0]
    StartOrient = p.getQuaternionFromEuler([0,0,3.14])
    file_path = "racecar/racecar.urdf"
    carId = p.loadURDF(file_path,StartPos, StartOrient)

    p.resetDebugVisualizerCamera(5, 45, -30, StartPos)

    running = True
    try:
        while running:
            keys = p.getKeyboardEvents()

            # ASCII値 'u' は 117
            # ASCII値 'd' は 100
            # ASCII値 'r' は 114
            # ASCII値 'l' は 108
            # ASCII値 'q' は 113
            
            if 117 in keys and keys[117] & p.KEY_IS_DOWN:
                p.setJointMotorControlArray(carId,[2,3,5,7],p.VELOCITY_CONTROL,targetVelocities=contrallrace.up())
            elif 100 in keys and keys[100] & p.KEY_IS_DOWN:
                p.setJointMotorControlArray(carId,[2,3,5,7],p.VELOCITY_CONTROL,targetVelocities=contrallrace.back())
            elif 114 in keys and keys[114] & p.KEY_IS_DOWN:
                p.setJointMotorControlArray(carId,[2,3,5,7],p.VELOCITY_CONTROL,targetVelocities=contrallrace.right())
            elif 108 in keys and keys[108] & p.KEY_IS_DOWN:
                p.setJointMotorControlArray(carId,[2,3,5,7],p.VELOCITY_CONTROL,targetVelocities=contrallrace.left())
            else:
                p.setJointMotorControlArray(carId,[2,3,5,7],p.VELOCITY_CONTROL,targetVelocities=contrallrace.stop())
            
            p.stepSimulation()
            
            if 113 in keys and keys[113] & p.KEY_IS_DOWN:
                print("シミュレーションを終了します。")
                running = False # ループを終了させるフラグをFalseに設定
                
            time.sleep(timestep)

    except Exception as e:
        print(f"エラーが発生しました: {e}")
    finally:
        p.disconnect()
        print("シミュレーションが終了しました。")
        
        
if __name__ == "__main__":
    main()

動いている様子はこんな感じ

まとめ

pybulletでのシミュレーションをざっくりとですが行いました。
解説も適宜追加していく予定です。
これからの予定としてはpybulletを使ってオムニ、メカナムとかもシミュレーションできないか調べて行きます。

参考リンク

https://qiita.com/KamikawaTakato/items/f7ae65c25bc30b51e96d

Discussion