V-REPのLine TracerをPythonで制御する
急に3Dシミュレーションやりたい欲が出てきたのでV-REPというのを使ってみた。 V-REPを選択した理由は次の通り。
- Pythonで制御できる
- いろんなリアルロボットをシミュレートできる
- 以下の記事があったのでハードルが低かった
この記事に沿って進めていくだけで、以下の動画のようにPythonでLine Tracerを制御できるようになる。 しかし、途中からLine Trace機能が効かなくなって、Line Tracerが落下するという結末が待っていた。
そこで、その原因を究明するために、ソースコードを少しだけちゃんと読んでみた。
ソースコード解析
前半はV-REPをPythonから制御するための前処理的なことなので飛ばして、まずは扱うUIとObjectの取得から。 次の処理では、メソッドの第２引数から分かるように、順にセンサーディスプレイ、左・中央・右センサー、左・右車輪と、1つのUIと5つのObjectを取得している。
# Get obejects 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) # Left wheel res, rightJointDynamic = vrep.simxGetObjectHandle(clientID, "DynamicRightJoint", vrep.simx_opmode_blocking) # Right wheel
最初の"sensorDisplay"とは、下図の左上にあるUI。
Left sensorが黒く、それ以外が黄色になっている。
これはLine Tracerの前方に付いている3つのセンサーを見ると分かる。
左センサーが黒いラインを捉えているため、UIにそれが反映されている。
以下がその設定メソッドsetLeds()。
elHandleに上記で取得した"sensorDisplay"のObject displayを第1引数として渡す。
第2～4引数のleft, middle, rightは、TrueかFalseで、Falseがラインを捉えていることを示す。
最初にFalseで初期化して、Trueなら設定し直すという感じかな、たぶん。
setLeds()という名前にしているのは、sensorをLEDとしてUIで表現しているからだと思う。
# Update the sensor display. def setLeds(elHandle, left, middle, right): # Initialize display at first. 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) # Set LEDs of the display if sensors don't catch the line. 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)
続いて、本処理に入る前にセンサー情報を格納するlistを初期化。
# Initialize sensors sensorReading = [False, False, False] # Left, Middle, Right 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)
そして本処理だが、ここがLine Tracerが落下した原因の一つ。
whileの行に50秒以内であれば制御する書いてある。
確かに観測してるとゲートを通る直前ぐらいで50秒が過ぎている。
このときにPythonの処理が終わって、その時の左右の車輪の速度が同じでかつ前進しているから、そのままLine Tracerが落下すると思われる。
なので、とりあえずは50を∞に近い数字すれば半永久的にLine Traceしてくれるはず。
なお後に続くのは、センサー情報を取得して上記のsetLeds()でディスプレイにしている処理。
while time.time() - startTime < 50: # Set near ∞ if you want!!!!!!!!!! # Try to retrieve the streamed data. returnCode, data = vrep.simxGetIntegerParameter(clientID,vrep.sim_intparam_mouse_x,vrep.simx_opmode_buffer) # 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]) # Update the sensor display setLeds(display, sensorReading[0], sensorReading[1], sensorReading[2])
最後はLine Tracerの制御。 まずはまっすぐ進むように左右の車輪を同じ速度に設定。 その後、左センサーがラインを捉えていたら左に少し曲がるように設定。そのまた逆も然り。 そして、その設定コマンドをLine Tracerに送る。
# Decide about both left and right velocities. s = 1.0 linearVelocityLeft = nominalLinearVelocity*s linearVelocityRight = nominalLinearVelocity*s # Turn left a little if the left sensor catches line(False), and vice versa. if not sensorReading[0]: linearVelocityLeft = linearVelocityLeft*0.3 if not sensorReading[2]: linearVelocityRight = linearVelocityRight*0.3 # Update both left and right velocities. 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)
上でwhileの処理時間を∞に近い数字すればいいと言ったが、そんなことしなくてもシンプルに処理が終わったらLine Tracerを止めればいい、という考えもある。そのためには、与える速度を左右の車輪共に0.0にして、かつ第4引数をvrep.simx_opmode_blockingにすればOK。ちなみに、vrep.simx_opmode_oneshotは前のコマンドと同じものを返すみたい？なので、速度を0.0にしても、そのまま落下する結末を免れることができなかった。詳しくは参考文献を見て下さい。
# Stop the line tracer. vrep.simxSetJointTargetVelocity(clientID, leftJointDynamic, 0.0, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(clientID, rightJointDynamic, 0.0, vrep.simx_opmode_blocking)
以上。
全ソースコード
# -*- 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) # Get obejects. 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) # Left wheel res, rightJointDynamic = vrep.simxGetObjectHandle(clientID, "DynamicRightJoint", vrep.simx_opmode_blocking) # Right wheel if res != vrep.simx_return_ok: print('Failed to get sensor Handler') vrep.simxFinish(clientID) sys.exit('Program ended') # Update the sensor display. def setLeds(elHandle, left, middle, right): # Initialize display at first. 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) # Set LEDs of the display if sensors don't catch the line. 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) # Initialize sensors sensorReading = [False, False, False] # Left, Middle, Right 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: # Try to retrieve the streamed data. returnCode, data = vrep.simxGetIntegerParameter(clientID,vrep.sim_intparam_mouse_x,vrep.simx_opmode_buffer) # 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]) # Update the sensor display setLeds(display, sensorReading[0], sensorReading[1], sensorReading[2]) # Decide about both left and right velocities. s = 1.0 linearVelocityLeft = nominalLinearVelocity*s linearVelocityRight = nominalLinearVelocity*s # Trun left a little if the left sensor catches line(False), and vice versa. if not sensorReading[0]: linearVelocityLeft = linearVelocityLeft*0.3 if not sensorReading[2]: linearVelocityRight = linearVelocityRight*0.3 # Update both left and right velocities. 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) # Stop the line tracer. vrep.simxSetJointTargetVelocity(clientID, leftJointDynamic, 0.0, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(clientID, rightJointDynamic, 0.0, vrep.simx_opmode_blocking)
