Webots controller編程指南

1. controller程序設(shè)計流程

1.1 controller與場景樹節(jié)點

在Webots中，場景樹節(jié)點（Scene Tree Nodes）是Webots仿真環(huán)境中的各種對象，包括機器人模型、傳感器、環(huán)境物體等。每個節(jié)點都有其在場景樹中的位置，節(jié)點之間可以形成層次結(jié)構(gòu)，以便組織和管理模擬環(huán)境。

控制器程序（Controller Program）是用于控制機器人在仿真環(huán)境中執(zhí)行動作和決策的代碼。它可以是任何支持Webots API的編程語言，例如C、C++、Python等?？刂破鞒绦蚩梢栽L問和操縱場景樹中的節(jié)點，以實現(xiàn)機器人的行為。

關(guān)聯(lián)場景樹節(jié)點和控制器程序的過程通常如下：

1. 創(chuàng)建或?qū)霗C器人模型和其他物體，這些將成為場景樹節(jié)點的一部分。
2. 在Webots中創(chuàng)建一個控制器程序文件，并將其與一個或多個場景樹節(jié)點關(guān)聯(lián)。關(guān)聯(lián)可以通過以下方式之一完成：
   • 使用Webots圖形用戶界面（GUI）：選擇一個或多個節(jié)點，然后在GUI中設(shè)置它們的控制器程序路徑。
   • 使用Webots API：在控制器程序中使用適當(dāng)?shù)腁PI函數(shù)來獲取對場景樹節(jié)點的引用，并將其與控制器程序關(guān)聯(lián)起來。
3. 編寫控制器程序，使用適當(dāng)?shù)腁PI函數(shù)來操作和控制場景樹節(jié)點。這包括讀取傳感器數(shù)據(jù)、發(fā)送控制命令、更新節(jié)點狀態(tài)等。
4. 在Webots中運行仿真，控制器程序?qū)⑴c場景樹節(jié)點進行交互，實現(xiàn)機器人的控制和行為。

通過這種方式，控制器程序可以與場景樹節(jié)點進行關(guān)聯(lián)，并利用節(jié)點的屬性和功能來控制仿真環(huán)境中的機器人行為。

1.2 進程角度分析

從進程的角度觀察，每個控制器進程都是webots進程的子進程，同時每個控制器進程和webots進程并不共享內(nèi)存（除Camera外）

1.3 仿真時間步長與控制器程序更新延時步長

step指的是仿真步長，這個步長是整個仿真環(huán)境（具體來說是場景樹）中更新計算的時間間隔，在場景樹中指定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的執(zhí)行。

2. 例解控制器編程

2.1 Hello world 實例

wb_robot_init 為C Api在函數(shù)調(diào)用之前調(diào)用的初始化函數(shù)，初始化controller和webots lib的通信，在C API中特有，其他編程語言中并不存在

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

wb_robot_step 在每個controller中都有，定期調(diào)用，因此通常放在主循環(huán)中，參數(shù)代表毫秒數(shù)，，為控制步驟的持續(xù)時間，函數(shù)根據(jù)設(shè)定的毫秒數(shù)進行仿真返回計算值，仿真時間量并不是真實的時間，在實際中可能是1毫秒或者一分鐘的時間。注意，webots終止時返回wb_robot_step -1。只要仿真進行，那么控制回路就會運行

Hello world入門實例如下：

#include <webots/robot.h>
#include <stdio.h>

int main() {
  wb_robot_init();

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

  wb_robot_cleanup();
  return 0;
}

2.2 傳感器讀取實例

#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <stdio.h>

#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;
}

使用設(shè)備需要獲取設(shè)備的標簽，類型為WbDeviceTag，獲取方式為wb_robot_get_device，參數(shù)為機器人描述文件（.wbt或者.proto文件）中的設(shè)備名稱，獲取失敗返回0

傳感器設(shè)備在使用之前需要調(diào)用wb_*_enable進行使能，參數(shù)為獲取的WbDeviceTag類型的變量（指定使能的設(shè)備）以及傳感器數(shù)據(jù)兩次更新之間的延時時間（TIME_STEP），一般的此處設(shè)置的延時時間與wb_robot_step的參數(shù)有關(guān)，一般為倍數(shù)關(guān)系，例如延時時間設(shè)置為控制步長的兩倍，那么傳感器每兩次wb_robot_step調(diào)用更新一次，如果設(shè)置的延時時間比控制步長還要短是沒有意義的，可以理解wb_robot_step的控制步長為一個最小單位時間。

wb_*_disable禁用設(shè)備，這樣有可能會提高仿真的速度。wb_distance_sensor_get_value函數(shù)可以獲取距離傳感器的最新值，那么，類似的，還有很多獲取傳感器值的API，但是有可能這些設(shè)備返回的是個數(shù)組，例如加速度計、GPS、陀螺儀等等，接收數(shù)據(jù)如下：

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 執(zhí)行器的使用

controller也需要WbDeviceTag類型的變量作為入?yún)⒅付ㄔO(shè)備，其不需要使能設(shè)備

電機控制實例

#include <webots/robot.h>
#include <webots/motor.h>
#include <math.h>

#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;
}

功能說明:使旋轉(zhuǎn)電機以2Hz正弦信號震蕩.

API解釋

wb_motor_set_position設(shè)置位置之后并不會立即啟動電機，而是等待wb_robot_step的驅(qū)動，將驅(qū)動命令發(fā)送到RationalMotor中，在指定的控制步長的時間（單位毫秒）內(nèi)仿真電機的運動，一個控制步長的時間并不一定可以完成整個運動的過程。

