OpenAI Gym是强化学习领域的事实标准。研究员使用Gym来与Gym中的基准比较他们的算法。Gym暴露通用的接口,方便开发。两个重要的设计决定造就了这样的通用接口:
- RL的两个核心的概念是agent和environment。Gym只提供了environment的抽象接口,agent没有,理由是可以创造出很复杂的agent。
- 在一个特定环境的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 手指位置]
---
所以,__init__()函数最终是这样:
```python
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的奖励。
def step(self,action):
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
orientation=p.getQuaternionFromEuler([0.,-math.pi,math.pi/2.])
dv=0.005
dx=action[0]*dv
dy=action[1]*dv
dz=action[2]*dv
fingers=action[3]
currentPose=p.getLinkState(self.pandaUid,11)
#currentPosition=currentPose[0]
currentPosition=currentPose[4]
newPosition=[currentPosition[0]+dx,
currentPosition[1]+dy,
currentPosition[2]+dz]
jointPoses=p.calculateInverseKinematics(self.pandaUid,11,newPosition,orientation)
p.setJointMotorControlArray(self.pandaUid,list(range(7))+[9,10],p.POSITION_CONTROL,list(jointPoses)+2*[fingers])
p.stepSimulation()
state_object,_=p.getBasePositionAndOrientation(self.objectUid)
state_robot=p.getLinkState(self.pandaUid,11)[0]
state_fingers=(p.getJointState(self.pandaUid,9)[0],p.getJointState(self.pandaUid,10)[0])
if state_object[2]>0.45:
reward=1
done=True
else:
reward=0
done=False
info=state_object
observation=state_robot+state_fingers
return observation,reward,done,info
直接从环境中使用state_object,_=p.getBasePositionAndOrientation(self.objectUid)
来获取物体的位置是不现实的, 在现实中是没法这么做的. 大多数情况下, 我们需要一个视觉传感器. 在执行每一个step()
之前, 我们通过render()
函数输出相机的图像, 并且将图像传入到神经网络中, 神经网络负责输出物体的位置. 在render()
函数里, 使用pybullet.computeViewMatrixFromYawPitchRoll()
放置相机到目标位置和姿势(orientation), 这个函数会返回view_matrix
和proj_matrix
, 然后, 使用pybulelt.getCameraImage()
获取图像.
最后的render()
函数长这样:
def render(self,mode='human'):
view_matrix=p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=[0.7,0,0.05],
distance=.7,
yaw=90,
pitch=-70,
roll=0,upAxisIndex=2)
proj_matrix=p.computeProjectionMatrixFOV(fov=60,aspect=float(960)/720,
nearVal=0.1,
farVal=100.0)
(_,_,px,_,_)=p.getCameraImage(width=960,height=720,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_array=np.array(px,dtype=np.uint8)
rgb_array=np.reshape(rgb_array,(720,960,4))
rgb_array=rgb_array[:,:,:3]
return rgb_array
API的具体介绍可在pybullet的官方文档中找到.Pybullet Quickstart Guide
还有最后一个close()
函数.
def close(self):
p.disconnect()
发布你的环境到社区(PyPI)
如果你想把你封装的环境发布到PyPI上供其他人使用, 应该按照如下步骤:
● 在PyPI上创建一个账户
● 安装以下包
pip install setuptools wheel twine
● 我们将环境命名为gym-panda
, 前面几个part已经介绍了自定义gym的基本结构. gym-panda的结构应该这样:
gym-panda/
README.md
setup.py
gym_panda/
__init__.py
envs/
__init__.py
panda_env.py
`
● gym-panda/setup.py的内容如下:
import setuptools
from pathlib import Path
setuptools.setup(
name='gym_panda',
version='0.0.1',
description='An OpenAI Gym Env for Panda',
long_description=Path('README.md').read_text(),
long_description_content_type='text/markdown',
packages=setuptools.find_packages(include='gym_panda*'),
install_requires=['gym']
)
● gym-panda/gym_panda/__init__.py
from gym.envs.registration import register
register(
id='panda-v0',
entry_point='gym_panda.envs:PandaEnv'
)
● gym-panda/gym_panda/envs/__init__.py
from gym_panda.envs.panda_env import PandaEnv
● gym-panda/gym_panda/envs/panda_env.py
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):
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))
def step(self,action):
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
orientation=p.getQuaternionFromEuler([0.,-math.pi,math.pi/2.])
dv=0.005
dx=action[0]*dv
dy=action[1]*dv
dz=action[2]*dv
fingers=action[3]
currentPose=p.getLinkState(self.pandaUid,11)
#currentPosition=currentPose[0]
# 经小伙伴指正,这个地方应该是currentPose[4],而不是0,具体原因请看文章最后的分析。
currentPosition=currentPose[4]
newPosition=[currentPosition[0]+dx,
currentPosition[1]+dy,
currentPosition[2]+dz]
jointPoses=p.calculateInverseKinematics(self.pandaUid,11,newPosition,orientation)[0:7]
p.setJointMotorControlArray(self.pandaUid,list(range(7))+[9,10],p.POSITION_CONTROL,list(jointPoses)+2*[fingers])
p.stepSimulation()
state_object,_=p.getBasePositionAndOrientation(self.objectUid)
state_robot=p.getLinkState(self.pandaUid,11)[0]
state_fingers=(p.getJointState(self.pandaUid,9)[0],p.getJointState(self.pandaUid,10)[0])
if state_object[2]>0.45:
reward=1
done=True
else:
reward=0
done=False
info=state_object
observation=state_robot+state_fingers
return observation,reward,done,info
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,i,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
def render(self,mode='human'):
view_matrix=p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=[0.7,0,0.05],
distance=.7,
yaw=90,
pitch=-70,
roll=0,upAxisIndex=2)
proj_matrix=p.computeProjectionMatrixFOV(fov=60,aspect=float(960)/720,
nearVal=0.1,
farVal=100.0)
(_,_,px,_,_)=p.getCameraImage(width=960,height=720,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_array=np.array(px,dtype=np.uint8)
rgb_array=np.reshape(rgb_array,(720,960,4))
rgb_array=rgb_array[:,:,:3]
return rgb_array
def close(self):
p.disconnect()
● 编译
python setup.py sdist bdist_wheel
● 上传
twine upload dist/*
但是注意, 以gym-panda这个名字上传是会失败的, 因为有同名的存在.
● 可以使用pip install -e gym-panda
本地安装, 即从源码安装; 如果你已经把包上传到PyPI, 也可以使用pip install gym-panda
来安装, 然后就可以使用gym.make('gym_panda:panda-v0')
来创建环境的一个实例.
在这个地址kuka_rl, 是一个使用KUKA机器人的应用Deep Q Learning和PPO算法的仓库.
后面我们将会讲解将Panda环境应用强化学习算法的过程.
使用panda gym环境
安装
● 从PyPI安装
pip install gym-panda
● 从源码安装
git clone https://github.com/borninfreedom/gym-panda.git
cd gym-panda
pip install -e .
使用
我们写一个简单的测试程序. 在gym-panda文件夹下, 建立test文件夹, 新建test.py
import gym
import gym_panda
env=gym.make('panda-v0')
env.reset()
for _ in range(1000):
env.render()
env.step(env.action_space.sample())
env.close()
observations
● ```observation```(object): 返回的是[$x_{gripper}$, $y_{gripper}$, $z_{gripper}$, $J_{finger1}$, $J_{finger2}$]
● ```reward```(float)
● ```done```(boolean) : 如果done==True, 代表当前episode结束, 重置环境,调用```reset()
● info
(dict): [$x_{object}, y_{object}, z_{object}$]的调试信息
所以稍微复杂和标准的RL code应该这样:
import gym
import gym_panda
env=gym.make('panda-v0')
env.reset()
for i_episode in range(20):
observation=env.reset()
for t in range(100):
env.render()
print(observation)
action=env.action_space.sample()
observation,reward,done,info=env.step(action)
if done:
print('Episode finished aftert {} timesteps'.format(t+1))
break
env.close()
PD控制, 更进一步
为了更好的控制机械臂的动作, 我们使用PD控制方法. 使用pybullet默认的240Hz的频率来计算导数. 我们将动作分解为3大块: ①夹爪抵达物体位置; ②夹住物体; ③抓起物体. $K_p, K_d$分别设为10
和1
. 控制误差为0.01.
import gym
import gym_panda
env = gym.make('panda-v0')
observation=env.reset()
done = False
error = 0.01
fingers = 1
object_position = [0.7, 0, 0.1]
k_p = 10
k_d = 1
dt = 1./240. # the default timestep in pybullet is 240 Hz
t = 0
for i_episode in range(20):
observation = env.reset()
fingers = 1
for t in range(100):
env.render()
print(observation)
dx = object_position[0]-observation[0]
dy = object_position[1]-observation[1]
target_z = object_position[2]
if abs(dx) < error and abs(dy) < error and abs(dz) < error:
fingers = 0
if (observation[3]+observation[4])<error+0.02 and fingers==0:
target_z = 0.5
dz = target_z-observation[2]
pd_x = k_p*dx + k_d*dx/dt
pd_y = k_p*dy + k_d*dy/dt
pd_z = k_p*dz + k_d*dz/dt
action = [pd_x,pd_y,pd_z,fingers]
observation, reward, done, info = env.step(action)
object_position=info['object_position']
if done:
print("Episode finished after {} timesteps".format(t+1))
break
env.close()
补充
经小伙伴指正,在step()函数中 ,currentPostion=currentPose[0]这句应该为currentPosition=currentPose[4],经过验证,小伙伴说的是对的。怎么验证呢?
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setGravity(0,0,-9.8)
p.setAdditionalSearchPath(pd.getDataPath())
p.resetDebugVisualizerCamera(cameraDistance=0.5,cameraYaw=0,\
cameraPitch=-40,cameraTargetPosition=[0,-0.9,1])
planeId=p.loadURDF("plane.urdf")
robotId=p.loadURDF("franka_panda/panda.urdf",useFixedBase=True)
print("robotId=",robotId)
for i in range(p.getNumJoints(robotId)):
joint_info=p.getJointInfo(robotId,i)
if joint_info[2]!=4:
print(joint_info[0],joint_info[1],joint_info[8],joint_info[9])
"""
0 b'panda_joint1' -2.9671 2.9671
1 b'panda_joint2' -1.8326 1.8326
2 b'panda_joint3' -2.9671 2.9671
3 b'panda_joint4' -3.1416 0.0
4 b'panda_joint5' -2.9671 2.9671
5 b'panda_joint6' -0.0873 3.8223
6 b'panda_joint7' -2.9671 2.9671
9 b'panda_finger_joint1' 0.0 0.04
10 b'panda_finger_joint2' 0.0 0.04
"""
slideId0=p.addUserDebugParameter('panda_joint1',p.getJointInfo(robotId,0)[8],p.getJointInfo(robotId,0)[9],0)
slideId1=p.addUserDebugParameter('panda_joint2',p.getJointInfo(robotId,1)[8],p.getJointInfo(robotId,0)[9],0)
slideId2=p.addUserDebugParameter('panda_joint3',p.getJointInfo(robotId,2)[8],p.getJointInfo(robotId,0)[9],0)
slideId3=p.addUserDebugParameter('panda_joint4',p.getJointInfo(robotId,3)[8],p.getJointInfo(robotId,0)[9],0)
slideId4=p.addUserDebugParameter('panda_joint5',p.getJointInfo(robotId,4)[8],p.getJointInfo(robotId,0)[9],0)
slideId5=p.addUserDebugParameter('panda_joint6',p.getJointInfo(robotId,5)[8],p.getJointInfo(robotId,0)[9],0)
slideId6=p.addUserDebugParameter('panda_joint7',p.getJointInfo(robotId,6)[8],p.getJointInfo(robotId,0)[9],0)
slideId9=p.addUserDebugParameter('panda_finger_joint1',p.getJointInfo(robotId,9)[8],p.getJointInfo(robotId,0)[9],0)
slideId10=p.addUserDebugParameter('panda_finger_joint2',p.getJointInfo(robotId,10)[8],p.getJointInfo(robotId,0)[9],0)
while True:
p.stepSimulation()
time.sleep(0.1)
slideId0_value=p.readUserDebugParameter(slideId0)
slideId1_value = p.readUserDebugParameter(slideId1)
slideId2_value = p.readUserDebugParameter(slideId2)
slideId3_value = p.readUserDebugParameter(slideId3)
slideId4_value = p.readUserDebugParameter(slideId4)
slideId5_value = p.readUserDebugParameter(slideId5)
slideId6_value = p.readUserDebugParameter(slideId6)
slideId9_value = p.readUserDebugParameter(slideId9)
slideId10_value = p.readUserDebugParameter(slideId10)
p.setJointMotorControl2(robotId,0,controlMode=p.POSITION_CONTROL,targetPosition=slideId0_value)
p.setJointMotorControl2(robotId, 1, controlMode=p.POSITION_CONTROL, targetPosition=slideId1_value)
p.setJointMotorControl2(robotId, 2, controlMode=p.POSITION_CONTROL, targetPosition=slideId2_value)
p.setJointMotorControl2(robotId, 3, controlMode=p.POSITION_CONTROL, targetPosition=slideId3_value)
p.setJointMotorControl2(robotId, 4, controlMode=p.POSITION_CONTROL, targetPosition=slideId4_value)
p.setJointMotorControl2(robotId, 5, controlMode=p.POSITION_CONTROL, targetPosition=slideId5_value)
p.setJointMotorControl2(robotId, 6, controlMode=p.POSITION_CONTROL, targetPosition=slideId6_value)
p.setJointMotorControl2(robotId, 9, controlMode=p.POSITION_CONTROL, targetPosition=slideId9_value)
p.setJointMotorControl2(robotId, 10, controlMode=p.POSITION_CONTROL, targetPosition=slideId10_value)
link4_state=p.getLinkState(robotId,4)
print('link4_state=',link4_state)
massCenterLineId=p.addUserDebugLine([link4_state[0][0],0,0],link4_state[0],[1,0,0])
linkLineId=p.addUserDebugLine([link4_state[4][0],0,0],link4_state[4],[0,1,0])
这段代码会在机械臂上添加辅助线帮助识别,如图
其中红色线是currentPose[0]代表的位置,绿色线是currentPose[4]代表的位置,很明显绿色线是符合我们上面代码的意思的。
更加详细的API说明请参考pybullet的文档。
评论(2)
您还未登录,请登录后发表或查看评论