Calibrator

The Skin Calibrator

Often times the sensor data need to be coupled with sensor positions and orientations to be of use. The task of the calibrator bundled with Skinware is to provide just this data. The task of finding the actual positions and orientations is a topic of research and is beyond what Skinware does. The calibrator thus reads this information from a file and provides a calibration service. It also provides a small library that can be used to conveniently retrieve these data from the service.

The calibration file is a database that maps all known sensors to their position and orientation relative to the link they are placed on. The currently attached sensors can then be looked up in this database. The format of the calibration file is as follows.

N
(N lines) type uid  p_x p_y p_z  o_x o_y o_z  r  l

In the above format, N is the number of sensors in the database. Each following line describes one sensor. The sensor is identified by its (type, uid) pair, which constitutes the sensor type and its unique id among sensors of that type. For each sensor, the following information is stored:

• (p_x, p_y, p_z) shows the position of the sensor,
• (o_x, o_y, o_z) shows the orientation of the sensor,
• r shows the radius of the sensor,
• l shows the robot link the sensor is placed on.

The position, orientation and radius values are stored in nanometers, and the link is an identifier meaningful to the user of the skin. The link value is useful for calculating the global position and orientation of the sensor given the current robot pose.

Skinware's calibration service comes with a header file that defines the structure holding the calibration data:

struct skin_sensor_calibration_info
{
    int64_t  position_nm[3];    /* 3d position of the sensor (local to robot link reference frame in nanometers) */
    int64_t  orientation_nm[3]; /* 3d orientation of the sensor (local to robot link reference frame in nanometers) */
    uint64_t radius_nm;         /* radius of sensor in nanometers */
    uint32_t robot_link;        /* the robot link this sensor is located on */
    bool     calibrated;        /* whether the values here are valid, i.e. the sensor is calibrated */
};

The calibration service memory contains the above information paired with sensor types and unique ids for each sensor. A small library is provided to extract these information. The following shows how this library can be used.

First, bear in mind that each object in Skinware can be extended with further data through its user_data field. Using initialization and cleanup hooks, one can allocate memory for this field. For example:

struct sensor_extra_data
{
    struct skin_sensor_calibration_info calib_info;
    struct some_other_data other_data;
};

void sensor_init_hook(struct skin_sensor *s, void *user_data)
{
    s->user_data = urt_mem_new(sizeof(struct sensor_extra_data));
    *(struct sensor_extra_data *)s->user_data = (struct sensor_extra_data){0};
}

void sensor_clean_hook(struct skin_sensor *s, void *user_data)
{
    urt_mem_delete(s->user_data);
}

/* after `skin_init()` */
skin_set_sensor_init_hook(skin, sensor_init_hook);
skin_set_sensor_clean_hook(skin, sensor_clean_hook);

With the above code, every time a new skin_sensor object is created (e.g. through skin_update()), the init hook is called and every time it is destroyed, the clean hook is called. This means that each sensor object now has a struct sensor_extra_data attached to it.

The following shows how the library that comes with the calibrator can be used to fill the above memory:

struct skin_sensor_calibration_info *get_calib_info(struct skin_sensor *s, void *user_data)
{
    struct sensor_extra_data *extra_data = (struct sensor_extra_data *)s->user_data;
    return &extra_data->calib_info;
}

void calibrate(struct skin_reader *reader, void *mem, size_t size, void *user_data)
{
    skin_calibrate(skin, mem, get_calib_info);
}

/* after `skin_init()`: try to connect to calibrator */
calibrator_service = skin_service_attach(skin,
                                         &(struct skin_reader_attr){ .name = "CAL" },
                                         &(urt_task_attr){0},
                                         &(struct skin_reader_callbacks){ .read = calibrate });
if (calibrator_service == NULL)
    urt_err("error: calibrator service not running\n");

.....

/* later in a URT thread, call the service once after each update to skin, e.g., through `skin_update()` */
skin_reader_request(calibrator_service);

The above code attaches to the calibrator service with a sporadic reader. This means that the reader's read callback is called only upon request. Inside a URT thread, skin_reader_request() is used to request a read from this service. In the read callback, the library function skin_calibrate() is called, given the service memory as well as a function that returns the pointer to the struct skin_sensor_calibration_info associated with each sensor (get_calib_info()). Using the previous init and clean hooks, the get_calib_info() function returns the portion of the memory previously allocated for this purpose. The skin_calibrate() function iterates through all sensors of the skin and calibrates them accordingly, if information for the sensor is available in the database.

It is understandable that calibrating the skin is not an easy task, and during testing and debugging at least, this information is not necessary. That's why the calibrator program takes an argument that tells it to fake the calibration data. With this argument, the service memory doesn't actually hold a database and the call to skin_calibrate() would place the sensors in some grid. To run the calibrator with this argument, do:

$ skin_calibrate fake_fill

As a final note, Skinware processes are loosely coupled. Since this service doesn't depend on any other services to operate, it could be run given the database (or fake_fill) early on and left running forever, regardless of what drivers, users and other services come and go.

---

Next: See more tutorials.