6.1 Controller Programming
The programming examples provided here are in C, but same concepts apply to C++/Java/Python/Matlab.
6.1.1 Hello World Example
The tradition in computer science is to start with a "Hello World!" example. So here is a "Hello World!" example for a Webots controller:
#include <webots/robot.h> |
Webots C API (Application Programming Interface) is provided by regular C header files. These header files must be included using statements like #include <webots/xyz.h> where xyz represents the name of a Webots node in lowercase. Like with any regular C code it is also possible to include the standard C headers, e.g. #include <stdio.h>. A call to the initialization function wb_robot_init() is required before any other C API function call. This function initializes the communication between the controller and Webots. Note that wb_robot_init() exists only in the C API, it does not have any equivalent in the other supported programming languages.
Usually the highest level control code is placed inside a for or a while loop. Within that loop there is a call to the wb_robot_step() function. This function synchronizes the controller's data with the simulator. The function wb_robot_step() needs to be present in every controller and it must be called at regular intervals, therefore it is usually placed in the main loop as in the above example. The value 32 specifies the duration of the control steps, i.e. the function wb_robot_step() shall compute 32 milliseconds of simulation and then return. This duration specifies an amount of simulated time, not real (wall clock) time, so it may actually take 1 millisecond or one minute of CPU time, depending on the complexity of the simulated world.
Note that in this "Hello World!" example the while loop has no exit condition, hence the return statement is never reached. It is usual to have an infinite loop like this in the controller code: the result is that the controller runs as long as the simulation runs.
6.1.2 Reading Sensors
Now that we have seen how to print a message to the console, we shall see how to read the sensors of a robot. The next example does continuously update and print the value returned by a DistanceSensor:
#include <webots/robot.h> |
Each sensor must be enabled before it can be used. If a sensor is not enabled it returns undefined values. Enabling a sensor is achieved using the corresponding wb_*_enable() function, where the star (*) stands for the sensor type. Every wb_*_enable() function allows to specify an update delay in milliseconds. The update delay specifies the desired interval between two updates of the sensor's data.
In the usual case, the update delay is chosen to be similar to the control step (TIME_STEP) and hence the sensor will be updated at every wb_robot_step(). If, for example, the update delay is chosen to be twice the control step then the sensor data will be updated every two wb_robot_step(): this can be used to simulate a slow device. Note that a larger update delay can also speed up the simulation, especially for CPU intensive devices like the Camera. On the contrary, it would be pointless to choose an update delay smaller than the control step, because it will not be possible for the controller to process the device's data at a higher frequency than that imposed by the control step. It is possible to disable a device at any time using the corresponding wb_*_disable() function. This may increase the simulation speed.
The sensor value is updated during the call to wb_robot_step(). The call to wb_distance_sensor_get_value() retrieves the latest value.
Note that some device return vector values instead of scalar values, for example these functions:
const double *wb_gps_get_values(WbDeviceTag tag); |
const double *pos = wb_gps_get_values(gps); |
const double *pos = wb_gps_get_values(gps); |
6.1.3 Using Actuators
The example below shows how to make a servo motor oscillate with a 2 Hz sine signal.
Just like sensors, each Webots actuator must be identified by a WbDeviceTag returned by the wb_robot_get_device() function. However, unlike sensors, actuators don't need to be expressly enabled; they actually don't have wb_*_enable() functions.
To control a motion, it is generally useful to decompose that motion in discrete steps that correspond to the control step. As before, an infinite loop is used here: at each iteration a new target position is computed according to a sine equation. The wb_servo_set_position() function stores a new position request for the corresponding servo motor. Note that wb_servo_set_position() stores the new position, but it does not immediately actuate the motor. The effective actuation starts on the next line, in the call to wb_robot_step(). The wb_robot_step() function sends the actuation command to the Servo but it does not wait for the Servo to complete the motion (i.e. reach the specified target position); it just simulates the motor's motion for the specified number of milliseconds.
#include <webots/robot.h> |
Note that wb_servo_set_position() only specifies the desired target position. Just like with real robots, it is possible (in physics-based simulations only), that the Servo is not able to reach this position, because it is blocked by obstacles or because the motor's torque (maxForce) is insufficient to oppose to the gravity, etc.
If you want to control the motion of several Servos simultaneously, then you need to specify the desired position for each Servo separately, using wb_servo_set_position(). Then you need to call wb_robot_step() once to actuate all the Servos simultaneously.
6.1.4 How to use wb_robot_step()
Webots uses two different time steps:
-
The control step (the argument of the wb_robot_step() function)
-
The simulation step (specified in the Scene Tree: WorldInfo.basicTimeStep)
The control step is the duration of an iteration of the control loop. It corresponds to the parameter passed to the wb_robot_step() function. The wb_robot_step() function advances the controller time of the specified duration. It also synchronizes the sensor and actuator data with the simulator according to the controller time.
Every controller needs to call wb_robot_step() at regular intervals. If a controller does not call wb_robot_step() the sensors and actuators won't be updated and the simulator will block (in synchronous mode only). Because it needs to be called regularly, wb_robot_step() is usually placed in the main loop of the controller.
The simulation step is the value specified in WorldInfo.basicTimeStep (in milliseconds). It indicates the duration of one step of simulation, i.e. the time interval between two computations of the position, speed, collisions, etc. of every simulated object. If the simulation uses physics (vs. kinematics), then the simulation step also specifies the interval between two computations of the forces and torques that need to be applied to the simulated rigid bodies.
The execution of a simulation step is an atomic operation: it cannot be interrupted. Hence a sensor measurement or a motor actuation can only take place between two simulation steps. For that reason the control step specified with each wb_robot_step() must be a multiple of the simulation step. So for example, if the simulation step is 16 ms, then the control step argument passed to wb_robot_step() can be 16, 32, 64, 128, etc.
6.1.5 Using Sensors and Actuators Together
Webots and each robot controller are executed in separate processes. For example, if a simulation involves two robots, there will be three processes in total: one for Webots and two for the two robots. Each controller process exchanges sensors and actuators data with the Webots process during the calls to wb_robot_step(). So for example, wb_servo_set_position() does not immediately send the data to Webots. Instead it stores the data locally and the data are effectively sent when wb_robot_step() is called.
For that reason the following code snippet is a bad example. Clearly, the value specified with the first call to wb_servo_set_position() will be overwritten by the second call:
wb_servo_set_position(my_leg, 0.34); // BAD: ignored |
Similarly this code does not make much sense either:
while (1) { |
while (1) { |
while (1) { |
while (1) { |
#include <webots/robot.h> |
6.1.6 Using Controller Arguments
In the .wbt file, it is possible to specify arguments that are passed to a controller when it starts. They are specified in the controllerArgs field of the Robot, Supervisor or DifferentialWheels node, and they are passed as parameters of the main() function. For example, this can be used to specify parameters that vary for each robot's controller.
For example if we have:
Robot { |
#include <webots/robot.h> |
argv[0]=demo |
6.1.7 Controller Termination
Usually a controller process runs in an endless loop: it is terminated (killed) by Webots when the user reverts (reloads) the simulation or quits Webots. The controller cannot prevent its own termination but it can be notified shortly before this happens. The wb_robot_step() function returns -1 when the process is going to be terminated by Webots. Then the controller has 1 second (clock time) to save important data, close files, etc. before it is effectively killed by Webots. Here is an example that shows how to detect the upcoming termination:
#include <webots/robot.h> |
In this case the controller should just save the experiment results and quit by returning from the main() function or by calling the exit() function. This will terminate the controller process and freeze the simulation at the current simulation step. The physics simulation and every robot involved in the simulation will stop.
// freeze the whole simulation |
If only one robot controller needs to terminate but the simulation should continue with the other robots, then the terminating robot should call wb_robot_cleanup() right before quitting:
// terminate only this robot controller |