Webots Reference Manual - chapter 7 - section 4

Webots Reference Manual


7.4 Developing Your Own Fast2D Plugin

The Enki-based Fast2D plugin that comes with Webots is highly optimized, and should be suitable for most 2D simulations. However, in some cases you might want to use your own implementation of kinematics and collision detection. In this case you will have to develop your own Fast2D plugin; this section explains how to proceed.

7.4.1 Header File

The data types and interfaces required to compile your own Fast2D plugin are defined in the fast2d.h header file. This file is located in Webots installation directory, in the include/plugins/fast2d subdirectory. It can be included like this:

#include <plugins/fast2d/fast2d.h>
...

The fast2d.h file contains C types and function declarations; it can be compiled with either a C or C++ compiler.

7.4.2 Fast2D Plugin Types

Four basic types are defined in fast2d.h: ObjectRef, SolidRef, RobotRef and SensorRef. In order to enforce a minimal amount of type-checking and type-awareness, these basic types are declared as non-interchangeable pointer types. They are only dummy types, not designed to be used as-is, but rather to be placeholders for the real data types that the plugin programmer is responsible for implementing. We suggest that you declare your own four data types as C structs or C++ classes. Then in your implementation of the Fast2D functions, you should cast the addresses of your data instances to the Fast2D types, as in the example below, where MyRobotClass and MySensorClass are user-defined types:

RobotRef webots_fast2d_create_robot() {
  return (RobotRef) new MyRobotClass();
}

void webots_fast2d_robot_add_sensor(RobotRef robotRef, 
          SensorRef sensorRef, double x, double y, double angle) {

  MyRobotClass *robot = (MyRobotClass*) robotRef;
  MySensorClass *sensor = (MySensorClass*) sensorRef;
  robot->addSensor(sensor, x, y, angle);
  ...
}

In this example, Webots calls webots_fast2d_create_robot() when it requires a new robot object; this function instantiates the object and casts it to a Fast2D type before returning it. Webots will then pass back this pointer as an argument to every subsequent plugin call that involves the same object. Apart from storing its address and passing it back, Webots does nothing with the object; it is completely safe for you to cast to any pointer type. However, the simplest and most effective method is to directly cast the addresses of your data instances. You are however free to do otherwise, provided that you assign a unique reference to each object.

Your data types should contain certain attributes in order for the Fast2D functions to be able to operate on them. The UML diagram in in figure 7.1 shows the types and attributes that make sense according to the Fast2D functionality. This diagram is an implementation guideline for your own type declarations. We recommended implementing four data types in order to match exactly the four Fast2D basic types; we also suggest that in the implementation of these types you use similar attributes as those indicated in the diagram.

fast2d_class_diagram

Figure 7.1: Fast2D Plugin Entity Relationship

7.4.3 Fast2D Plugin Functions

In order for your plugin to be operational, it has to implement all of the Fast2D functions. Once the plugin is loaded, Webots checks that every function is present; if a function is missing, Webots will attempt to run the simulation using the built-in 3D routines instead of the Fast2D plugin.

The Fast2D API uses two types of coordinates: global and local. The global coordinate system is the world's coordinate system, as described in table 7.1. Positions and angles of an ObjectRef (including RobotRef and SolidRef) are expressed in the global coordinate system. On the other hand, the position and angle of SensorRef and the coordinates of bounding rectangles are expressed in the local coordinate system of the object they belong to. For example, the position and angle of a sensor is expressed with respect to the local coordinate system of the robot which contains the sensor. As in 3D, an angle of zero in the Fast2D coordinate system matches up with a direction parallel to the x-axis.

void webots_fast2d_init()

The webots_fast2d_init() function is called by Webots to initialize the plugin. This function is called before any other Fast2D function: its purpose is to allocate and initialize the plugin's global data structures. Note than when the Revert button is pressed or whenever something changes in the scene tree, Webots reinitializes the plugin by first calling webots_fast2d_cleanup() and then webots_fast2d_init(). See also figure 7.2.

void webots_fast2d_cleanup()

This function must be implemented to free all the data structures used by the plugin. After a call to this function, no further Fast2D calls will be made by Webots, with the exception of webots_fast2d_init(). A subsequent call to webots_fast2d_init() will indicate that the plugin must be reinitialized because the world is being re-created. The plugin is responsible for allocating and freeing all of the Fast2D objects. If webots_fast2d_cleanup() fails to free all the memory that was allocated by the plugin, this will result in memory leaks in Webots.

void webots_fast2d_step(double dt)

