Webots：Controller编写（Python）

最新推荐文章于 2025-04-23 17:24:54 发布

小蜗牛( ˙-˙ )

最新推荐文章于 2025-04-23 17:24:54 发布

阅读量7.2k

点赞数 17

分类专栏： Webots

本文链接：https://blog.youkuaiyun.com/Miss_leading/article/details/115004107

版权

Webots 专栏收录该内容

2 篇文章

订阅专栏

Controller

下面的内容相当于 Webots：User Guide 的翻译，可能会更改或省略某些部分。

Hello World

接下来的代码，反复打印“Hello World”到Webots控制台（打印到标准输出流，输出重定向到Webots控制台）。

from controller import Robot
# 实例化
robot = Robot()
while robot.step(32) != -1:
	print('Hello World')

通常，最高级别的控制代码放置在for或while循环内。在该循环中，有一个对 robot.step 函数的调用。该功能使控制器的数据与模拟器同步。该 robot.step 函数必须存在于每个控制器中，并且必须以固定的间隔调用。因此，如上例所示，通常将其放置在主循环中。值32指定控制步骤的持续时间，即 robot.step 功能应计算32毫秒的仿真然后返回。此持续时间指定的是模拟时间量，而不是实际（墙上时钟）时间，因此实际时间可能要花费1毫秒或1分钟，具体取决于模拟世界的复杂程度。

robot.step函数在Webots终止控制器时的确会返回-1。但当仿真一直运行时，该循环也将一直运行，输出“Hello World”。

传感器读数

有时，我们也需要阅读机器人传感器的值，下面这个例子阅读了 DistanceSensor 返回的值。

from controller import Robot, DistanceSensor

TIME_STEP = 32

robot = Robot()

sensor = robot.getDevice("distancename")
sensor = enable(TIME_STEP)

while robot.step(TIME_STEP) != -1:
	value = sensor.getValue()
	print("Sensor value is：", value)

使用getDevice函数获取相应的设备标签（WbDeviceTag）。
传递给函数的字符串“Distancename”是指定的设备名称，如果机器人没有指定名称的设备，则函数返回0。
使用enable函数启用传感器，可以指定两次喜欢干起数据更新之间的期望间隔，称为更新延迟。更新延迟的选择与step(time_step)的选择相似，因此传感器将在每次step函数调用时更新。较大的更新延迟可以加快仿真速度，尤其对于CPU密集型设备。
可以随时使用disable()函数禁用设备。
getValue()函数的调用可以获取传感器的最新值。
注意：下面这些设备返回的是矢量值，而不是标量值。

# 传感器的测量值将作为3个浮点数的数组返回
# [x, y, z]
GPS.getValues()
Accelerometer.getValues()
Gyro.getValues()

使用下面的代码可以获取返回值：

values = gps.getValues()
print("MY_ROBOT is at position: %g %g %g" % (values[0], values[1], values[2]))

使用执行器

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

from controller import Robot, Motor
from math import pi, sin

TIME_STEP = 32

robot = Robot()
# 使用getDevice函数获取设备标识
motor = robot.getDevice("my_motor")

F = 2.0   # frequency 2 Hz
t = 0.0   # elapsed simulation time

while robot.step(TIME_STEP) != -1:
    position = sin(t * 2.0 * pi * F)
    motor.setPosition(position)
    t += TIME_STEP / 1000.0

在每次迭代中，根据正弦方程计算一个新的目标位置。setPosition功能存储相应旋转电机的新位置请求。但是不会立刻启动电动机，有效地激活从robot.step()函数的下一行开始。
如果要同时控制多个RotationalMotor的运动，则需要使用该功能分别为每个RotationalMotor指定所需的位置setPosition。然后，调用robot.step一次函数以同时启动所有RotationalMotors。

Step & robot.step()

webots拥有两个不同的时间步骤：

