本篇將會創建幾個基本的仿真環境，包含了一些必要的功能函數，實現動態調整離地高度。能夠與決策模型進行交互，爲後續強化學習訓練做準備
一、框架
按照強化學習的基本思路，我們需要構建仿真環境與代理進行交互的部分。每一個循環需要執行以下步驟：
- 初始化環境：包括導入相關模型，設置好動力學參數之類的
- 執行動作：根據agent產生的動作來使模型運動
- 返回狀態和獎勵：執行完動作之後，重新獲取當前狀態並計算獎勵值，返回給agent
因此，我們的環境框架需要包含一下功能：
class Cheetah(object):
def __init__(self):
'''
初始化參數
'''
pass
def reset(self):
'''
環境復位
'''
pass
def step(self):
'''
執行動作
'''
pass
def get_observe(self):
'''
獲取當前狀態
'''
pass
def reward(self):
'''
計算獎勵值
'''
pass
這裏我們先不考慮強化學習部分，因此可以暫時忽略獎勵函數部分
1、__init__
這裏我們需要初始化以下內容：
- pybullet客戶端：pybullet的使用標準
- motor_id：用於設置電機角度，需要區分各個關節類型，這裏爲了方便直接給出了對應的編號。
- 設置重力
- 設置參數：
addUserDebugParameter這個api提供用戶自定義的參數，我這裏設置成 髖關節角度 - 設置視角：
resetDebugVisualizerCamera這個api可以設置相機姿態，在我的pybulet實踐文章中已經介紹過其使用
當我們的類初始化的時候需要調用一次reset()函數來設置mini cheetah的姿態
def __init__(self):
# self.pybullet_client = self._pybullet_client = bc.BulletClient(connection_mode=pybullet.GUI)
self.pybullet_client = pybullet
self.pybullet_client.connect(self.pybullet_client.GUI)
self.pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
self.motor_id_list = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
self.pybullet_client.setGravity(0, 0, -9.8)
# self.upper_angle = self.pybullet_client.addUserDebugParameter("upper_angle", 0, 1, 0.4)
self.pybullet_client.resetDebugVisualizerCamera(0.2, 45, -30, [1, -1, 1])
self.reset()
2、reset
- 導入地面
- 導入mini_cheetah模型
- 打印關節信息：
getNumJoints用於獲取關節信息 - 調用
reset_pos（）函數來設置每一條腿姿態
def reset(self):
init_position = [0, 0, 0.5]
self._ground_id = self.pybullet_client.loadURDF('plane.urdf')
self.quadruped = self.pybullet_client.loadURDF(
"mini_cheetah/mini_cheetah.urdf",
init_position,
useFixedBase=False)
num_joints = self.pybullet_client.getNumJoints(self.quadruped)
for i in range(num_joints):
print(self.pybullet_client.getJointInfo(self.quadruped, i))
for i in range(4):
self.reset_pos(i, 0.7853982)
3、step
由於這裏只是演示，所以沒有采用更高級的動作指令，這裏實現的功能是根據時間動態調整mini_cheetah的離地高度(sin函數)。
readUserDebugParameter這個api適用於讀取用戶自定的參數的，在__init__函數中已經初始化過了，我們可以通過滑塊來調整離地高度，注意不能喝sin函數同時使用。
def step(self):
t = 0
while 1:
t += 0.001
angle = 0.4 * np.sin(t) + 0.5
# angle = self.pybullet_client.readUserDebugParameter(self.upper_angle)
for i in range(4):
self.reset_pos(i, angle)
self.pybullet_client.stepSimulation()
4、reset_pos
設置腿部姿態，setJointMotorControl2這個api是pybullet中最爲常用的，因爲控制關節都是通過這個api進行的。這裏利用該api分別對髖關節和膝關節進行角度控制。
def reset_pos(self, leg_id, angle):
l1 = 208
l2 = 180
hip_angle = 0.0
upper_angle = -angle
# 離地高度L與髖關節角度alpha的關係，在數學問題-初始姿態這篇文章介紹過該公式
L = l1 * np.cos(angle) + np.sqrt(-l1 ** 2 * np.sin(angle) ** 2 + l2 ** 2)
gamma = np.arccos((-l1 ** 2 + L ** 2 + l2 ** 2) / (2 * L * l2))
beta = angle + gamma
self.pybullet_client.setJointMotorControl2(self.quadruped,
jointIndex=self.motor_id_list[3 * leg_id],
controlMode=self.pybullet_client.POSITION_CONTROL,
targetPosition=hip_angle)
self.pybullet_client.setJointMotorControl2(self.quadruped,
self.motor_id_list[3 * leg_id + 1],
self.pybullet_client.POSITION_CONTROL,
targetPosition=upper_angle)
self.pybullet_client.setJointMotorControl2(self.quadruped,
self.motor_id_list[3 * leg_id + 2],
self.pybullet_client.POSITION_CONTROL,
targetPosition=beta)
5、執行
if __name__ == '__main__':
env = Cheetah()
env.step()
現在我們執行程序看效果如何：
動態效果：
