D13：使用pybullet讓機器人移動

2022 iThome 鐵人賽

DAY 13

自我挑戰組

高中生也可以！利用強化學習讓機器人動起來！系列第 13 篇

14th鐵人賽

golucky_sir

2022-09-14 00:52:53

2468 瀏覽

分享至

大家好，今天終於要來講如何操控機器人移動了，正常來說我們都是通過控制關節馬達來移動，不過在某些特殊情況下，我們會通過直接對機器人施力來移動，例如要使用空氣動力學讓無人機起飛，並不是透過旋轉機翼而是直接模擬升力來達成。詳情可以看看這篇論文以及他們的實驗影片，github也會附在下面，有興趣可以再去看看。

Learning to Fly -- a Gym Environment with PyBullet Physics for Reinforcement Learning of Multi-agent Quadcopter Control

Learning to Fly-a Gym Environment with PyBullet Physics for RL of Multi-agent Quadcopter Control

GitHub - utiasDSL/gym-pybullet-drones: PyBullet Gym environments for single and multi-agent reinforcement learning of quadcopter control

題外話就到這裡，現在就直接進入正題，讓我們來控制R2D2吧！

控制關節馬達

通常我們會使用p.setJointMotorControl2(模型ID, jointIndex=關節索引值, controlMode=控制方法)方法來控制關節馬達的移動，根據控制方法的不同，最後一個要設定的參數名稱也會不一樣，控制方法有分兩種，一種是速度控制p.VELOCITY_CONTROL，一種是位置控制p.POSITION_CONTROL，這兩個的差別是速度控制是設定馬達的速度，設定好值後馬達就會等速的持續移動，位置控制是可以設定馬達的位置，當呼叫了以後馬達會在瞬間移動到那個位置，不過如果位置沒有持續更動的話他就是會卡在那個固定位置。通常模擬都是使用速度控制比較多。

那如果我們的控制方法參數是設定p.VELOCITY_CONTROL的話，p.setJointMotorControl2()這個方法裡面就要再使用targetVelocity=數值這個參數來設定速度的數值；那如果是使用p.POSITION_CONTROL的話，同理p.setJointMotorControl2()這個方法裡面就要再使用targetPosition=數值這個參數來設定速度的數值。昨天有看到輪子的四個關節索引值分別為2、3、6、7，接著就來設定輪子的轉速吧。

wheel_value = p.readUserDebugParameter(wheel)
p.setJointMotorControl2(r2d2, jointIndex=2, controlMode=p.VELOCITY_CONTROL, targetVelocity=wheel_value)
p.setJointMotorControl2(r2d2, jointIndex=3, controlMode=p.VELOCITY_CONTROL, targetVelocity=wheel_value)
p.setJointMotorControl2(r2d2, jointIndex=6, controlMode=p.VELOCITY_CONTROL, targetVelocity=wheel_value)
p.setJointMotorControl2(r2d2, jointIndex=7, controlMode=p.VELOCITY_CONTROL, targetVelocity=wheel_value)

生動了不少呢！另外如果我們要一次控制很多關節，但關節的操作設定都一樣的話，可以使用p.setJointMotorControlArray(模型ID,jointIndices=多個關節索引值組合的串列,controlMode=p.VELOCITY_CONTROL,targetVelocities=各關節的數值)，前面有說到四個輪子關節分別是2、3、6、7，所以把他組合成一個串列就好，最後各關節的數值這個部分也是這四個關節索引值的速度值組合的串列，但因為數值都一樣，所以用生程式去創建串列就好。完整的程式如下：

#while迴圈外
wheel_lst=[2,3,6,7]
#while迴圈內
wheel_value = p.readUserDebugParameter(wheel)
wheel_values = [wheel_value for i in range(len(wheel_lst))]
p.setJointMotorControlArray(r2d2,jointIndices=wheel_lst,
														controlMode=p.VELOCITY_CONTROL,
														targetVelocities=wheel_values)

不過各位應該都會發現機器人如果這樣跑遲早會跑出螢幕外面，所以我們有兩種選擇，第一個是讓鏡頭(上帝視角)跟著機器人移動，或者可以透過按鈕來講機器人重新設置到原點。我想先介紹設定上帝視角的部分，不過在講上帝視角的部分之前，我們首先要知道機器人目前的位置對吧，所以就先取得機器人當前的座標吧。

取得機器人當前狀態