模拟步骤 ----- 在场景树中指定 WorldInfo.basicTimeStep
控制步骤 ----- 指定 robot.step 为每个机器人的功能参数
~~不想打字~~

在每一步中，robot.step() 函数调用语句前的所有命令都首先执行，并模拟在 robot.step() 函数执行的中间停止。Webots API函数已执行，但仅在调用该wb_robot_step函数时（即，控制器进程与Webots进程进行通信时），它们才应用于模拟领域。模拟停止时，已经计算出新的模拟状态，更新了模拟时间并且准备了新的传感器值。

一起使用传感器和执行器

webots和每个机器人控制器在单独的过程中执行。在 robot.step() 函数调用期间，每个控制器进行都与webots进行交换传感器和执行器数据。
eg：setPosition() 函数不会立刻将数据发送到webots（即视图中的机器人不会立即运动），而是本地存储数据，并在 robot.step() 调用该函数时有效地发送数据。

无效代码：

my_leg.setPosition(0.34) # BAD: ignored
my_leg.setPosition(0.56)
robot.step(40)

或者：

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

有效版本应该是：

while robot.step(40) != -1:
	d1 = sensor.getValue(sensor)
	# 调用一下rebot.step()，更改传感器返回的值
	if robot.step(40) == -1:
		break
	d2 = sensor.getValue(sensor)
	if d2 > d1:
		avoidCollision()

但是，通常建议的方法是，robot.step() 在主控制循环中使用单个功能调用，并使用它来同时更新所有传感器和执行器。

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

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

from controller import Robot, Motor, DistanceSensor

TIME_STEP = 32

robot = 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 sensors
    left_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 motors
    left_motor.setVelocity(left)
    right_motor.setVelocity(right)

使用控制器参数

在 .wbt 文件中，可以指定启动时传递给控制器的参数。在机器人节点的 controllerArgs字段中指定，并且作为函数的参数传递。
将controllerArgs 设定为 “one two three”
controller名称为demo

from controller import robot
import sys

robot = Robot()

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

例如：
在这里插入图片描述
输出就是这样：

控制器终止

控制器进程一般是无限循环的进行的，直到以下事件之一发生：

webots被退出
模拟重置
世界被重新加载
加载了新的模拟
控制器名称已更改

当上述事件之一发生时，robot.step() 函数返回-1。可以在controller即将终止之前保存数据：

from controller import Robot, DistanceSensor

TIME_STEP = 32

robot = Robot()

sensor = robot.getDevice("my_distance_sensor")
sensor = enable(TIME_STEP)

while robot.step(TIME_STEP) != -1:
	value = sensor.getValue()
	print("Sensor value is:", value)

# 保存数据
saveExperimentData()

使用controller停止仿真

import sys

if finished:
	saveExperimentData()
	sys.exit(0)

控制台输出

如前所述，默认情况下，打印到控制器stdout或stderr从控制器打印将被重定向到Webots控制台。但是请注意，Webots控制台不支持stdin输入。与大多数终端一样，它支持一些基本的ANSI转义代码，用于设置文本样式和清除控制台内容：

3位颜色（前景和背景）
粗体风格
下划线样式
清除屏幕（与clear终端中的发布命令相同）
重置（颜色和样式）

为了演示如何使用它们，在“ WEBOTS_HOME / projects / samples / howto / worlds / console.wbt ”和“ WEBOTS_HOME / projects / samples / howto / controllers / console / console ”中分别有一个示例世界和一个控制器文件。 c ”。

相关的C头文件位于“ WEBOTS_HOME / include / controller / c / webots / utils / ansi_codes.h ”中，它在常量之上还包含一些有用的宏，以供使用：

from controller import AnsiCodes

print("This is " + AnsiCodes.RED_FOREGROUND + "red" + AnsiCodes.RESET + "!")

共享库

