前面几节，我们已经把PPO算法实现完成了，下面只剩下环境了。今天我们把环境的封装完成。

对于机械臂或者机器人的训练来说，一个比较好用的仿真器是pybullet，另外一个是unity。gazebo、vrep等虽然可以做，但是相比pybullet直接使用Python驱动，没有额外的操作来说，gazebo和vrep都显得繁琐了一些，而且在强化学习方面，pybullet的生态较好。unity因为有unity ml-agent的加持，也特别适合用来做游戏和机器人的强化学习。在最近，微软开源了unity robot模块，可以直接使用unity来做机器人的建模、控制、和ROS结合以及强化学习和机器学习，感兴趣的可以留言讨论，我也整理过一个文档，关于unity做机器人的。那我们呢就是用pybullet，当然有时间我也会去探索一些unity做机器人的例子分享。

使用pybullet做强化学习的环境和gym的区别不是太大，也是完成 init, reset, step, render等几个函数。如果想了解更具体的细节，可以参考这篇文章为PyBullet创建Gym环境。

init函数

init函数我们主要完成一些基本的pybullet配置、机械臂的null space配置（这个是用来复制求解逆解）、机械臂的初始化配置等，这个是使用pybullet基本必须的配置，同时呢，还要完成环境的observation_space、action_space的定义。

    def __init__(self,is_render=False,is_good_view=False):

        self.is_render=is_render
        self.is_good_view=is_good_view

        if self.is_render:
            p.connect(p.GUI)
        else:
            p.connect(p.DIRECT)

        self.x_low_obs=0.2
        self.x_high_obs=0.7
        self.y_low_obs=-0.3
        self.y_high_obs=0.3
        self.z_low_obs=0
        self.z_high_obs=0.55

        self.x_low_action=-0.4
        self.x_high_action=0.4
        self.y_low_action=-0.4
        self.y_high_action=0.4
        self.z_low_action=-0.6
        self.z_high_action=0.3

        p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=0, cameraPitch=-40,
                                     cameraTargetPosition=[0.55, -0.35, 0.2])

        self.action_space=spaces.Box(low=np.array([self.x_low_action,self.y_low_action,self.z_low_action]),
                                     high=np.array([self.x_high_action,self.y_high_action,self.z_high_action]),
                                     dtype=np.float32)
        self.observation_space=spaces.Box(low=np.array([self.x_low_obs,self.y_low_obs,self.z_low_obs]),
                                     high=np.array([self.x_high_obs,self.y_high_obs,self.z_high_obs]),
                                     dtype=np.float32)
        self.step_counter=0

        self.urdf_root_path = pybullet_data.getDataPath()
        # lower limits for null space
        self.lower_limits = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
        # upper limits for null space
        self.upper_limits = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
        # joint ranges for null space
        self.joint_ranges = [5.8, 4, 5.8, 4, 5.8, 4, 6]
        # restposes for null space
        self.rest_poses = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
        # joint damping coefficents
        self.joint_damping = [0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001]

        self.init_joint_positions = [0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539]

        self.orientation = p.getQuaternionFromEuler([0., -math.pi, math.pi / 2.])




        self.seed()
        self.reset()

reset函数

reset函数是较为重要的一个函数，它定义了机械臂初始化时的状态、机械臂的初始化、observation等。同时，我们在机械臂在桌面上的工作空间的边缘都加上了白色的线，用来表示工作空间的边缘。在reset里面，我们需要将用到的模型添加进来，包括机械臂的模型、放置机械臂的桌子模型和被reach的物体模型。最后呢，需要将observation传递出去。我们在讲解机械臂reach的第一篇的时候，深度强化学习专栏 —— 7. 实现机械臂reach某点之PPO算法实现（一），曾经说过这样一个设计思路：

如果想让机械臂正确的学会reach到一个点，基本的设计思路有以下几种：
（1）state为当前要reach的点的坐标，actor可以根据这个点的坐标决定自己的动作，critic可以根据这个点的坐标输出价值函数的值（这个地方重点看一下，有待商榷）；
（2）state为当前要reach点和机械臂末端末端两者的坐标，actor可以根据当前末端坐标和reach坐标输出动作，critic也可以更容易的根据当前末端坐标和reach坐标生成价值的判断；
（3）state为当前工作场景的图像信息，actor根据当前的图像生成动作，critic根据当前的图像做出判断，这个是最难得，不过最后我们也会实现。