我們可以透過p.getBasePositionAndOrientation(模型ID)，這個方法來取得模型當前的位置跟繞軸轉向的弧度，前者是當前位置座標，後者是機器人方向的四元數，我們可以使用p.getEulerFromQuaternion(四元數串列)來講四元數轉換成歐拉角度，不過重點不是這個，我們只需要取得前面的座標值就好了，所以使用索引值0來取得座標。

r2d2pos = p.getBasePositionAndOrientation(r2d2)[0]
print(r2d2pos)

上帝視角(攝影機位置)

我們可以使用以下的方式來設定我們上帝視角要看的位置在哪，

p.resetDebugVisualizerCamera(
    cameraDistance=距離,
    cameraYaw=俯視視角,
    cameraPitch=水平視角,
    cameraTargetPosition=相機目標位置
  )

俯視視角跟水平視角都是以角度為單位的整數或者浮點數，基本上我也會把這些參數加入到debug視窗中並讀取。可以參考這個code片段

#在while迴圈外
pitch=p.addUserDebugParameter(paramName='camerapitch',rangeMin=0,rangeMax=360,startValue=0)
yaw=p.addUserDebugParameter(paramName='camerayaw',rangeMin=0,rangeMax=360,startValue=30)
distance=p.addUserDebugParameter(paramName='cameradistance',rangeMin=0,rangeMax=6,startValue=2)

#在while迴圈內
r2d2pos = p.getBasePositionAndOrientation(r2d2)[0]
cameraDistance = p.readUserDebugParameter(distance)
cameraYaw = p.readUserDebugParameter(yaw)
cameraPitch = p.readUserDebugParameter(pitch)
p.resetDebugVisualizerCamera(
    cameraDistance=cameraDistance,
    cameraYaw=cameraYaw,
    cameraPitch=cameraPitch,
    cameraTargetPosition=r2d2pos
  )

這樣就可以設定上帝視角的部分了，各位可以依照自己的視覺喜好來設定這些參數的初始值，才不用每次打開視窗都要調整。接下來來介紹另一個方法，可以讓機器人重新生成到指定的位置。

重新生成機器人

通常我們會使用到這個方法的情況會是在機器人可能跌到或者跑一跑不見甚至穿模碰撞想卡bug等等情況才會將機器人重新設定位置，在這些情況中會比較忌諱直接又新增一隻機器人進來，這樣有時候會因為模型太多造成畫面卡卡的，在操控機器人的時候也常常會有一些問題，所以認識了這個方法才可以將出狀況的機器人救出來。這個方法非常簡單，參數設定跟p.loadURDF()大致上一樣，只是第一個參數是使用已經存在的模型ID，另外座標跟面朝的方向可以直接用初始設定的方式也可以重新設定！

p.resetBasePositionAndOrientation(r2d2,r2d2StartPos,r2d2StartOrientation)

移動的另一個方法

正如我前面提及的，在模擬空氣動力學等情況下，會直接透過對施力處施加一個力來模擬模型的受力狀況，不過在使用這個方法的時候模擬方式不能使用p.setRealTimeSimulation(1)，而是要在迴圈中使用p.stepSimulation()來模擬，有興趣可以下載今天這篇文開頭的那個無人機模擬飛行的程式，在gym_pybullet_drones/assets中可以看到cf2x.urdf跟cf2p.urdf，這兩個都是無人機的模型，可以使用它來測試看看受力情形～那我就先來介紹一下p.applyExternalForce(模型ID,關節索引值,[fx,fy,fz],[px,py,pz],flags)，這個方法有點長，待我一個一個向各位介紹，首先模型ID跟關節索引值就不再重複了，接下來是一個串列，分別是該關節位置三個軸的分力；然後還是一個串列，分別是施力點距離關節處的距離；接下來flag的意思是設定施力跟施力點的座標系統，有兩個參數可以選擇，p.LINK_FRAME跟p.WORLD_FRAME，前者是以關節的所在處當成原點，後者則是以世界笛卡兒座標為基準去設定，通常應該都會使用前者。

雷射射線

今天的最後我想介紹雷射射線給各位，這個射線也可以當成輔助線，也可以偵測是否有碰到東西等，在訓練或者搜集資訊上都有著一定的用處。建立一條雷射射線也很簡單，使用p**.**addUserDebugLine(起始處, 終止處, 顏色)，起始處跟終止處都是座標串列，顏色為RGB組合的串列，這樣子就建立好雷射了，那如果要有偵測的功能的話，我們就必須再使用另一個方法p.rayTest(起始處, 終止處)來建立一個帶有偵測功能的射線，這個射線是透明的，但可以根據射線偵測的結果來改變剛剛建立的雷射射線的顏色。p.rayTest()方法會回傳偵測到的資訊，現階段我們只需要知道有沒有偵測到東西就好，所以要取回傳結果的索引值[0][0]，如果這個值是-1代表沒有偵測到東西，否則就是有偵測到東西。

