Volo di notte

お勉強の成果メモや日常のこと

[V-REP] Pythonでライントレーサーをシミュレーション

f:id:Chachay:20160505173437g:plain

はじめに

ロボットをディープラーニングで動かす実験でライントレーサーに教育をした。

ライントレーサーをDeep Q Learningで教育する - Chainer - Qiita

ここまでは、自前で書いた2Dシミュレーション上でトレーサを走らせていて、 ロボットにかかる物理的な現象は無視してきた。 もう少しだけリアリティのある環境でディープラーニングを試すために、ロボット開発用のシミュレータを導入した。

ロボットシミュレータの種類

Wikipedia英語版Robotics simulatorにいろいろ種類があったけれども、 うちの環境で使えそうなシミュレータで、そこそこアクティブなものから選んだ。

うちの環境

シミュレータのリスト

今回採用:V-REP (Virtual Robot Experimentation Platform)
趣味でプログラミングしている人もEducational Versionの枠組みで使わせてもらえるようで、フリーで使える。Gazeboよりインストールも楽ちん。
Gazebo: Windows
Windowsで使うにはBoostやらQtの導入に始まり、コンパイルも必要、中間ファイルで30GB必要という点で忍耐力が必要そうだから今回はパス。
Webot
使うには、何かにつけて、お金が必要そう。軽くお試してみたい感じには向かない。

さっそくライントレーサーをPythonで動かす

Server上でSimulationされているロボットにClientになるプログラムを書いて命令を飛ばせば動くらしい。 Serverに指令を送る手段としてはRemote APIが定義されている。

サーバー側の準備 (環境 + ライントレーサー)

サンプルとしてついてくるライントレーサーをベースにバッサリ準備。

Vrepを起動

f:id:Chachay:20160505164310p:plain

サンプルを呼び出す

シーンを呼び出す[Open Scene...]から

f:id:Chachay:20160505164348p:plain

LineTraer-threaded.tttを呼び出して、

f:id:Chachay:20160505164352p:plain

この状態でツールバーの再生ボタンを押すとライントレーサーが線をフォローし始めます。 f:id:Chachay:20160505164358p:plain (再生ボタンはスクショでは隠れちゃっている)

ライントレーサのロボットにサーバーになってもらう

ロボットの動作定義用スクリプトを開いて f:id:Chachay:20160505164924p:plain

いっぱい書かれているのをバッサリ変更 f:id:Chachay:20160505164937p:plain

ポート19999で待っているだけの状態にする f:id:Chachay:20160505164957p:plain

simExtRemoteApiStart(19999)

これで準備OK

クライアント側の準備(ライントレーサー動作スクリプト)

必要なファイルの準備

作業フォルダに以下3つのファイルをコピーします。

  • ラッパーファイル
    C:\Program Files (x86)\V-REP3\V-REP_PRO_EDU\programming\remoteApiBindings\python\python 以下の
    • vrep.py
    • vrepConst.py
  • DLLファイル
    C:\Program Files (x86)\V-REP3\V-REP_PRO_EDU\programming\remoteApiBindings\lib\lib 以下の32bitか64bitかは自分のPythonの環境に合わせて…
    • remoteApi.dll

スクリプトファイル

大まかには

  1. Vrepモジュールを読み込み
  2. サーバーへのコネクションを確立
  3. ロボットの手足、センサへのハンドラーを取得
  4. ロボットを操作
  5. お片付け

の順序。

全体は最後に乗せるとして順番に要点を挙げる。

Vrepモジュールの読み込み

サンプルからそのままコピペ

# -*- coding: utf-8 -*-
try:
    import vrep
except:
    print ('--------------------------------------------------------------')
    print ('"vrep.py" could not be imported. This means very probably that')
    print ('either "vrep.py" or the remoteApi library could not be found.')
    print ('Make sure both are in the same folder as this file,')
    print ('or appropriately adjust the file "vrep.py"')
    print ('--------------------------------------------------------------')
    print ('')

V-REPへの接続

V-REP側が実行された状態だとlocalhost(127.0.0.1)のポート19999でサーバが立っているはず。

vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5)

if clientID!=-1:
    print ('Connected to remote API server')
