[Robot Simulation Webots Tutorial] - Controller Programming Guide

Webots controller programming guide

1. Controller program design process

1.1 controller and scene tree nodes

In Webots, Scene Tree Nodes are various objects in the Webots simulation environment, including robot models, sensors, environmental objects, etc. Each node has its position in the scene tree, and a hierarchy can be formed between nodes to organize and manage the simulation environment.

The controller program (Controller Program) is the code used to control the robot to perform actions and decisions in the simulation environment. It can be any programming language that supports Webots API, such as C, C++, Python, etc. The controller program can access and manipulate the nodes in the scene tree to achieve the behavior of the robot.

The process of associating scene tree nodes and controller programs is usually as follows:

1. Create or import robot models and other objects that will become part of the scene tree nodes.
2. Create a controller program file in Webots and associate it with one or more scene tree nodes. Association can be done in one of the following ways:
   • Using the Webots Graphical User Interface (GUI): Select one or more nodes and set their controller program paths in the GUI.
   • Using the Webots API: Use the appropriate API function in the controller program to obtain a reference to the scene tree node and associate it with the controller program.
3. Write the controller program to use the appropriate API functions to manipulate and control the scene tree nodes. This includes reading sensor data, sending control commands, updating node status, etc.
4. Running the simulation in Webots, the controller program will interact with the scene tree nodes to realize the control and behavior of the robot.

In this way, the controller program can be associated with the scene tree nodes and use the properties and functions of the nodes to control the robot behavior in the simulation environment.

1.2 Process perspective analysis

From the perspective of the process, each controller process is a child process of the webots process, and each controller process and webots process do not share memory (except Camera)

1.3 Simulation time step and controller program update delay step

step refers to the simulation step size, which is the time interval for updating calculations in the entire simulation environment (specifically, the scene tree), and is specified in the scene tree WorldInfo.basicTimeStep;

It iswb_robot_step the update interval of the controller program, a wb_robot_step contains several steps (simulation step)

For example, the simulation step length is 16ms, then wb_robot_step can be 16, 32, 64, 128ms. Click the step button in the interface to take a simulation step. If wb_robot_step contains multiple simulation steps, the execution of wb_robot_step will be interrupted.

2. Example controller programming

2.1 Hello world example

wb_robot_initIt is the initialization function called by C Api before the function call, and initializes the communication between the controller and webots lib. It is unique in C API and does not exist in other programming languages.

wb_robot_cleanupDoing aftermath, unique to the C API, does not exist in other programming languages.

wb_robot_stepIn each controller, it is called periodically, so it is usually placed in the main loop. The parameter represents the number of milliseconds, and is the duration of the control step. The function performs simulation according to the set number of milliseconds and returns the calculated value. The simulation time is not The real time may be 1 millisecond or a minute in practice. Note that webots returns -1 when terminated wb_robot_step. The control loop runs as long as the simulation is running

An example of getting started with Hello world is as follows:

#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 Example of sensor reading

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

To use the device, you need to obtain the tag of the device. The type is WbDeviceTag, and the method of obtaining is wb_robot_get_device. The parameter is the device name in the robot description file (.wbt or .proto file). If the acquisition fails, return 0

The sensor device needs to be called to enable it before it can be used wb_*_enable. The parameter is the obtained WbDeviceTag type variable (designated enabled device) and the delay time between two updates of the sensor data (TIME_STEP). Generally, the delay set here The time wb_robot_stepis related to the parameters, generally a multiple relationship. For example, if the delay time is set to twice the control step, the sensor will be wb_robot_stepupdated every two calls. It is meaningless if the delay time is set to be shorter than the control step. , the understandable wb_robot_stepcontrol step size is a minimum unit time.

wb_*_disableDisables the device, which may increase the speed of the simulation. wb_distance_sensor_get_valueThe function can get the latest value of the distance sensor. Then, similarly, there are many APIs to get the sensor value, but it is possible that these devices return an array, such as accelerometer, GPS, gyroscope, etc. The received data is as follows:

API prototype

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

Obtain

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 Use of Actuator

The controller also needs a variable of WbDeviceTag type as an input parameter to specify the device, and it does not need to enable the device

Motor Control Example

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

Function description: Make the rotating motor oscillate with a 2Hz sinusoidal signal.

API explained

Function name	function function	parameter
wb_motor_set_position	Set Rotary Motor Position	device descriptor , location

wb_motor_set_positionAfter setting the position, the motor does not start immediately, but waits for wb_robot_stepthe drive, sends the drive command to RationalMotor, and simulates the movement of the motor within the specified control step time (in milliseconds). The time of one control step does not Must be able to complete the entire exercise process.

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