我们今天要用的就是思路1，把被reach物体的坐标作为observation传递出去。但是在第一篇文章中，为什么加了个“有待商榷”，这是因为PPO是基于actor-critic架构的，actor好说，策略函数根据被reach物体的坐标生成动作即可，但是critic呢？critic的计算公式是((obs-R)**2).mean()，在这里，每次环境加载的时候，被reach物体的坐标都是随机生成的，那么在critic的公式里面，obs和Reward都是在整个episode结束的时候才会发生变化，那么这样的话，critic到底起了作用没有？不过呢，结果还是不错的，能达到98%左右的成功率。但是我个人觉着，使用思路2的方式，是更为严谨的方式。

    def reset(self):
        #p.connect(p.GUI)
        self.step_counter=0

        p.resetSimulation()
        #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
        self.terminated=False
        p.setGravity(0, 0, -10)

        #这些是周围那些白线，用来观察是否超过了obs的边界
        p.addUserDebugLine(lineFromXYZ=[self.x_low_obs,self.y_low_obs,0],
                           lineToXYZ=[self.x_low_obs,self.y_low_obs,self.z_high_obs])
        p.addUserDebugLine(lineFromXYZ=[self.x_low_obs,self.y_high_obs,0],
                           lineToXYZ=[self.x_low_obs,self.y_high_obs,self.z_high_obs])
        p.addUserDebugLine(lineFromXYZ=[self.x_high_obs,self.y_low_obs,0],
                           lineToXYZ=[self.x_high_obs,self.y_low_obs,self.z_high_obs])
        p.addUserDebugLine(lineFromXYZ=[self.x_high_obs,self.y_high_obs,0],
                           lineToXYZ=[self.x_high_obs,self.y_high_obs,self.z_high_obs])

        p.addUserDebugLine(lineFromXYZ=[self.x_low_obs,self.y_low_obs,self.z_high_obs],
                           lineToXYZ=[self.x_high_obs,self.y_low_obs,self.z_high_obs])
        p.addUserDebugLine(lineFromXYZ=[self.x_low_obs,self.y_high_obs,self.z_high_obs],
                           lineToXYZ=[self.x_high_obs,self.y_high_obs,self.z_high_obs])
        p.addUserDebugLine(lineFromXYZ=[self.x_low_obs,self.y_low_obs,self.z_high_obs],
                           lineToXYZ=[self.x_low_obs,self.y_high_obs,self.z_high_obs])
        p.addUserDebugLine(lineFromXYZ=[self.x_high_obs,self.y_low_obs,self.z_high_obs],
                           lineToXYZ=[self.x_high_obs,self.y_high_obs,self.z_high_obs])

        p.loadURDF(os.path.join(self.urdf_root_path, "plane.urdf"), basePosition=[0, 0, -0.65])
        self.kuka_id = p.loadURDF(os.path.join(self.urdf_root_path, "kuka_iiwa/model.urdf"), useFixedBase=True)
        p.loadURDF(os.path.join(self.urdf_root_path, "table/table.urdf"), basePosition=[0.5, 0, -0.65])
       # p.loadURDF(os.path.join(self.urdf_root_path, "tray/traybox.urdf"),basePosition=[0.55,0,0])
        #object_id=p.loadURDF(os.path.join(self.urdf_root_path, "random_urdfs/000/000.urdf"), basePosition=[0.53,0,0.02])
        self.object_id=p.loadURDF(os.path.join(self.urdf_root_path,"random_urdfs/000/000.urdf"),
                   basePosition=[random.uniform(self.x_low_obs,self.x_high_obs),
                                 random.uniform(self.y_low_obs,self.y_high_obs),
                                 0.01])

        self.num_joints = p.getNumJoints(self.kuka_id)

        for i in range(self.num_joints):
            p.resetJointState(bodyUniqueId=self.kuka_id,
                              jointIndex=i,
                              targetValue=self.init_joint_positions[i],
                              )

        self.robot_pos_obs=p.getLinkState(self.kuka_id,self.num_joints-1)[4]
        #logging.debug("init_pos={}\n".format(p.getLinkState(self.kuka_id,self.num_joints-1)))
        p.stepSimulation()
        self.object_pos=p.getBasePositionAndOrientation(self.object_id)[0]
        return np.array(self.object_pos).astype(np.float32)
        #return np.array(self.robot_pos_obs).astype(np.float32)

step函数

