【机器人仿真Webots教程】-控制器编程指南

Webots controller编程指南

1. controller程序设计流程

1.1 controller与场景树节点

在Webots中，场景树节点（Scene Tree Nodes）是Webots仿真环境中的各种对象，包括机器人模型、传感器、环境物体等。每个节点都有其在场景树中的位置，节点之间可以形成层次结构，以便组织和管理模拟环境。

控制器程序（Controller Program）是用于控制机器人在仿真环境中执行动作和决策的代码。它可以是任何支持Webots API的编程语言，例如C、C++、Python等。控制器程序可以访问和操纵场景树中的节点，以实现机器人的行为。

关联场景树节点和控制器程序的过程通常如下：

1. 创建或导入机器人模型和其他物体，这些将成为场景树节点的一部分。
2. 在Webots中创建一个控制器程序文件，并将其与一个或多个场景树节点关联。关联可以通过以下方式之一完成：
   • 使用Webots图形用户界面（GUI）：选择一个或多个节点，然后在GUI中设置它们的控制器程序路径。
   • 使用Webots API：在控制器程序中使用适当的API函数来获取对场景树节点的引用，并将其与控制器程序关联起来。
3. 编写控制器程序，使用适当的API函数来操作和控制场景树节点。这包括读取传感器数据、发送控制命令、更新节点状态等。
4. 在Webots中运行仿真，控制器程序将与场景树节点进行交互，实现机器人的控制和行为。

通过这种方式，控制器程序可以与场景树节点进行关联，并利用节点的属性和功能来控制仿真环境中的机器人行为。

1.2 进程角度分析

从进程的角度观察，每个控制器进程都是webots进程的子进程，同时每个控制器进程和webots进程并不共享内存（除Camera外）

1.3 仿真时间步长与控制器程序更新延时步长

step指的是仿真步长，这个步长是整个仿真环境（具体来说是场景树）中更新计算的时间间隔，在场景树中指定WorldInfo.basicTimeStep；

而wb_robot_step是controller程序的更新间隔，一个wb_robot_step包含若干step（simulation step）

例如仿真步长为16ms，那么wb_robot_step可以是16、32、64、128ms，单击界面中的step按钮，即走一个simulation step，如果wb_robot_step包含多个simulation step，那么会打断wb_robot_step的执行。

2. 例解控制器编程

2.1 Hello world 实例

wb_robot_init 为C Api在函数调用之前调用的初始化函数，初始化controller和webots lib的通信，在C API中特有，其他编程语言中并不存在

wb_robot_cleanup 做善后工作，在C API中特有，其他编程语言中并不存在。

wb_robot_step 在每个controller中都有，定期调用，因此通常放在主循环中，参数代表毫秒数，，为控制步骤的持续时间，函数根据设定的毫秒数进行仿真返回计算值，仿真时间量并不是真实的时间，在实际中可能是1毫秒或者一分钟的时间。注意，webots终止时返回wb_robot_step -1。只要仿真进行，那么控制回路就会运行

Hello world入门实例如下：

#include 
#include 

int main() {
  wb_robot_init();

  while(wb_robot_step(32) != -1)
    printf("Hello World!\n");

  wb_robot_cleanup();
  return 0;
}

2.2 传感器读取实例

#include 
#include 
#include 

#define TIME_STEP 32

int main() {
  wb_robot_init();

  WbDeviceTag sensor = wb_robot_get_device("my_distance_sensor");
  wb_distance_sensor_enable(sensor, TIME_STEP);

  while (wb_robot_step(TIME_STEP) != -1) {
    const double value = wb_distance_sensor_get_value(sensor);
    printf("Sensor value is %f\n", value);
  }

  wb_robot_cleanup();
  return 0;
}

使用设备需要获取设备的标签，类型为WbDeviceTag，获取方式为wb_robot_get_device，参数为机器人描述文件（.wbt或者.proto文件）中的设备名称，获取失败返回0

传感器设备在使用之前需要调用wb_*_enable进行使能，参数为获取的WbDeviceTag类型的变量（指定使能的设备）以及传感器数据两次更新之间的延时时间（TIME_STEP），一般的此处设置的延时时间与wb_robot_step的参数有关，一般为倍数关系，例如延时时间设置为控制步长的两倍，那么传感器每两次wb_robot_step调用更新一次，如果设置的延时时间比控制步长还要短是没有意义的，可以理解wb_robot_step的控制步长为一个最小单位时间。

