本篇将会创建几个基本的仿真环境，包含了一些必要的功能函数，实现动态调整离地高度。能够与决策模型进行交互，为后续强化学习训练做准备  

一、框架

 
  按照强化学习的基本思路，我们需要构建仿真环境与代理进行交互的部分。每一个循环需要执行以下步骤：    

• 初始化环境：包括导入相关模型，设置好动力学参数之类的
• 执行动作：根据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()

  现在我们执行程序看效果如何：  
  动态效果：