Webots控制器编程

本文主要内容是如何编写Webots控制器，使用语言为Python。

1. 新增控制器

对机器人Robot新增控制器的方式：

Wizards -> New Robot Controller

默认的Python代码模版是：

# You may need to import some classes of the controller module. Ex:
#  from controller import Robot, Motor, DistanceSensor
# 1. 导入相关控制器类
from controller import Robot# create the Robot instance.
# 2. 实例化控制器类得到控制的对象
robot = Robot()# get the time step of the current world.
# 3. 获取环境仿真时间步
timestep = int(robot.getBasicTimeStep())# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# 4. 获取要操控的设备并设置仿真时间步
#  motor = robot.getDevice('motorname')
#  ds = robot.getDevice('dsname')
#  ds.enable(timestep)# Main loop:
# - perform simulation steps until Webots is stopping the controller
# 5. 主循环中按照时间步进行仿真
while robot.step(timestep) != -1:# Read the sensors:# Enter here functions to read sensor data, like:#  val = ds.getValue()# Process sensor data here.# Enter here functions to send actuator commands, like:#  motor.setPosition(10.0)pass# Enter here exit cleanup code.

可以看到一般控制器程序的代码可以分为五部分：

1. 导入相关控制器类
2. 实例化控制器类得到控制的对象
3. 获取环境仿真时间步
4. 获取要操控的设备并设置仿真时间步
5. 主循环中按照时间步进行仿真

2. Hello World Example

from controller import Robotrobot = Robot()while robot.step(32) != -1:print("Hello World!")

在主循环中每隔32ms会进行执行一次，执行的内容可能需要1ms也可能需要1min的时间完成这次仿真的推进，具体的时间需要看执行内容的复杂程度。

3. 读取传感器

from controller import Robot, DistanceSensorTIME_STEP = 32robot = Robot()# my_distance_sensor这个需要根据你命名的传感器名称进行定义
sensor = robot.getDevice("my_distance_sensor")
# 传感器需要先启用才能使用，TIME_STEP传感器的两次数据更新的时间间隔
# 一般设置TIME_STEP和仿真时间步相同
sensor.enable(TIME_STEP)while robot.step(TIME_STEP) != -1:value = sensor.getValue()print("Sensor value is: ", value)

需要注意的是有些传感器返回的值不是标量而是向量，如下代码所示：

GPS.getValues()
Accelerometer.getValues()
Gyro.getValues()# return the sensor measurement as an array of 3 floating point numbers: `[x, y, z]`.

values = gps.getValues()# OK, to read the values they should never be explicitly deleted by the controller code
print("MY_ROBOT is at position: %g %g %g" % (values[0], values[1], values[2]))# there is no need to copy these values

4. 使用执行器

下面的示例显示了如何使用 2 Hz 正弦信号使旋转电机振荡。

from controller import Robot, Motor
from math import pi, sinTIME_STEP = 32robot = Robot()
# 获取机器人上名为"my_motor"的电机设备
motor = robot.getDevice("my_motor")# 设置频率F为2 Hz，用于控制电机的周期性运动，使电机每秒进行两个完整周期的运动。
F = 2.0   # frequency 2 Hz
t = 0.0   # elapsed simulation timewhile robot.step(TIME_STEP) != -1:position = sin(t * 2.0 * pi * F)motor.setPosition(position)t += TIME_STEP / 1000.0

与传感器不同，执行器不需要明确启用。为了控制运动，通常有用的是将运动分解为与控制步骤相对应的离散步骤。和以前一样，这里使用无限循环：在每次迭代时，根据正弦方程计算新的目标位置。

需要注意的是，motor.setPosition 函数存储新位置，但它不会立即启动电机。有效驱动调用 robot.step 函数开始。

当 robot.step 函数返回时，电机已移动一定的（线性或旋转）量，该量取决于目标位置、控制步骤的持续时间（使用 robot.step 函数参数指定）、速度、加速度、力等电机“.wbt”描述中指定的参数。例如，如果指定非常小的控制步长或较低的电机速度，则当 robot.step 函数返回时，电机不会移动太多。在这种情况下，旋转电机需要几个控制步骤才能到达目标位置。如果指定更长的持续时间或更高的速度，则当 robot.step 函数返回时，电机可能已完全完成运动。

请注意，motor.setPosition 函数仅指定所需的目标位置。就像真实的机器人一样，旋转电机有可能无法到达该位置（仅在基于物理的模拟中），因为它被障碍物阻挡或因为电机的扭矩（最大力）不足以抵抗重力等。

如果要同时控制多个旋转电机的运动，则需要使用motor.setPosition 函数分别为每个旋转电机指定所需的位置。然后您需要调用一次 robot.step 函数来同时驱动所有旋转电机。

5. 理解step和robot.step函数

Webots 使用两种不同的时间步长：

• The simulation step（在场景树中指定：WorldInfo.basicTimeStep）
• The control step（指定为每个机器人的 robot.step 函数的参数）