This function must perform a simulation step of dt seconds. It is invoked by Webots once for each simulation step (basic simulation step) when the simulation is running, or once each time the Step button is pressed. The dt parameter corresponds to the world's basic time step (set in the WorldInfo node) converted to seconds (i.e., divided by 1000). The job of this function is to compute the new position and angle (as returned by webots_fast2d_object_get_transform()) of every simulated object (ObjectRef) according to your implementation of kinematics and collision handling. This function usually requires the largest amount of implementation work on the user's part.

RobotRef webots_fast2d_create_robot()

Requests the creation of a robot by the plugin. This function must return a valid robot reference (RobotRef) to Webots. The exact properties of the robot will be specified in subsequent Fast2D calls.

SolidRef webots_fast2d_create_solid()

Requests the creation of a solid object by the plugin. This function must return a valid solid reference (SolidRef) to Webots. The exact properties of the solid object will be specified in subsequent Fast2D calls.

void webots_fast2d_add_object(ObjectRef object)

Requests the insertion of an object (robot or solid) into the 2D world model. This function is called by Webots after an object's properties have been set and before executing the first simulation step (webots_fast2d_step()).

SensorRef webots_fast2d_create_irsensor(RobotRef robot, double xpos, double ypos, double angle, double range, double aperture, int numRays, const double rayWeights[])

Requests the creation of an infra-red sensor. This function must return a valid sensor reference (SensorRef) to Webots. The robot parameter is a robot reference previously created through webots_fast2d_create_robot(). The xpos, ypos and angle parameters indicate the desired position and orientation of the sensor in the the local coordinate system of the robot. The range parameter indicates the maximum range of the sensor. It is determined by the lookupTable of the corresponding DistanceSensor in the Webots scene tree. The aperture parameter corresponds to the value of the aperture field of the DistanceSensor. The numRays parameter indicates the value of the numberOfRays field of the DistanceSensor. The rayWeights parameter is an array of numRays double-precision floats which specifies the individual weights that must be associated with each sensor ray. The sum of the ray weights provided by Webots is always exactly 1.0, and it is always left/right symmetrical. For more information on the sensor weights, please refer to the description of the DistanceSensor node in the Webots Reference Manual. In order to be consistent with the Webots graphical representation, the plugin's implementation of the sensors requires that:

double webots_fast2d_sensor_get_activation(SensorRef sensor)

Requests the current distance measured by a sensor. The sensor parameter is a sensor reference that was created through webots_fast2d_create_irsensor(). This function must return the average of the weighted distances measured by the sensor rays. The distances must be weighted using the rayWeights values that were passed to webots_fast2d_create_ irsensor(). Note that this function is responsible only for calculating the weighted average distance measured by the sensor. It is Webots responsibility to compute the final activation value (the value that will finally be returned to the controller) from the average distance and according to the DistanceSensor's lookup table.

void webots_fast2d_object_set_bounding_rectangle(ObjectRef object, const double x[4], const double y[4])

Defines an object as rectangular and sets the object's bounding rectangle. The object parameter is a solid or robot reference. The x and y arrays specify the coordinates of the four corners of the bounding rectangle in the object's coordinate system. The sequence (x[0], y[0]), (x[1], y[1]), (x[2], y[2]), (x[3], y[3]) is specified counter-clockwise.

void webots_fast2d_object_set_bounding_radius(ObjectRef object, double radius)

Defines an object as circular and sets the objects's bounding radius. The object parameter is a solid or robot reference. In the Fast2D plugin, an object can be either rectangular or circular; Webots indicates this by calling either webots_fast2d_object_set_bounding_rectangle() or webots_fast2d_object_set_bounding_radius().

void webots_fast2d_object_set_mass(ObjectRef object, double mass)

Request to set the mass of an object. The object parameter is a solid or robot reference. The mass parameter is the object's required mass. According to your custom implementation, the mass of an object can be involved in the calculation of a robot's acceleration and ability to push other objects. The implementation of this function is optional. Note that Webots calls this function only if the corresponding object has a Physics node. In this case the mass parameter equals the mass field of the Physics node. A negative mass must be considered infinite. If your model does not support the concept of mass, you should implement an empty webots_fast2d_object_set_mass() function.

void webots_fast2d_object_set_position(ObjectRef object, double xpos, double ypos)

Request to set the position of an object. The object parameter is a solid or robot reference. The xpos and ypos parameters represent the required position specified in the global coordinate system. This function is called by Webots during the construction of the world model. Afterwards, the object positions are only modified by the webots_fast2d_step() function. See also figure 7.2.

void webots_fast2d_object_set_angle(ObjectRef object, double angle)

Request to set the angle of an object. The object parameter is a solid or robot reference. The angle parameter is the requested object angle specified in the global coordinate system. This function is called by Webots during the construction of the world model. Afterwards, the object angles are only modified by the webots_fast2d_step() function. See also figure 7.2.

