[model_io] Load sensors information from URDF

[traversaro]

We currently have a SensorsList data structure that is meant to represent the information about the sensors available on the kinematic tree, and provide some helpers function (for example for translating a measurement from the sensors frame to the link frame).

C++ sensors to add

This was added in #24, and for now the only implemented sensors are the six axis F/T sensors.

For implementing BERDY, we need to add several sensors, starting from:

• Accelerometer,
• Gyroscope,
• Orientation sensor (not actually a sensor, but normally an output of a lower level estimate coming from an IMU/MPU).

In the future we may need also joint torque sensors, camera, skin.. but I guess that we can start from this three ones.

Sensor model loading

Once we define a C++ class for representing this sensors, we need a way of loading this information from file. For model input/output, currently we are mostly relying on the URDF format. [1] [2] [3] The URDF spec has several problems (not clearly defined, do not provide explicit support for extensions, mix the concept of frames and links, does not support closed loop structures) but its use is fairly widespread, so for dissemination is a force choice.

Current state of Sensors spec in URDF

Currently the "official" spec for Sensors in URDF [4] supports only laser sensors and camera sensors. A new proposal is open for adding new sensors to the URDF spec [5], but there is clearly a lack of interest in the community. In the meanwhile, several software such as Phobos [6] and Drake [7] are adding mutually incompatible sensors information.

Then, my proposal is:

ref: https://xkcd.com/927/

Jokes aside, the quickest solution for now is that we defined our own sensors URDF extension, and we can take part (but without waiting for the conclusion of the discussion) to ros/urdfdom#28 .

Roadmap

An outline of possible steps are:

• We prepare a draft of URDF extensions that covers our use cases
• We extend the xsd spec in https://github.com/ros/urdfdom/blob/master/xsd/urdf.xsd
• We write a parser for our modified xsd, either extending urdfdom (not suggested, it is a terrible codebase), writing our own (probably the sanest solution) or by experimenting with some fancy tool for xsd --> C++ dom conversion ( @francesco-romano can help in this)
• We write a function that takes a C++ dom for our URDF sensor extension and output a SensorsList

[1] http://wiki.ros.org/urdf
[2] https://github.com/ros/urdfdom_headers
[3] https://github.com/ros/urdfdom
[4] http://drake002.csail.mit.edu/drake/urdf/drakeURDF.html
[5] ros/urdfdom#28
[6] dfki-ric/phobos#11
[7] http://drake002.csail.mit.edu/drake/urdf/drakeURDF.html

[traversaro]

cc @naveenoid

[traversaro]

A useful note for modeling of 6 Axis F/T sensors, Gyroscope and Accelerometers is available in : http://wiki.codyco.eu.dev/doku.php?id=outline_of_state_derivatives_of_rigid_body_dynamics#measurement_equations_and_their_derivatives .

[traversaro]

Regarding the loading of sensors informations from URDF, I recently exposed (in 9f221c7) the function for creating a SensorsList from a URDF (for now supporting only F/T sensors expressed with the gazebo extensions to URDF) from the bindings, for use in preliminary BERDY integration.

The function added is available in https://github.com/robotology/idyntree/blob/master/model_io/urdf/include/iDynTree/ModelIO/URDFSensorsImport.h and an example on how to use it is available in https://github.com/robotology/idyntree/tree/master/examples/matlab/SensorsListParsing .

@claudia-lat We can use this tutorial as a starting point (when the spatialV2 forward kinematics is ready) to compute the transform between the spatialV2 frames and the sensor frames.

@naveenoid It would be nice to extend the tutorial to other sensors as soon as we implement them.

[arocchi]

Great work. Let's see how this proceeds and let's keep in touch with the community ;)

[naveenoid]

Update : 815fce9 presents first successful tests. Run the test on UnitTestURDFGenericSensorImport to see how sensors can be added a read from URDF.

[traversaro]

cc @fjandrad