1 对控制器代码的分析及说明
首先请访问：关于大赛-文件下载-Webots专题系列4-用Python控制机器人的运动-附件-v1.1，下载完成后打开walk.wbt文件。点击工具栏-向导-新机器人控制器，选择Python语言，完成创建，并在文本编辑器中打开。复制附件代码，覆盖默认代码，保存。
将机器人控制器设置为新建控制器，保存世界。
在walk.py中，为了完成对机器人的控制，需要引入几个Python库。
1. from controller import Robot
2. import os
3. import sys
4.
5. libraryPath = os.path.join(os.environ.get("WEBOTS_HOME"), 'projects', 'robots', 'robotis', 'darwin-op', 'libraries',
6. 'python37')
7. libraryPath = libraryPath.replace('/', os.sep)
8. sys.path.append(libraryPath)
9. from managers import RobotisOp2GaitManager, RobotisOp2MotionManager
其中，对机器人环境进行控制的Robot库包含了对各种机器人元件的控制。RobotisOp2GaitManager与RobotisOp2MotionManager库分别为机器人步态控制库与动作组库，分别实现机器人的行走控制与预编动作执行。为了能在Pycharm等外部IDE中正常运行，导入这两个库前需要添加路径信息。
注意：本段代码适用于Python 3.7版本，其他版本的Python可能存在兼容性问题。
定义Walk类，实现对机器人的控制，初始函数如下。
1. class Walk():
2. def __init__(self):
3. self.robot = Robot() # 初始化Robot类以控制机器人
4. self.mTimeStep = int(self.robot.getBasicTimeStep()) # 获取当前每一个仿真步所仿真时间mTimeStep
5. self.HeadLed = self.robot.getLED('HeadLed') # 获取头部LED灯
6. self.EyeLed = self.robot.getLED('EyeLed') # 获取眼部LED灯
7. self.HeadLed.set(0xff0000) # 点亮头部LED灯并设置一个颜色
8. self.EyeLed.set(0xa0a0ff) # 点亮眼部LED灯并设置一个颜色
9. self.mAccelerometer = self.robot.getAccelerometer('Accelerometer') # 获取加速度传感器
10. self.mAccelerometer.enable(self.mTimeSt ep) # 激活传感器，并以mTimeStep为周期更新数值
11. self.fup = 0
12. self.fdown = 0 # 定义两个类变量，用于之后判断机器人是否摔倒
13.
14. self.mGyro = self.robot.getGyro('Gyro') # 获取陀螺仪
15. self.mGyro.enable(self.mTimeStep) # 激活陀螺仪，并以mTimeStep为周期更新数值
16.
17. self.positionSensors = [] # 初始化关节角度传感器
18. self.positionSensorNames = ('ShoulderR', 'ShoulderL', 'ArmUpperR', 'ArmUpperL',
19. 'ArmLowerR', 'ArmLowerL', 'PelvYR', 'PelvYL',
20. 'PelvR', 'PelvL', 'LegUpperR', 'LegUpperL',
21. 'LegLowerR', 'LegLowerL', 'AnkleR', 'AnkleL',
22. 'FootR', 'FootL', 'Neck', 'Head') # 初始化各传感器名
23.
24. # 获取各传感器并激活，以mTimeStep为周期更新数值
25. for i in range(0, len(self.positionSensorNames)):
26. self.positionSensors.append(self.robot.getPositionSensor(self.positionSensorNames[i] + 'S'))
27. self.positionSensors[i].enable(self.mTimeStep)
28.
29. self.mKeyboard = self.robot.getKeyboard() # 初始化键盘读入类
30. self.mKeyboard.enable(self.mTimeStep) # 以mTimeStep为周期从键盘读取
31.
32. self.mMotionManager = RobotisOp2MotionManager(self.robot) # 初始化机器人动作组控制器
33. self.mGaitManager = RobotisOp2GaitManager(self.robot, " Head ") # 初始化机器人步态控制器
定义Walk类中的myStep()函数，完成一个仿真步长的仿真。
1. def myStep(self):
2. ret = self.robot.step(self.mTimeStep)
3. if ret == -1:
4. exit(0)
定义Walk类中的wait()函数，输入为等待毫秒数，使机器人等待一段时间。
1. def wait(self, ms):
2. startTime = self.robot.getTime()
3. s = ms / 1000.0
4. while (s + startTime >= self.robot.getTime()):
5. self.myStep()
run()函数通过键盘输入来控制机器人的运动：
1. def run(self):
2. print("-------Walk example of ROBOTIS OP2-------")
3. print("This example illustrates Gait Manager")
4. print("Press the space bar to start/stop walking")
5. print("Use the arrow keys to move the robot while walking")
6. self.myStep() # 仿真一个步长，刷新传感器读数
7.
8. self.mMotionManager.playPage(9) # 执行动作组9号动作，初始化站立姿势，准备行走
9. self.wait(200) # 等待200ms
10.
11. self.isWalking = False # 初始时机器人未进入行走状态
12.
13. while True:
14. self.checkIfFallen() # 判断是否摔倒
15. self.mGaitManager.setXAmplitude(0.0) # 前进为0
16. self.mGaitManager.setAAmplitude(0.0) # 转体为0
17. key = 0 # 初始键盘读入默认为0
18. key = self.mKeyboard.getKey() # 从键盘读取输入
19. if key == 32: # 如果读取到空格，则改变行走状态
20. if (self.isWalking): # 如果当前机器人正在走路，则使机器人停止
21. self.mGaitManager.stop()
22. self.isWalking = False
23. self.wait(200)
24. else: # 如果机器人当前停止，则开始走路
25. self.mGaitManager.start()
26. self.isWalking = True
27. self.wait(200)
28. elif key == 315: # 如果读取到‘↑’，则前进
29. self.mGaitManager.setXAmplitude(1.0)
30. elif key == 317: # 如果读取到‘↓’，则后退
31. self.mGaitManager.setXAmplitude(-1.0)
32. elif key == 316: # 如果读取到‘←’，则左转
33. self.mGaitManager.setAAmplitude(-0.5)
34. elif key == 314: # 如果读取到‘→’，则右转
35. self.mGaitManager.setAAmplitude(0.5)
36. self.mGaitManager.step(self.mTimeStep) # 步态生成器生成一个步长的动作
37. self.myStep() # 仿真一个步长
checkIfFallen函数用于检测机器人是否倒地，并完成倒地起身动作：
1. def checkIfFallen(self):
2. acc_tolerance = 60.0
3. acc_step = 100 # 计数器上限
4. acc = self.mAccelerometer.getValues() # 通过加速度传感器获取三轴的对应值
5. if acc[1] < 512.0 - acc_tolerance : # 面朝下倒地时y轴的值会变小
6. self.fup += 1 # 计数器加1
7. else :
8. self.fup = 0 # 计数器清零
9. if acc[1] > 512.0 + acc_tolerance : # 背朝下倒地时y轴的值会变大
10. self.fdown += 1 # 计数器加 1
11. else :
12. self.fdown = 0 # 计数器清零
13.
14. if self.fup > acc_step : # 计数器超出上限，即倒地时间超过acc_step个仿真步长
15. self.mMotionManager.playPage(10) # 执行面朝下倒地起身动作
16. self.mMotionManager.playPage(9) # 恢复准备行走姿势
17. self.fup = 0 # 计数器清零
18. elif self.fdown > acc_step :
19. self.mMotionManager.playPage(11) # 执行背朝下倒地起身动作
20. self.mMotionManager.playPage(9) # 恢复准备行走姿势
21. self.fdown = 0 # 计数器清零
通过加速度传感器获取机器人y轴的加速度值，当其值大于/小于某值一段时间后，判断机器人背部朝下/面部朝下摔倒，然后执行对应的起身动作。
主函数定义如下。
1. if __name__ == '__main__':
2. walk = Walk() # 初始化Walk类
3. walk.run() # 运行控制器
2. 对代码进行修改来控制机器人的运动
修改run()函数如下，可以使机器人自动行走，不借助键盘输入。
1. def run(self):
2. self.myStep() # 仿真一个步长，刷新传感器读数
3.
4. self.mMotionManager.playPage(9) # 执行动作组9号动作，初始化站立姿势，准备行走
5. self.wait(200) # 等待200ms
6.
7. self.isWalking = False # 初始时机器人未进入行走状态
8. while True:
9. self.checkIfFallen() # 判断是否摔倒
10. self.mGaitManager.start() # 步态生成器进入行走状态
11. self.mGaitManager.setXAmplitude(1.0) # 设置机器人前进
12. self.mGaitManager.step(self.mTimeStep) # 步态生成器生成一个步长的动作
13. self.myStep() # 仿真一个步长
修改后保存代码文本，如下图。
之后重新运行仿真，就可以看到机器人开始自动前进。
While循环中的self.mGaitManager.setXAmplitude(1.0)语句使机器人持续向前走动，若想让机器人在适当位置停止，可以修改while循环的条件。
3 常用函数列表
函数名称 | 用途 | 备注 |
mGaitManager.start () | 启动 | |
mGaitManager.stop () | 停止 | |
mGaitManager.step (t) | 生成一段时间的步态规划 | t的单位为毫秒 |
mGaitManager.setXAmplitude (double X) | 前进/后退 | X影响脚步向前的长度，它可以取-1到1之间的任何值 |
mGaitManager.setYAmplitude (double Y) | 左移/右移 | Y影响脚步在侧面方向上的长度，它可以取-1到1之间的任何值 |
mGaitManager.setAAmplitude (double A) | 左转/右转 | A影响步态的角度并允许机器人在行走过程中旋转，它可以取0到1之间的任何值 |
mKeyboard.getKey() | 获取键盘输入 | 获取的值为输入字符对应的ASCII码 |
wait (int t) | 等待 | t的单位为毫秒 |
mMotionManager.playPage() | 执行对应动作 | playPage为一系列封装好的机器人动作文件 |
4 playPage部分动作列表
动作组号码 | 名称 | 描述 | 初始姿态 |
9 | walkready | 准备行走 | 直立 |
10 | f up | 起身 | 面部朝下倒地 |
11 | b up | 起身 | 背部朝下倒地 |
更 G更多的动作可以查询Webots User Guide中Robots- Robotis OP2的相关文档。
5 修改配置文件来调整机器人的站姿与步态
从Webotsprojects obots obotisdarwin-opcontrollerswalk路径下找到config.ini，复制一份到控制器代码所在路径下。之后在代码初始化部分通过self.mGaitManager = RobotisOp2GaitManager(self.robot, "config.ini") 语句加载配置文件。可以使用记事本打开，如下图：
可以修改配置文件中的各项数值来调整机器人的站姿、运动时的步态。各参数的具体解释可以查询Webots User Guide中Robots- Robotis OP2的附录部分。