Part 1.4 - 为PyBullet创建Gym环境

OpenAI Gym是强化学习领域的事实标准。研究员使用Gym来与Gym中的基准比较他们的算法。Gym暴露通用的接口，方便开发。两个重要的设计决定造就了这样的通用接口：

1. RL的两个核心的概念是agent和environment。Gym只提供了environment的抽象接口，agent没有，理由是可以创造出很复杂的agent。
2. 在一个特定环境的RL算法的性能可以从两个方面来衡量：
   （1） 最终的表现
   （2） 学习需要的时间

---

为Panda机器人创建一个Gym环境

我们将要创建一个机器人抓取的环境

对于自定义Gym环境，我们在前面的几个Part中已经做了很详细的说明，如果没有看过，请翻到本篇最后，那里有链接。

创建的Panda环境命名为gym-panda，文件名为panda_env.py。
按照Gym的规则，文件框架应该这样：

import gym
from gym import error,spaces,utils
from gym.utils import seeding

import os
import pybullet as p
import pybullet_data
import math
import numpy as np
import random

class PandaEnv(gym.Env):
    metadata = {'render.modes':['human']}

    def __init__(self):
        ...
    def step(self,action):
        ...
    def reset(self):
        ...
    def render(self,mode='human'):
        ...
    def close(self):
        ...

---

一个函数一个函数的填充。
首先要初始化。我们使用pybullet的GUI显示模式pybullet.connect(pybullet.GUI)，然后添加一个相机pybullet.resetDebugVisualizerCamera()。

---

Gym中每个环境都有action_space和observation_space。Gym中的Space描述了可行的action和observations的格式。Space中box space和discrete space是用的最多的space。

Box space代表n维的box空间。

我们将会读取每一个手指的值（一共两个夹爪）和机械臂末端的笛卡尔坐标位置（同时，我们假设夹爪是一直朝下的，所以问题得以简化，不需要姿势信息）。所以可行的observations将会是5个元素的数组。[夹爪1值 夹爪2值 末端位置x y z]

设定action为末端的目标笛卡尔坐标位置，同时还有两只手指的共同位置，所以是4个元素的数组。[x y z 手指位置]

action_space和observation_space可以使用spaces.Box()

---

所以，init()函数最终是这样：

    def __init__(self):
        p.connect(p.GUI)
        p.resetDebugVisualizerCamera(cameraDistance=1.5,cameraYaw=0,\
                                     cameraPitch=-40,cameraTargetPosition=[0.55,-0.35,0.2])
        self.action_space=spaces.Box(np.array([-1]*4),np.array([1]*4))
        self.observation_space=spaces.Box(np.array([-1]*5),np.array([1]*5))

---

然后编写reset()函数。我们使用pybullet.resetSimulation()来重置环境。使用pybullet.setGravity()设置重力。使用pybullet.loadURDF()来添加ground, robot, table, tray, object等组件。盒子中的物体使用random.uniform()来随机放置位置。我们可以使用pybullet.resetJointState()来让机器人的每个关节放置到目标位置(rest_poses)。机器人末端的姿态和位置可以使用pybullet.getLinkState()来读取。我们只需要位置信息，所以我们可以使用pybullet.getLinkState()[0]只获取位置信息。关节信息可以通过pybullet.getJointState()来获取。Panda中两个手指的关节索引是9和10，我们可以单独控制他们，但是为了简化，action中只包含一个控制信息：同时对两个手指发送相同的控制信号。最后，我们将state_robot和state_fingers组合为observation。在加载环境中物体的过程中，我们可以不使用可视化，使用pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING,0)关闭可视化，使用pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING,1)打开可视化。

最后代码：

    def reset(self):
        p.resetSimulation()
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
        p.setGravity(0,0,-10)
        
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        planeUid=p.loadURDF("plane.urdf",basePosition=[0,0,-0.65])
        rest_poses=[0,-0.215,0,-2.57,0,2.356,2.356,0.08,0.08]
        self.pandaUid=p.loadURDF("franka_panda/panda.urdf",useFixedBase=True)
        for i in range(7):
            p.resetJointState(self.pandaUid,rest_poses[i])
        tableUid=p.loadURDF("table/table.urdf",basePosition=[0.5,0,-0.65])
        trayUid=p.loadURDF("tray/traybox.urdf",basePosition=[0.65,0,0])
        state_object=[random.uniform(0.5,0.8),random.uniform(-0.2,0.2),0.05]
        self.objectUid=p.loadURDF("ramdom_urdfs/000/000.urdf",basePosition=state_object)
        state_robot=p.getLinkState(self.pandaUid,11)[0]
        state_fingers=(p.getJointState(self.pandaUid,9)[0],p.getJointState(self.pandaUid,10)[0])
        observation=state_robot+state_fingers
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
        return observation

---

现在我们可以编写env.step(action)函数了。前面提到，action是由末端的位置和手指的值组成的。我们将会使用pybullet.calculateInverseKinematics()来计算指定位置的关节转角值。我们将会使用dv来平滑计算逆解，同时使用p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)来平滑视觉渲染。为了简化，夹爪的姿势是一直垂直地面的。使用pybullet.getQuaternionFromEuler()将欧拉角转换为四元数。

在每一个step，首先使用p.getLinkState()获取夹爪的笛卡尔坐标位置，添加一些朝向目标点的小变化，然后使用p.calculateInverseKinematics()来计算逆解，也就是计算到达这个目标点，每个关节应该转多少角度，然后使用p.setJointMotorControlArray()来驱动电机，一次驱动多个。然后使用p.stepSimulation()来一步一步的运行。然后，读取物体、夹爪和手指的状态，将夹爪和手指的状态作为observation，物体的状态作为调试信息输出。

在step()函数里，我们同样定义了reward：如果机器人将物体抓住了，并且抓取到了0.45高度，会获得+1的奖励。