前面几节,我们已经把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算法应用到这个环境上,我们就可以得到下面的结果。
再看一下训练过程。
全部的环境代码可在borninfreedom/kuka-reach-drl找到。
猜你想看:
- Ubuntu助手 — 一键自动安装软件,一键进行系统配置
- 深度强化学习专栏 —— 1.研究现状
- 深度强化学习专栏 —— 2.手撕DQN算法实现CartPole控制
- 深度强化学习专栏 —— 3.实现一阶倒立摆
- 深度强化学习专栏 —— 4. 使用ray做分布式计算
- 深度强化学习专栏 —— 5. 使用ray的tune组件优化强化学习算法的超参数
- 深度强化学习专栏 —— 6. 使用RLLib和ray进行强化学习训练
- 深度强化学习专栏 —— 7. 实现机械臂reach某点之PPO算法实现(一)
- 深度强化学习专栏 —— 8. 实现机械臂reach某点之PPO算法实现(二)
- 深度强化学习专栏 —— 9. 实现机械臂reach某点之PPO算法实现(三)
- pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)
- pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(二)
- pybullet电机控制总结
- Part 1 – 自定义gym环境
- Part 1.1 – 注册自定义Gym环境
- Part 1.2 – 实现一个井字棋游戏的gym环境
- Part 1.3 – 熟悉PyBullet
- Part 1.4 – 为PyBullet创建Gym环境