#include <webots/robot.h>
#include <webots/motor.h>
#include <math.h>

#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的參數(shù)。

2.4 傳感器與執(zhí)行器綜合例程

功能描述：機器人使用的是差動轉(zhuǎn)向。它使用兩個距離傳感器 ( DistanceSensor) 來檢測障礙物。

#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/distance_sensor.h>

#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;
}

注意，更新傳感器狀態(tài)一定要通過wb_robot_step，因為在wb_robot_step之前，API僅僅做了預(yù)備工作，并不會直接對仿真的進程進行推進（也即設(shè)備狀態(tài)不會立即得到響應(yīng)），只有調(diào)用了wb_robot_step才會推進仿真進程的響應(yīng)。

2.5 控制器讀入?yún)?shù)、程序終止

對于控制器的參數(shù)，通過控制器的main函數(shù)入口的argv接收，指定方式為機器人的controllerArgs節(jié)點字段

例：

參數(shù)：

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

控制器（名為demo）程序：

#include <webots/robot.h>
#include <stdio.h>

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

一般控制器的主程序程序是一個大循環(huán)，控制器的終止也是循環(huán)的終止。導(dǎo)致控制器發(fā)生終止的事件一般有：

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

當(dāng)上述事件發(fā)生時wb_robot_step返回-1，控制器進程不再與webots進程通信，在實際時間的1s后，如果控制器程序沒有主動終止，Webots進行將發(fā)送SIGKILL信號殺死控制器進程，并給控制器程序足夠的時間完成數(shù)據(jù)的轉(zhuǎn)儲和文件關(guān)閉。

示例：

功能描述：控制器程序終止之前保存數(shù)據(jù)。

#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <stdio.h>

#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;
}

在某些情況下，由控制器做出終止模擬的決定。例如在搜索和優(yōu)化算法的情況下：搜索可能會在找到解決方案時或在固定次數(shù)的迭代（或生成）之后終止。

在這種情況下，控制器應(yīng)該只保存實驗結(jié)果并通過從函數(shù)返回main或調(diào)用exit函數(shù)退出。這將終止控制器進程并在當(dāng)前仿真步驟凍結(jié)仿真。物理仿真和仿真中涉及的每個機器人都將停止。

2.6 控制臺

控制器程序支持stdout、stderr，并被程序重定向到了webots的控制臺，webots不支持stdin（即標準輸入），僅支持一些ANSI轉(zhuǎn)義碼進行文本樣式的設(shè)置和清除，支持類型有以下所示：

• 3 位色彩（前景和背景）
• Blob 風(fēng)格
• 下劃線 風(fēng)格
• 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 <webots/utils/ansi_codes.h>

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

2.7 共享庫&環(huán)境變量

共享庫對于控制器程序和插件之間的代碼共享非常有用

• 可以將共享庫放入libraries子目錄中

• WEBOTS_HOME/resources/Makefile.include，手動修改鏈接共享庫
  示例： WEBOTS_HOME/resources/projects/libraries/qt_utils

• 將共享庫添加到環(huán)境變量[[DY]LD_LIBRARY_]PATH

每個控制器程序目錄下支持一個配置文件的方式自動在運行前加載環(huán)境變量到當(dāng)前環(huán)境下，該配置文件名為"runtime.ini"，runtime.ini以鍵值對的形式設(shè)置，包括以下7個字段：

• [environment variables with paths]

  此部分應(yīng)僅包含具有相對或絕對路徑的環(huán)境變量。路徑必須使用冒號“:”分隔，目錄組件必須使用斜杠符號“/”分隔。本節(jié)中聲明的變量將添加到每個平臺上。在 Windows 上，根據(jù) Windows 語法，冒號將替換為分號，斜線將替換為反斜線。

• [environment variables]

  本節(jié)中定義的環(huán)境變量也將添加到每個平臺的環(huán)境中，但它們將直接寫入而不更改語法。對于不包含任何路徑的變量來說，這是一個很好的位置。

• [environment variables for Windows]

  如果控制器在 Windows 平臺上運行，則此部分中定義的變量將僅添加到環(huán)境中。如果你想在這個部分聲明路徑，值應(yīng)該寫在雙引號符號之間。

• [environment variables for macOS]

  此處定義的變量只會在 macOS 上添加，在其他平臺上會被忽略。

• [environment variables for Linux]

  此處定義的變量將添加到所有 Linux 平臺上，但不會添加到 Mac 或 Windows 上。

• [environment variables for Linux 32]

  僅當(dāng) Linux 平臺為 32 位時才會添加這些變量。

• [environment variables for Linux 64]

  僅當(dāng) 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中還有關(guān)于編程語言的特定配置項，字段分別有

• [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環(huán)境變量自動生成的。因此，您不應(yīng)將其添加到OPTIONS密鑰中，而應(yīng)將其添加到“runtime.ini”文件中的標準環(huán)境變量中。在上面的示例中，-classpath傳遞給 Java 虛擬機的最終選項包括“$(WEBOTS_HOME)/lib/Controller.jar”，當(dāng)前目錄（“.”）或控制器 jar 文件（如果存在）（“MyController.jar”）。 jar”），最后是“…/lib/MyLibrary.jar”。文章來源地址http://www.zghlxwxcb.cn/news/detail-458634.html

到了這里，關(guān)于【機器人仿真Webots教程】-控制器編程指南的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！