In general, to model the progress of the control motion, the entire motion is decomposed into discrete combination steps. Generally, the unit of this discrete step is the wb_robot_stepparameter of .

2.4 Integrated routines for sensors and actuators

Function description: The robot uses differential steering. It uses two distance sensors (DistanceSensor) to detect obstacles.

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

Note that updating the sensor status must be passed wb_robot_step, because before wb_robot_step, the API only does preparatory work, and does not directly advance the simulation process (that is, the device status will not be responded immediately), and the simulation will only be advanced when it is wb_robot_stepcalled Process response.

2.5 The controller reads in parameters and the program terminates

For the parameters of the controller, it is received through the argv of the main function entry of the controller, and the specified method is the controllerArgs node field of the robot

example:

parameter:

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

Controller (named demo) program:

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

output:

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

Generally, the main program of the controller is a large loop, and the termination of the controller is also the termination of the loop. Events that cause the controller to terminate generally include:

• Webots exit
• simulation reset
• world reload
• load new simulation
• Controller name modification

When the above event occurs, wb_robot_stepit returns -1, and the controller process no longer communicates with the webots process. After 1 second of the actual time, if the controller program does not actively terminate, Webots will send a SIGKILL signal to kill the controller process and give the controller Enough time for the program to complete the dump of the data and close the file.

Example:

Function description: Save data before the controller program terminates.

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

In some cases, the decision to terminate the simulation is made by the controller. For example in the case of search and optimization algorithms: the search may terminate when a solution is found or after a fixed number of iterations (or generations).

In this case, the controller should just save the experiment results and exit by returning from the function mainor calling the function. exitThis will terminate the controller process and freeze the simulation at the current simulation step. Every robot involved in the physics simulation and simulation will be stopped.

2.6 Console

The controller program supports stdout and stderr, and is redirected to the webots console by the program. Webots does not support stdin (that is, standard input), but only supports some ANSI escape codes for setting and clearing text styles. The supported types are as follows :

• 3-bit color (foreground and background)
• blob style
• underline style
• clear clear screen
• reset (color and style)

Sample program:

world file:

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

Controller file:

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

ANSI header file:

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 Shared library & environment variables

Shared libraries are useful for code sharing between controller programs and plugins

• Shared libraries can be placed into the libraries subdirectory

• WEBOTS_HOME/resources/Makefile.include, manually modify the linked shared library
  Example: WEBOTS_HOME/resources/projects/libraries/qt_utils

• Add the shared library to the environment variable [[DY]LD_LIBRARY_]PATH

Each controller program directory supports a configuration file to automatically load environment variables into the current environment before running. The configuration file is named "runtime.ini", and runtime.ini is set in the form of key-value pairs, including the following 7 fields:

• [environment variables with paths]

  This section should only contain environment variables with relative or absolute paths. Paths must be separated by a colon ':' and directory components must be separated by a slash symbol '/'. Variables declared in this section will be added on each platform. On Windows, colons are replaced with semicolons and slashes are replaced with backslashes according to Windows syntax.

• [environment variables]

  The environment variables defined in this section will also be added to each platform's environment, but they will be written directly without changing the syntax. This is a good location for variables that don't contain any paths.

• [environment variables for Windows]

  The variables defined in this section will only be added to the environment if the controller is running on a Windows platform. If you want to declare the path in this section, the value should be written between double quotes symbols.

• [environment variables for macOS]

  Variables defined here are only added on macOS and ignored on other platforms.

• [environment variables for Linux]

  Variables defined here will be added on all Linux platforms, but not on Mac or Windows.

• [environment variables for Linux 32]

  These variables are only added if the Linux platform is 32-bit.

• [environment variables for Linux 64]

  These variables are only added if the Linux platform is 64-bit.

Example:

; 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

At the same time, there are specific configuration items about programming languages ​​in runtime.ini, and the fields are respectively

• [java]
• [python]
• [matlab]

Each field contains two keys COMMANDand OPTIONS, which represent commands and options respectively

Example:

; runtime.ini for a Python controller on macOS

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

The above configuration is equivalent to

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

Example:

; 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

Note : The Java -classpath(or - cp) option is CLASSPATHautomatically generated from the environment variable. Therefore, you should not add it to OPTIONSthe key, but to the standard environment variables in the "runtime.ini" file. In the example above, -classpaththe final options passed to the Java Virtual Machine include "$(WEBOTS_HOME)/lib/Controller.jar", the current directory ("."), or the controller jar file if present ("MyController.jar" ). jar") and finally ".../lib/MyLibrary.jar".