wb_*_disable禁用设备，这样有可能会提高仿真的速度。wb_distance_sensor_get_value函数可以获取距离传感器的最新值，那么，类似的，还有很多获取传感器值的API，但是有可能这些设备返回的是个数组，例如加速度计、GPS、陀螺仪等等，接收数据如下：

API原型

const double *wb_gps_get_values(WbDeviceTag tag);
const double *wb_accelerometer_get_values(WbDeviceTag tag);
const double *wb_gyro_get_values(WbDeviceTag tag);

获取

const double *values = wb_gps_get_values(gps);

// OK, to read the values they should never be explicitly deleted by the controller code
printf("MY_ROBOT is at position: %g %g %g\n", values[0], values[1], values[2]);

// OK, to copy the values
double x, y, z;
x = values[0];
y = values[1];
z = values[2];

2.3 执行器的使用

controller也需要WbDeviceTag类型的变量作为入参指定设备，其不需要使能设备

电机控制实例

#include 
#include 
#include 

#define TIME_STEP 32

int main() {
  wb_robot_init();

  WbDeviceTag motor = wb_robot_get_device("my_motor");

  const double F = 2.0;   // frequency 2 Hz
  double t = 0.0;         // elapsed simulation time

  while (wb_robot_step(TIME_STEP) != -1) {
    const double position = sin(t * 2.0 * M_PI * F);
    wb_motor_set_position(motor, position);
    t += (double)TIME_STEP / 1000.0;
  }

  wb_robot_cleanup();
  return 0;
}

功能说明:使旋转电机以2Hz正弦信号震荡.

API解释

函数名	函数功能	参数
wb_motor_set_position	设置旋转电机位置	设备描述符,位置量

wb_motor_set_position设置位置之后并不会立即启动电机，而是等待wb_robot_step的驱动，将驱动命令发送到RationalMotor中，在指定的控制步长的时间（单位毫秒）内仿真电机的运动，一个控制步长的时间并不一定可以完成整个运动的过程。

#include 
#include 
#include 

#define TIME_STEP 32

int main() {
  wb_robot_init();

  WbDeviceTag motor = wb_robot_get_device("my_motor");

  const double F = 2.0;   // frequency 2 Hz
  double t = 0.0;         // elapsed simulation time

  while (wb_robot_step(TIME_STEP) != -1) {
    const double position = sin(t * 2.0 * M_PI * F);
    wb_motor_set_position(motor, position);
    t += (double)TIME_STEP / 1000.0;
  }

  wb_robot_cleanup();
  return 0;
}

一般情况下为控制运动的行进建模，将整个运动分解为离散的组合步骤，一般这个离散步骤的单位就是wb_robot_step的参数。

2.4 传感器与执行器综合例程

功能描述：机器人使用的是差动转向。它使用两个距离传感器 ( DistanceSensor) 来检测障碍物。

#include 
#include 
#include 

#define TIME_STEP 32

int main() {
  wb_robot_init();

  WbDeviceTag left_sensor = wb_robot_get_device("left_sensor");
  WbDeviceTag right_sensor = wb_robot_get_device("right_sensor");
  wb_distance_sensor_enable(left_sensor, TIME_STEP);
  wb_distance_sensor_enable(right_sensor, TIME_STEP);

  WbDeviceTag left_motor = wb_robot_get_device("left_motor");
  WbDeviceTag right_motor = wb_robot_get_device("right_motor");
  wb_motor_set_position(left_motor, INFINITY);
  wb_motor_set_position(right_motor, INFINITY);
  wb_motor_set_velocity(left_motor, 0.0);
  wb_motor_set_velocity(right_motor, 0.0);

  while (wb_robot_step(TIME_STEP) != -1) {

    // read sensors
    const double left_dist = wb_distance_sensor_get_value(left_sensor);
    const double right_dist = wb_distance_sensor_get_value(right_sensor);

    // compute behavior (user functions)
    const double left = compute_left_speed(left_dist, right_dist);
    const double right = compute_right_speed(left_dist, right_dist);

    // actuate wheel motors
    wb_motor_set_velocity(left_motor, left);
    wb_motor_set_velocity(right_motor, right);
  }

  wb_robot_cleanup();
  return 0;
}

注意，更新传感器状态一定要通过wb_robot_step，因为在wb_robot_step之前，API仅仅做了预备工作，并不会直接对仿真的进程进行推进（也即设备状态不会立即得到响应），只有调用了wb_robot_step才会推进仿真进程的响应。