void webots_fast2d_robot_set_speed(RobotRef robot, double dx, double dy)

Request to change the speed of a robot. The robot parameter is a robot reference. The dx and dy parameters are the two vector components of the robot's speed in the global coordinate system. This corresponds to change per second in the position of the robot (xpos and ypos). More precisely: dx = v * sin(α) and dy = v * cos(α), where α is the robot's orientation angle and where v is the robot's absolute speed which is calculated according to the wheels' radius and rotation speed. For more information, see the description of the DifferentialWheels node and the differential_wheels_set_speed() function in the Webots Reference Manual.

void webots_fast2d_robot_set_angular_speed(RobotRef object, double da)

Request to change the angular speed of a robot. The robot parameter is a robot reference. The da parameter indicates the requested angular speed. A robot's angular speed is the speed of its rotation around its center in radians per second.

void webots_fast2d_object_get_transform(ObjectRef object, double *xpos, double *ypos, double *angle)

Reads the current position and angle of an object. The object parameter is a robot or solid reference. The xpos, ypos and angle parameters are the pointers to double values where this function should write the values. These parameters are specified according to the global coordinate system.

7.4.4 Fast2D Plugin Execution Scheme

This section describes the sequence used by Webots for calling the plugin functions. Please refer to the diagram in figure 7.2.

fast2d_execution_scheme

Figure 7.2: Fast2D Plugin Execution Scheme

  1. The plugin is loaded. Go to step 2.

  2. The webots_fast2d_init() function is called. Go to step 3 or 5.

  3. The world model is created. This is achieved through a sequence of calls to the functions webots_fast2d_create_*(), webots_fast2d_set_*() and webots_fast2d_add_*(). Question marks are used to represent a choice among several functions names. Although the exact sequence is unspecified, for each object it is guarantied that: the corresponding webots_fast2d_create_*() function is called first, the corresponding webots_fast2d_set_*() functions are called next and that the corresponding webots_ fast2d_add_*() function is called last. Go to step 4 or 5.

  4. A simulation step is carried out. This is achieved through an unspecified sequence of calls to webots_fast2d_step(), webots_fast2d_set_speed, webots_fast2d_set_ angular_speed() and webots_fast2d_get_transform(). Go to step 4 or 5.

  5. The webots_fast2d_cleanup() function is called. Go to step 2 or 6.

  6. The plugin is unloaded. Go to step 1.

7.4.5 Fast2D Execution Example

This section shows an example of a Webots scene tree and the corresponding Fast2D calls that are carried out when the world is interpreted using Fast2D. Ellipses represent omitted code or parameters. Examine this example carefully. In keeping with what was explained earlier, you will notice that, when transformed from 3D to Fast2D:

Solid {
  translation 0.177532 0.03 0.209856
  rotation 0 1 0 0.785398
  ...
  boundingObject Box {
    size 0.2 0.06 0.2
  }
}

DifferentialWheels {
  translation -0.150197 0 0.01018
  rotation 0 1 0 -4.85101
  children [
    ...
    DistanceSensor {
      translation -0.0245 0.0145 -0.012
      rotation 0 1 0 3.0543
      ...
      lookupTable [
        0 1023 0
        0.05 0 0.01
      ]
      aperture 0.5
    }
  ]
  ...
  boundingObject Transform {
    translation 0 0.011 0
    children [
      Cylinder {
        height 0.022
        radius 0.0285
      }
    ]
  }
...
}

webots_fast2d_init()
webots_fast2d_create_solid()
webots_fast2d_object_set_bounding_polygon(...)
webots_fast2d_object_set_position(..., xpos=0.177532, ypos=0.209856)
webots_fast2d_object_set_angle(..., angle=-0.785398)
webots_fast2d_add_object()

webots_fast2d_create_robot()
webots_fast2d_object_set_bounding_radius(..., radius=0.0285)
webots_fast2d_object_set_position(..., xpos=-0.150197, ypos=0.01018)
webots_fast2d_object_set_angle(..., angle=4.85101)
webots_fast2d_add_object()

webots_create_irsensor(..., xpos=-0.0245, ypos=-0.012, angle=-3.0543,
  range=0.05, aperture=0.5, numRays=1, ...)

Finally, note that the largest input value of the DistanceSensor's lookup table (0.05) becomes the sensor's range in Fast2D.

You will find further information about the DifferentialWheels and DistanceSensor nodes and controller API in the Webots Reference Manual.

release 7.0.2
Copyright © 2012 Cyberbotics Ltd. All right reserved.