else:
    print ('Failed connecting to remote API server')
    sys.exit('Program Ended')

ライントレーサーのタイヤやセンサなどのハンドル取得

真面目にエラー判定してなくてすみません。

  • ライントレーサの光センサ3つ
  • 左右輪
  • Lチカ用のLED (本当はLEDじゃないんだけどLED代わりに使えるみたい)

のハンドル取得

res, display     = vrep.simxGetUIHandle(clientID, "sensorDisplay", vrep.simx_opmode_blocking)
res, leftSensor  = vrep.simxGetObjectHandle(clientID, "LeftSensor", vrep.simx_opmode_blocking)
res, middleSensor= vrep.simxGetObjectHandle(clientID, "MiddleSensor", vrep.simx_opmode_blocking)
res, rightSensor = vrep.simxGetObjectHandle(clientID, "RightSensor", vrep.simx_opmode_blocking)
res, leftJointDynamic  = vrep.simxGetObjectHandle(clientID, "DynamicLeftJoint" , vrep.simx_opmode_blocking)
res, rightJointDynamic = vrep.simxGetObjectHandle(clientID, "DynamicRightJoint", vrep.simx_opmode_blocking)

if res != vrep.simx_return_ok:
    print ('Failed to get sensor Handler')
    vrep.simxFinish(clientID)
    sys.exit('Program ended')

光センサのセンシング

    sensorReading[0]=(vrep.simxReadVisionSensor(clientID, leftSensor,   vrep.simx_opmode_buffer)[1])
    sensorReading[1]=(vrep.simxReadVisionSensor(clientID, middleSensor, vrep.simx_opmode_buffer)[1])
    sensorReading[2]=(vrep.simxReadVisionSensor(clientID, rightSensor,  vrep.simx_opmode_buffer)[1])

左右輪の動作

左右輪にはトルクではなく、回転速度で指令を出しています

    vrep.simxSetJointTargetVelocity(clientID, leftJointDynamic, linearVelocityLeft/(s*wheelRadius), vrep.simx_opmode_oneshot)
    vrep.simxSetJointTargetVelocity(clientID, rightJointDynamic,linearVelocityRight/(s*wheelRadius), vrep.simx_opmode_oneshot)

動作ロジックは味気なく、左側のセンサが反応しなくなったら左旋回。右側のセンサが反応しなくなったら右旋回。見本のまんまです。

    if not sensorReading[0]:
        linearVelocityLeft  = linearVelocityLeft*0.3
    if not sensorReading[2]:
        linearVelocityRight = linearVelocityRight*0.3

シミュレーションの実行

V-REP側のシミュレーションを実行(Start/Resume Simulation)した状態でPythonスクリプトを実行するだけ。ファイアウォールなどでアクセスがブロックされなかったらロボットが動いてくれるはず。

f:id:Chachay:20160505173437g:plain

サンプリングレート(Simulationレート)みたいなのはV-REP側で設定するのかな。

スクリプトの全体

# -*- coding: utf-8 -*-
try:
    import vrep
except:
    print ('--------------------------------------------------------------')
    print ('"vrep.py" could not be imported. This means very probably that')
    print ('either "vrep.py" or the remoteApi library could not be found.')
    print ('Make sure both are in the same folder as this file,')
    print ('or appropriately adjust the file "vrep.py"')
    print ('--------------------------------------------------------------')
    print ('')

import time
import sys
import ctypes

print ('Program started')
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5)

if clientID!=-1:
    print ('Connected to remote API server')
else:
    print ('Failed connecting to remote API server')
    sys.exit('Program Ended')

    
nominalLinearVelocity = 0.3
wheelRadius           = 0.027
interWheelDistance    = 0.119
    
res,objs=vrep.simxGetObjects(clientID, vrep.sim_handle_all,vrep.simx_opmode_blocking)
if res==vrep.simx_return_ok:
    print ('Number of objects in the scene: ',len(objs))
else:
    print ('Remote API function call returned with error code: ',res)

time.sleep(2)

startTime=time.time()
vrep.simxGetIntegerParameter(clientID,vrep.sim_intparam_mouse_x,vrep.simx_opmode_streaming)