完整的程式如下：

#在while迴圈外
raystartpos=[0,1,0]#雷射起始處
raytopos=[0,1,1]#雷射終止處

#在while迴圈內
ray = p.rayTest(raystartpos,raytopos)[0][0]#取得偵測結果
#如果沒有偵測到東西的話就顯示紅色雷射，有偵測到東西的話就顯示綠色雷射
if ray==-1:
    p.addUserDebugLine(raystartpos, raytopos, [1, 0, 0])
else:
    p.addUserDebugLine(raystartpos, raytopos, [0, 1, 0])
p.removeAllUserDebugItems()#舊的雷射需要刪除掉，不然雷射一直加上去會很卡

Untitled

可以看到機器人前面有一個紅色的雷射。

程式碼統整

今日份的完整程式碼如下，為了方便各位整理會與昨天的合併。

import pybullet as p
import pybullet_data
physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setRealTimeSimulation(1)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
r2d2StartPos = [0, 0, 0.5]
r2d2StartOrientation = p.getQuaternionFromEuler([0, 0, 0])
r2d2 = p.loadURDF('r2d2.urdf', r2d2StartPos, r2d2StartOrientation)
#numJoints = p.getNumJoints(r2d2)
#for joint in range(numJoints):
#  with open('par.txt','a') as fp:
#    fp.write(str(p.getJointInfo(r2d2, joint)))
#    fp.write('\n')
wheel = p.addUserDebugParameter(paramName='wheel',rangeMin=-50,rangeMax=50,startValue=0)
btn = p.addUserDebugParameter(paramName="btn",rangeMin=1,rangeMax=0,startValue=0)
btn_value = p.readUserDebugParameter(btn)
#上帝視角的設定
pitch=p.addUserDebugParameter(paramName='camerapitch',rangeMin=0,rangeMax=360,startValue=0)
yaw=p.addUserDebugParameter(paramName='camerayaw',rangeMin=0,rangeMax=360,startValue=30)
distance=p.addUserDebugParameter(paramName='cameradistance',rangeMin=0,rangeMax=6,startValue=2)
#設定四顆輪子的索引值串列
wheel_lst=[2,3,6,7]

while True:
	  #讀取輪子的數值
    wheel_value = p.readUserDebugParameter(wheel)
    # p.setJointMotorControl2(r2d2, jointIndex=2, controlMode=p.VELOCITY_CONTROL, targetVelocity=wheel_value)
    # p.setJointMotorControl2(r2d2, jointIndex=3, controlMode=p.VELOCITY_CONTROL, targetVelocity=wheel_value)
    # p.setJointMotorControl2(r2d2, jointIndex=6, controlMode=p.VELOCITY_CONTROL, targetVelocity=wheel_value)
    # p.setJointMotorControl2(r2d2, jointIndex=7, controlMode=p.VELOCITY_CONTROL, targetVelocity=wheel_value)
    wheel_values = [wheel_value for i in range(len(wheel_lst))]
    p.setJointMotorControlArray(r2d2,jointIndices=wheel_lst,controlMode=p.VELOCITY_CONTROL,targetVelocities=wheel_values)
    
		#如果按下按鈕則重新生成r2d2
    if p.readUserDebugParameter(btn) != btn_value:
        p.resetBasePositionAndOrientation(r2d2,r2d2StartPos,r2d2StartOrientation)
        btn_value = p.readUserDebugParameter(btn)

		#設定上帝視角
    r2d2pos = p.getBasePositionAndOrientation(r2d2)[0]
    cameraDistance = p.readUserDebugParameter(distance)
    cameraYaw = p.readUserDebugParameter(yaw)
    cameraPitch = p.readUserDebugParameter(pitch)
    p.resetDebugVisualizerCamera(
        cameraDistance=cameraDistance,
        cameraYaw=cameraYaw,
        cameraPitch=cameraPitch,
        cameraTargetPosition=r2d2pos
      )

		#設定雷射偵測
    raystartpos=[0,1,0]
    raytopos=[0,1,1]
    ray = p.rayTest(raystartpos,raytopos)[0][0]
    if ray==-1:
        p.addUserDebugLine(raystartpos, raytopos, [1, 0, 0])
    else:
        p.addUserDebugLine(raystartpos, raytopos, [0, 1, 0])
    p.removeAllUserDebugItems()