The simulation step（模拟步长）是 WorldInfo.basicTimeStep 中指定的值（以毫秒为单位）。它表示一步模拟的持续时间，即两次计算每个模拟对象的位置、速度、碰撞等的时间间隔。如果模拟使用physics (vs. kinematics)，则模拟步骤还指定需要应用于模拟刚体的力和扭矩的两次计算之间的间隔。

The control step（控制步长）是控制循环迭代的持续时间。它与传递给 robot.step 函数的参数相对应。robot.step 函数将指定持续时间的控制器时间提前。同时，它还会根据控制器时间将传感器和执行器数据与模拟器同步。

每个控制器都需要定期调用 robot.step 函数。如果控制器不调用 robot.step 函数，则传感器和执行器将不会更新，并且模拟器将阻塞（仅在同步模式下）。因为需要定期调用，所以 robot.step 函数调用通常放在控制器的主循环中。

a simulation step的执行是一个原子操作：它不能被中断。因此，传感器测量或电机驱动只能在两个simulation step之间进行。因此，每个 robot.step 函数调用指定的The control step必须是simulation step的倍数。因此，例如，如果simulation step为 16 ms，则传递给 robot.step 函数的控制步参数可以是 16、32、64、128 等。

如果模拟以逐步模式运行，即通过单击Step按钮，则执行具有模拟步骤持续时间的单个步骤。下图详细描述了仿真状态、控制器状态和Step点击之间的同步。

6. 同时使用传感器和执行器

Webots 和每个机器人控制器在不同的进程中执行。例如，如果模拟涉及两个机器人，则总共会有三个进程：一个是 Webots 进程，两个是两个机器人进程。在调用 robot.step 函数时，每个机器人控制器进程都会与 Webots 进程交换传感器和执行器数据。例如，my_leg.setPosition 函数不会立即将数据发送给 Webots。相反，它会在本地存储数据，并在调用 robot.step 函数时有效发送数据。

因此，下面的代码片段是一个糟糕的示例。显然，第一次调用 my_leg.setPosition 函数时指定的值将被第二次调用覆盖：

my_leg.setPosition(0.34) # BAD: ignored
my_leg.setPosition(0.56)
robot.step(40) # BAD: we don't test the return value of this function

同样，这段代码也没有什么意义：

while robot.step(40) != -1:d1 = sensor.getValue()d2 = sensor.getValue()if d2 > d1: # WRONG: d2 will always equal d1 hereavoidCollision()

由于在两次传感器读数之间没有调用 robot.step 函数，因此传感器返回的值不可能在此期间发生变化。一个正确的版本如下：

while robot.step(40) != -1:d1 = sensor.getValue()if robot.step(40) == -1:breakd2 = sensor.getValue()if d2 > d1:avoidCollision()

然而，通常推荐的方法是在主控制循环中调用一个 robot.step 函数，并使用它同时更新所有传感器和执行器，如下所示：

while robot.step(40) != -1:readSensors()actuateMotors()

请注意，在循环开始时调用 robot.step函数非常重要，以确保传感器在进入 readSensors 函数之前已经具有有效值。

这是一起使用传感器和执行器的完整示例。这里使用的机器人使用差速转向。它使用两个距离传感器（DistanceSensor）来检测障碍物。

from controller import Robot, Motor, DistanceSensorTIME_STEP = 32robot = Robot()left_sensor = robot.getDevice("left_sensor")
right_sensor = robot.getDevice("right_sensor")
left_sensor.enable(TIME_STEP)
right_sensor.enable(TIME_STEP)left_motor = robot.getDevice("left_motor")
right_motor = robot.getDevice("right_motor")
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))
left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)while robot.step(TIME_STEP) != -1:# read sensorsleft_dist = left_sensor.getValue()right_dist = right_sensor.getValue()# compute behavior (user functions)left = compute_left_speed(left_dist, right_dist)right = compute_right_speed(left_dist, right_dist)# actuate wheel motorsleft_motor.setVelocity(left)right_motor.setVelocity(right)

在Webots仿真环境中，控制电机的方式主要有两种：位置控制和速度控制。而在这段代码中，通过将电机的位置设置为“无限”float('inf')，就可以使电机进入速度控制模式 。

• 位置控制模式：通常情况下，如果给电机设置一个固定的目标位置（例如一个角度或距离），电机会尝试旋转到该位置并停在那里，这就是“位置控制”。
• 速度控制模式：通过将电机位置设为“无限”float('inf')，Webots将认为电机的目标位置是无限远的，这种情况下，电机会进入“速度控制模式”，即不再关注目标位置，而是只根据设定的速度值进行旋转或移动。

7. 控制器参数

在.wbt 文件中，可以指定控制器启动时传递的参数。这些参数在机器人节点的 controllerArgs 字段中指定，并作为主函数的参数传递。例如，这可用于指定每个机器人控制器的不同参数。请注意，使用 MATLAB 时不支持控制器参数检索。

比如：

Robot {...controllerArgs "one two three"...
}

如果控制器的名称是 “demo”，那么就会出现这段示例控制器代码

from controller import Robot
import sysrobot = Robot()for i in range(0, len(sys.argv)):print("argv[%i]=%s" % (i, sys.argv[i]))

argv[0]=demo
argv[1]=one
argv[2]=two
argv[3]=three