res, display     = vrep.simxGetUIHandle(clientID, "sensorDisplay", vrep.simx_opmode_blocking)
res, leftSensor  = vrep.simxGetObjectHandle(clientID, "LeftSensor", vrep.simx_opmode_blocking)
res, middleSensor= vrep.simxGetObjectHandle(clientID, "MiddleSensor", vrep.simx_opmode_blocking)
res, rightSensor = vrep.simxGetObjectHandle(clientID, "RightSensor", vrep.simx_opmode_blocking)
res, leftJointDynamic  = vrep.simxGetObjectHandle(clientID, "DynamicLeftJoint" , vrep.simx_opmode_blocking)
res, rightJointDynamic = vrep.simxGetObjectHandle(clientID, "DynamicRightJoint", vrep.simx_opmode_blocking)

if res != vrep.simx_return_ok:
    print ('Failed to get sensor Handler')
    vrep.simxFinish(clientID)
    sys.exit('Program ended')

def setLeds(elHandle, left, middle, right):
    vrep.simxSetUIButtonProperty(clientID, elHandle,  8, 
        vrep.sim_buttonproperty_staydown, vrep.simx_opmode_oneshot)
    vrep.simxSetUIButtonProperty(clientID, elHandle, 16, 
        vrep.sim_buttonproperty_staydown, vrep.simx_opmode_oneshot)
    vrep.simxSetUIButtonProperty(clientID, elHandle, 24, 
        vrep.sim_buttonproperty_staydown, vrep.simx_opmode_oneshot)
    if left:
        vrep.simxSetUIButtonProperty(clientID, elHandle, 8, 
            vrep.sim_buttonproperty_staydown + vrep.sim_buttonproperty_isdown, vrep.simx_opmode_oneshot)
    if middle:
        vrep.simxSetUIButtonProperty(clientID, elHandle,16, 
            vrep.sim_buttonproperty_staydown + vrep.sim_buttonproperty_isdown, vrep.simx_opmode_oneshot)
    if right:
        vrep.simxSetUIButtonProperty(clientID, elHandle,24, 
            vrep.sim_buttonproperty_staydown + vrep.sim_buttonproperty_isdown, vrep.simx_opmode_oneshot)

sensorReading=[False, False, False]
sensorReading[0]=(vrep.simxReadVisionSensor(clientID, leftSensor,   vrep.simx_opmode_streaming )==1)
sensorReading[1]=(vrep.simxReadVisionSensor(clientID, middleSensor, vrep.simx_opmode_streaming )==1)
sensorReading[2]=(vrep.simxReadVisionSensor(clientID, rightSensor,  vrep.simx_opmode_streaming )==1)

while time.time() - startTime < 50:
    returnCode,data=vrep.simxGetIntegerParameter(clientID,vrep.sim_intparam_mouse_x,vrep.simx_opmode_buffer) # Try to retrieve the streamed data
    
    # Read the sensors:

    sensorReading[0]=(vrep.simxReadVisionSensor(clientID, leftSensor,   vrep.simx_opmode_buffer)[1])
    sensorReading[1]=(vrep.simxReadVisionSensor(clientID, middleSensor, vrep.simx_opmode_buffer)[1])
    sensorReading[2]=(vrep.simxReadVisionSensor(clientID, rightSensor,  vrep.simx_opmode_buffer)[1])
    
    setLeds(display,sensorReading[0],sensorReading[1],sensorReading[2])

    # Decide about left and right velocities:
    s = 1.0
    linearVelocityLeft  = nominalLinearVelocity * s
    linearVelocityRight = nominalLinearVelocity * s

    if not sensorReading[0]:
        linearVelocityLeft  = linearVelocityLeft*0.3
    if not sensorReading[2]:
        linearVelocityRight = linearVelocityRight*0.3

    vrep.simxSetJointTargetVelocity(clientID, leftJointDynamic, linearVelocityLeft/(s*wheelRadius), vrep.simx_opmode_oneshot)
    vrep.simxSetJointTargetVelocity(clientID, rightJointDynamic,linearVelocityRight/(s*wheelRadius), vrep.simx_opmode_oneshot)
    
    time.sleep(0.005)