2.5 控制器读入参数、程序终止

对于控制器的参数，通过控制器的main函数入口的argv接收，指定方式为机器人的controllerArgs节点字段

例：

参数：

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

控制器（名为demo）程序：

#include 
#include 

int main(int argc, const char *argv[]) {
  wb_robot_init();

  int i;
  for (i = 0; i < argc; i++)
    printf("argv[%i]=%s\n", i, argv[i]);

  wb_robot_cleanup();
  return 0;
}

输出：

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

一般控制器的主程序程序是一个大循环，控制器的终止也是循环的终止。导致控制器发生终止的事件一般有：

• Webots退出
• 仿真重置
• world重新加载
• 加载新的仿真
• 控制器名称修改

当上述事件发生时wb_robot_step返回-1，控制器进程不再与webots进程通信，在实际时间的1s后，如果控制器程序没有主动终止，Webots进行将发送SIGKILL信号杀死控制器进程，并给控制器程序足够的时间完成数据的转储和文件关闭。

示例：

功能描述：控制器程序终止之前保存数据。

#include 
#include 
#include 

#define TIME_STEP 32

int main() {
  wb_robot_init();

  WbDeviceTag sensor = wb_robot_get_device("my_distance_sensor");
  wb_distance_sensor_enable(sensor, TIME_STEP);

  while (wb_robot_step(TIME_STEP) != -1) {
    const double value = wb_distance_sensor_get_value();
    printf("sensor value is %f\n", value);
  }

  // Webots triggered termination detected!
  // Past this point, new printf statements will no longer be
  // displayed in the Webots console

  saveExperimentData();  // this shouldn't last longer than one second

  wb_robot_cleanup();
  return 0;
}

在某些情况下，由控制器做出终止模拟的决定。例如在搜索和优化算法的情况下：搜索可能会在找到解决方案时或在固定次数的迭代（或生成）之后终止。

在这种情况下，控制器应该只保存实验结果并通过从函数返回main或调用exit函数退出。这将终止控制器进程并在当前仿真步骤冻结仿真。物理仿真和仿真中涉及的每个机器人都将停止。

2.6 控制台

控制器程序支持stdout、stderr，并被程序重定向到了webots的控制台，webots不支持stdin（即标准输入），仅支持一些ANSI转义码进行文本样式的设置和清除，支持类型有以下所示：

• 3 位色彩（前景和背景）
• Blob 风格
• 下划线 风格
• clear清屏
• 重置（颜色和样式）

示例程序：

world文件：

WEBOTS_HOME/projects/samples/howto/console/worlds/console.wbt

控制器文件：

WEBOTS_HOME/projects/samples/howto/console/controllers/console/console.c

ANSI头文件：

WEBOTS_HOME/include/controller/c/webots/utils/ansi_codes.h

#include 

printf("This is %sred%s!\n", ANSI_RED_FOREGROUND, ANSI_RESET);

2.7 共享库&环境变量

共享库对于控制器程序和插件之间的代码共享非常有用

• 可以将共享库放入libraries子目录中

• WEBOTS_HOME/resources/Makefile.include，手动修改链接共享库
  示例： WEBOTS_HOME/resources/projects/libraries/qt_utils

• 将共享库添加到环境变量[[DY]LD_LIBRARY_]PATH

每个控制器程序目录下支持一个配置文件的方式自动在运行前加载环境变量到当前环境下，该配置文件名为"runtime.ini"，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 位时才会添加这些变量。

示例：

; 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，分别代表命令和选项

示例：

; runtime.ini for a Python controller on macOS

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

上述配置等同于

/opt/local/bin/python3.8 -m package.name.given my_controller.py

示例：

; runtime.ini for a Java controller on Windows

[environment variables with paths]
CLASSPATH = ../lib/MyLibrary.jar
JAVA_LIBRARY_PATH = ../lib

[java]
COMMAND = javaw.exe
OPTIONS = -Xms6144k

注意：Java -classpath（或 - cp）选项是从CLASSPATH环境变量自动生成的。因此，您不应将其添加到OPTIONS密钥中，而应将其添加到“runtime.ini”文件中的标准环境变量中。在上面的示例中，-classpath传递给 Java 虚拟机的最终选项包括“$(WEBOTS_HOME)/lib/Controller.jar”，当前目录（“.”）或控制器 jar 文件（如果存在）（“MyController.jar”）。 jar”），最后是“…/lib/MyLibrary.jar”。