创建共享库对于在控制器和/或插件之间共享代码非常有用。有几种方法可以这样做，但是我们建议将它们放在libraries项目目录的子目录中。实际上，已修改控制器的环境变量，以将这些路径包括到您的[[DY] LD_LIBRARY_] PATH环境变量中（取决于OS）。此外，用于编译Webots控制器的主要Makefile（“ WEBOTS_HOME / resources / Makefile.include ”）能够创建共享库，并可以轻松地与Controller库，ODE或Qt框架链接。

一个很好的例子是位于这里的Qt实用程序库：“ WEBOTS_HOME / resources / projects / libraries / qt_utils ”。

如果由于某种原因共享库不能在libraries目录中，则WEBOTS_LIBRARY_PATH环境变量将非常有帮助。启动控制器时，它包含的路径将添加到库搜索路径（[[DY] LD_LIBRARY_] PATH）的开头。

环境变量

对于某些项目，有必要定义或更改在您的环境中定义的变量。可以在计算机的设置中更改它们，但可能仅持续当前会话，或者与其他应用程序或项目产生冲突。Webots为此提供了一个优雅的解决方案。可以将名为“ runtime.ini”的配置文件添加到控制器目录。每次控制器启动时，此文件中定义的任何环境变量都将被加载到环境中。

该配置文件使用标准的INI模板，该模板非常简单，易于读写。它包含可以在[sections]内部的键和值对。使用分号“;”后可以在一行上写注释。特点。

在这个文件中的环境变量可以包含使用此语法到其它环境变量的引用：$(MY_VARIABLE_NAME)。它们将自动替换为环境中已存在的实际值。Webots的“ runtime.ini”支持7个部分：

[environment variables with paths]
本部分应仅包含具有相对路径或绝对路径的环境变量。路径必须使用冒号“：”分隔，目录组件必须使用斜杠“ /”分隔。本节中声明的变量将添加到每个平台上。在Windows上，根据Windows语法，冒号将由分号替换，斜杠将由反斜杠替换。
[environment variables]
本节中定义的环境变量也将添加到每个平台的环境中，但是它们将直接编写而无需更改语法。对于不包含任何路径的变量，这是一个很好的位置。
[environment variables for Windows]
如果在Windows平台上运行控制器，则仅将本节中定义的变量添加到环境中。如果要在本节中声明路径，则应在双引号符号“之间写入值。
[environment variables for macOS]
此处定义的变量将仅在macOS上添加，而在其他平台上被忽略。
[environment variables for Linux]
此处定义的变量将在所有Linux平台上添加，但不会在Mac和Windows上添加。
[environment variables for Linux 32]
仅当Linux平台为32位时，才添加这些变量。
[environment variables for Linux 64]
仅当Linux平台是64位时，才添加这些变量。

这是典型的runtime.ini文件的示例。

; typical runtime.ini

[environment variables with paths]
WEBOTS_LIBRARY_PATH = lib:$(WEBOTS_LIBRARY_PATH):../../library

[environment variables]
ROS_MASTER_URI = http://localhost:11311

[environment variables for Windows]
NAOQI_LIBRARY_FOLDER = "bin;C:\Users\My Documents\Naoqi\bin"

[environment variables for macOS]
NAOQI_LIBRARY_FOLDER = lib

[environment variables for Linux]
NAOQI_LIBRARY_FOLDER = lib

语言设定

该“runtime.ini”文件还可以包含语言的特定部分，命名[java]，[python]和[matlab]。每个部分都可以包括两个键，即COMMAND和OPTIONS。该COMMAND键允许您定义语言解释器的特定版本，而OPTIONS键则允许您访问将立即传递给语言解释器的特定选项。例如：

; runtime.ini for a Python controller on macOS

[python]
COMMAND = /opt/local/bin/python2.7
OPTIONS = -m package.name.given

在上面的示例中，Webots发出的结果命令将是：/opt/local/bin/python2.7 -m package.name.given my_controller.py可能后跟controllerArgs相应的Robot节点的字段的值。