step函数也是一个非常重要的函数，它主要定义了机械臂根据强化学习算法生成的action，来决定自己怎样进行动作。其中主要涉及到了机械臂的控制、逆解等。
我在这里做的策略是把算法生成的action进行很小的分割，避免因为action的输出离散过大，造成机械臂突然过大幅度的运动。然后将分割后的action作为变化值，与之前的位置信息叠加作为最后的位置值。

    def step(self,action):
        dv=0.005
        dx=action[0]*dv
        dy=action[1]*dv
        dz=action[2]*dv

        self.current_pos=p.getLinkState(self.kuka_id,self.num_joints-1)[4]
       # logging.debug("self.current_pos={}\n".format(self.current_pos))
        self.new_robot_pos=[self.current_pos[0]+dx,
                            self.current_pos[1]+dy,
                            self.current_pos[2]+dz]
        #logging.debug("self.new_robot_pos={}\n".format(self.new_robot_pos))
        self.robot_joint_positions = p.calculateInverseKinematics(
                                                       bodyUniqueId=self.kuka_id,
                                                       endEffectorLinkIndex=self.num_joints - 1,
                                                       targetPosition=[self.new_robot_pos[0],
                                                                       self.new_robot_pos[1],
                                                                       self.new_robot_pos[2]],
                                                       targetOrientation=self.orientation,
                                                       jointDamping=self.joint_damping,
                                                       )
        for i in range(self.num_joints):
            p.resetJointState(bodyUniqueId=self.kuka_id,
                              jointIndex=i,
                              targetValue=self.robot_joint_positions[i],
                              )
        p.stepSimulation()

        #在代码开始部分，如果定义了is_good_view，那么机械臂的动作会变慢，方便观察
        if self.is_good_view:
            time.sleep(0.05)

        self.step_counter+=1
        return self._reward()

reward函数

对于一个强化学习项目来说，除了算法很重要之外，reward函数的设计更是重中之重。一个项目能不能正常的工作，reward函数所起的作用非常的大。reward设计不合理，即使算法是合适的，那么也有可能达不到自己想要的结果，但是如果reward设计合理，强化学习的项目一般是可以正常工作的。

对于本项目来说，reward的设计相对还是很简单的。这个项目的reward主要包含三项：
（1）如果机械臂末端的位置超出了工作空间，给一个-0.1的奖励；
（2）如果在达到一个episode允许的最大step还没达到reach的位置，给一个-0.1的奖励；
（3）如果机械臂在允许的step、允许的工作空间范围内reach了物体，给一个+1的奖励

那么怎么才算reach了呢？这里我们使用了一个距离的函数，即计算机械臂末端的当前位置和reach物体的距离，根据距离来判断是否reach到。

   def _reward(self):

        #一定注意是取第4个值，请参考pybullet手册的这个函数返回值的说明
        self.robot_state=p.getLinkState(self.kuka_id,self.num_joints-1)[4]
        # self.object_state=p.getBasePositionAndOrientation(self.object_id)
        # self.object_state=np.array(self.object_state).astype(np.float32)
        #
        self.object_state=np.array(
            p.getBasePositionAndOrientation(self.object_id)[0]
        ).astype(np.float32)

        square_dx=(self.robot_state[0]-self.object_state[0])**2
        square_dy=(self.robot_state[1]-self.object_state[1])**2
        square_dz = (self.robot_state[2] - self.object_state[2]) ** 2

        #用机械臂末端和物体的距离作为奖励函数的依据
        self.distance=sqrt(square_dx+square_dy+square_dz)
        #print(self.distance)

        x=self.robot_state[0]
        y=self.robot_state[1]
        z=self.robot_state[2]

        #如果机械比末端超过了obs的空间，也视为done，而且会给予一定的惩罚
        terminated=bool(
            x<self.x_low_obs
            or x>self.x_high_obs
            or y<self.y_low_obs
            or y>self.y_high_obs
            or z<self.z_low_obs
            or z>self.z_high_obs
        )

        if terminated:
            reward=-0.1
            self.terminated=True

        #如果机械臂一直无所事事，在最大步数还不能接触到物体，也需要给一定的惩罚
        elif self.step_counter>self.max_steps_one_episode:
            reward=-0.1
            self.terminated=True

        elif self.distance<0.1:
            reward=1
            self.terminated=True
        else:
            reward=0
            self.terminated=False

        info={'distance:',self.distance}
        #self.observation=self.robot_state
        self.observation=self.object_state
        return np.array(self.observation).astype(np.float32),reward,self.terminated,info

render函数

render函数对于标准的gym环境，即需要通过gym进行注册，使用的时候通过gym.make()来实例化的环境来说，是很重要的，但是我们虽然用了一些gym的轮子，但是不是标准的gym环境，因为我们直接使用环境类来实例化环境的实例，即env=KukaReachEnv()这样来实例化一个环境，是不一定非要提供render函数的，根据需要添加即可。

测试环境

if __name__ == '__main__':
    # 这一部分是做baseline，即让机械臂随机选择动作，看看能够得到的分数
    env=KukaReachEnv()
    print(env)
    print(env.observation_space.shape)
    # print(env.observation_space.sample())
    # print(env.action_space.sample())
    print(env.action_space.shape)
    obs=env.reset()
    print(obs)

好了，以上就是环境的所有内容。将前几节的PPO算法应用到这个环境上，我们就可以得到下面的结果。
深度强化学习专栏 —— 10. 实现机械臂reach某点之环境实现实现
再看一下训练过程。