robosuite（mujoco）中机械臂action与实际运动不一样的解决办法

1 前言

最近开始搞机械臂强化学习，所以下了robosuite(https://github.com/ARISE-Initiative/robosuite)和stable_baselines3(https://github.com/DLR-RM/stable-baselines3)，在测试demo时，发现了一个共同的问题。以robosuite的demo_random_action.py为例，在基于UR5e的Lift任务中，设置机械臂控制模式为末端位姿控制，发布action$[0,0,0.01,0,0,0,0]$，即在每次step只控制机械臂末端沿z轴平移1cm，然后订阅机械臂末端的3D位置，
代码片段如下：

for i in range(10000):
    action = [0, 0, 0.01, 0, 0, 0, 0] # xyzrpyw
    obs, reward, done, _ = env.step(action)
    pos = obs['robot0_eef_pos']
    print('robot0_eef_pos 00 = ', pos)
    env.render()

终端输出片段如下：

robot0_eef_pos =  [-0.23551467 -0.0243821   0.99280764]
robot0_eef_pos =  [-0.23552365 -0.02437741  0.99293071]
robot0_eef_pos =  [-0.23553237 -0.02437285  0.99305398]
robot0_eef_pos =  [-0.23554087 -0.02436841  0.99317743]
robot0_eef_pos =  [-0.23554921 -0.02436404  0.993301  ]
robot0_eef_pos =  [-0.23555745 -0.02435972  0.99342466]
robot0_eef_pos =  [-0.23556562 -0.02435544  0.99354838]
robot0_eef_pos =  [-0.23557381 -0.02435115  0.99367207]
robot0_eef_pos =  [-0.23558204 -0.02434683  0.9937957 ]
robot0_eef_pos =  [-0.23559023 -0.02434254  0.99391939]
robot0_eef_pos =  [-0.23559847 -0.02433821  0.99404301]
robot0_eef_pos =  [-0.23560677 -0.02433385  0.99416658]
robot0_eef_pos =  [-0.23561513 -0.02432945  0.9942901 ]

可以看到，z轴每个step的变化仅有0.1mm左右。这会导致在方针训练的方法完全无法移植到真实机器人中使用。
为此，需要看看robosuite在获取action之后是如何对机械臂进行控制的。

2 Robosuite 机械臂控制流程

以UR5e机械臂为例：
（1）env.step()读取action：/robosuite/environments/base.py，MujocoEnv.step()函数，377行附近。

该函数会根据输入的action对机械臂进行n次扭矩控制，使机械臂末端逐渐向action指定的目标位置靠近，$n=(1/f)/0.002$。0.002为robosuite计算一次扭矩并控制机械臂末端运动的时间，为超参数，$f$为设定的控制频率，即主程序执行step()的频率，如果希望仿真得快，可以提高$f$，但相应的机械臂根据action进行控制的次数就少，因此可能会到不了目标位置就执行下一次step()。降低$f$会导致仿真比较慢。$f$默认20。

在每次计算扭矩的过程中，执行如下操作：
（2）将action分配给不同的机械臂，分别控制：/robosuite/environments/robot_env.py，RobotEnv._pre_action()函数，560行附近。
（3）根据action计算扭矩：/robosuite/robots/single_arm.py，SingleArm.control()函数，216行附近。
计算扭矩包含以下步骤：

• 将action从$[-1,1]$的范围缩放到$[-0.05,0.05]$：/robosuite/controllers/osc.py，OperationalSpaceController.set_goal()函数，202行附近。
• 通过PID计算扭矩和力矩：/robosuite/controllers/osc.py，OperationalSpaceController.run_controller()函数，278行附近。

（4）根据扭矩和力矩控制机械臂运动。

3 解决方法

3.1 方法1

纵观整个控制流程，有两步影响了机械臂控制，action缩放和PID控制，因此对应的有两个措施：
（1）action的平移乘20，旋转乘2。（平移最大值不超过1cm，旋转最大角度不超过5度；可以同时沿3个轴平移，最好每次只沿一个轴旋转）
（2）增大P，实测从150改成15000：/robosuite/controllers/config/osc_pose.json，“kp”。
改完的主程序代码片段如下：

previous_pos=[0, 0, 0]
for i in range(10000):
    action = [0, 0, 0.01*20, 0, 0, 0, 0] # xyzrpyw
    obs, reward, done, _ = env.step(action)
    pos = obs['robot0_eef_pos']
    print('robot0_eef_pos = ', pos)
    offset_pos = [(pos[i]-previous_pos[i])*1000 for i in range(3)]
    print('offset_pos= ', offset_pos, 'mm')
    previous_pos = pos
    env.render()

部分结果如下：

robot0_eef_pos =  [-0.2342945  -0.01516201  1.13349579]
offset_pos=  [0.1444412818658869, 0.26288934973437045, 9.583669282512197] mm
robot0_eef_pos =  [-0.23413707 -0.01492414  1.14310729]
offset_pos=  [0.15743314785421347, 0.23787001802808883, 9.611498857711176] mm
robot0_eef_pos =  [-0.23395898 -0.0147149   1.15275716]
offset_pos=  [0.17808603700031345, 0.2092406013055586, 9.649864783828122] mm
robot0_eef_pos =  [-0.23376095 -0.01453091  1.16244509]
offset_pos=  [0.19803490825345826, 0.1839913994160034, 9.68793181744143] mm
robot0_eef_pos =  [-0.23354533 -0.01437108  1.17217047]
offset_pos=  [0.21561864511562834, 0.15983219793866757, 9.725385365261019] mm
robot0_eef_pos =  [-0.23331491 -0.01423421  1.18193126]
offset_pos=  [0.2304198092690779, 0.1368657154015053, 9.760783275659435] mm
robot0_eef_pos =  [-0.23307273 -0.01412232  1.1917262 ]
offset_pos=  [0.24217845162033402, 0.11188704218575117, 9.79494501030076] mm
robot0_eef_pos =  [-0.2328192  -0.01403239  1.20155432]
offset_pos=  [0.2535326251500991, 0.08993389463747385, 9.828116528686648] mm
robot0_eef_pos =  [-0.23255499 -0.01396302  1.21141505]
offset_pos=  [0.26420898104967394, 0.06936535053027153, 9.86073509872143] mm
robot0_eef_pos =  [-0.23228119 -0.01391304  1.22130746]
offset_pos=  [0.2738015549676731, 0.04998322230716744, 9.892405550669947] mm
robot0_eef_pos =  [-0.23199958 -0.0138829   1.23123042]
offset_pos=  [0.2816041835602967, 0.030143650726172436, 9.922962179197858] mm

可以看到，改完后，每次step()后的末端位姿与之前的位姿差大约就是1cm。
修改完训练Lift任务，发现机械臂在抓起物体后总是振荡。

3.2 方法2

在mask()环境时，控制频率control_freq设为1，代价是训练巨慢。

最后再放个渲染图吧