[Core] New Core Classes

[traversaro]

Preamble

Since when we started writing iDynTree, one of the core ideas was to rely on KDL data structures (such as KDL::Vector, KDL::Twist, KDL::Wrench, KDL::Joint, KDL::Tree, etc, etc, .. ) to increase interoperability with existing software (such as URDF parser) and to rely on an already existing and tested codebase of realtime safe data structures.

However, once we started working on floating base algorithms, we started realizing that some KDL data structures were not specifically designed for the floating base case in mind.
In particular, KDL::Tree and KDL::Segment were designed with a strong assumption on the fact that the Kinematic Tree describing the robot is a directed Tree. This make sense for classical fixed-base robots, where a specific body is fixed to the ground (the so-called "base"). For floating-based robots,however, it sometimes makes sense to have a different base for different types of algorithms. For example, in the original use case of iDynTree described in Fumagalli, Ivaldi et al. [1] the base for the kinematic phase of the RNEA (the "black triangle" following the formalism in [1]) can be different from the base for the dynamic phase of the RNEA (the "black rhombus"). This leads us to develop our own data structures for modeling a Tree, grouped in the KDL::CoDyCo::UndirectedTree (soon to be migrated in iDynTree::UndirectedTree) class.
Similarly, several other data structures were not suited for floating base robots, so we then ended up in the uncomfortable situation where, for basic manipulation of geometric quantities, we are using a mix of KDL data structures and iDynTree ones. This leads (together with a general lack of documentation : ( ) to a situation where the design of the library is quite hard to understand for, with a lot of "lasagna code".

Bindings to other languages

Recently there have been several needs of using iDynTree data structures in several high level languages:

• For developing a basis stepping coordinator in Lua using rFSM, we needed some basic geometric classes. We ended up developing some custom classes in Lua [2] wasting time developing and debugging classes that are in general less robust and less efficient then the C++ one used in iDynTree, even if functionally equivalent.
• For doing data analysis (for example for inertial parameter identification) we always used the classes of iDynTree directly from C++. However for dataset loading, preprocessing and plot/analysis is highly desirable to use a proper high level language, such as Matlab or Python.
• @iron76 developed a prototype in Matlab of the new dynamics estimator that we plan to use on iCub. He did that using the spatialv2 matlab native dynamics library. If he had the possibility of directly using iDynTree function and classes, the transition to C++ would be much simpler.
• @Ommac and @DiegoRomeres are really interested to work with dynamics regressors provided by iDynTree, but for several reason they need to do their offline processing in Matlab.

Considering this, it would make sense to provide bindings to several high level language for iDynTree classes. The language the community is more interested in at the moment are Lua, Python and Matlab. The easiest solution for providing these bindings is to use the SWIG tool [4] that recently has gained experimental Matlab support, thanks to the casadi project [5] [12].

SWIG and KDL use

However, SWIG is designed to wrap a library, but the usage of a lot of KDL types (that do not currently support SWIG) in the core headers could be a problem. Providing SWIG support itself to KDL would be a problem and would probably need some API breakage in KDL basic classes. In particular KDL includes Eigen headers in its own headers, and Eigen is an highly templated library that would probably cause a lot of issues if we try to wrap it with SWIG.
Even if we wanted to do this work to do the necessary changes to provide SWIG support in KDL, the KDL upstream maintainers have demonstrated to be a lot more concerned to preserve backward compatibility, so our patches would probably be rejected.

Providing our own basic classes

Considering the complication provided by using KDL, I think it could make sense to start implementing our own classes for the basic geometric entities such as point, vectors, wrenches, momentum, etc etc as we already did for more high level concept such as the UndirectedTree.
We can still provide KDL compatibility with appropriate conversion function such as toiDynTree and toKDL, for converting from KDL types to iDynTree types. In this way we can (slowly) move away from KDL dependency, while still maintaining all the legacy code.

List of Requirements

Given what we wrote in the previous sections, a possible list of desirable features for the iDynTree basic classes are:

1. They should be easily wrapped by SWIG, in any language.
2. They should be realtime safe, avoiding memory allocation unless in creation of structure with unknown size at compile time.
3. They should be easy to integrate with existing software.
4. They should be designed to avoid misuse, as much as possible.
5. They should be easy to use.

Design guidelines

Considering the features that we want for iDynTree classes, as a consequence we have the following design choices:

1. No external headers should be included in iDynTree classes public headers, perhaps except for basic STL datatypes (std::string ?). This type of encapsulation is usually done with the pimpl idiom, but requirement 2 (no malloc unless strictly necessary) is preventing us from using it, so data storage should be provided in private raw double arrays. Still depending on Eigen as PRIVATE header dependency [7] is ok, so operation on these arrays can be done efficiently in the implementation using the Eigen::Map [6] facilities.
2. As in 1., this means that we should avoid pimpl. For classes whose size is not known at compile time, constructors, destructors and resize methods should be the only non-realtime safe methods. Eigen provides several facilities to make sure to avoid memory allocation while manipulating matrices [8] [9].
3. Conversion function should be provided from and to Eigen, yarp and KDL datatypes.
4. This is the more critical point. The most basic way of ensuring this is at the algebraic level, by providing strictly defined classes and related operations. For example, if you express in your code a force f using the same Class that you use for representing a point vector, and you define the homogeneous transformation as a affine matrix transformation T = (R,p), a possible bug is that a user wants to rotate the force by the rotation provided in the homogeneous transformation, but if he simply computes T*f he will obtain as a result R*f+p, that is a sum of a force and a point. This kind of syntactical checks can be enforced by defining separated data types for point,force,torques,linear velocities, etc etc. However even if you ensure syntactical correctness, we can still do semantical errors in our code. For this reason, I think we should slowly insert also semantical checks in our basic data structures.
5. b another possible misuse is the use of overloaded operators for operations that allocate dynamical memory (that was the error done in yarp::sig::Vector). This operator overloading give to the user the false assumption that something is a "cheap" operation, even if it is an expensive one.

Semantics check

A good overview of the concept of semantical checks in geometric software is provided in
De Laet T, Bellens S, Smits R, Aertbeliën E, Bruyninckx H, and De Schutter J (2013), Geometric Relations between Rigid Bodies: Semantics for Standardization, IEEE Robotics & Automation Magazine, Vol. 20, No. 1, pp. 84-93. [10]
In that paper, the authors propose also a semantic model for the kinematic concepts (coordinates of points, velocities, twist) used in robotics software. Even if that model has some limitations and do not described dynamic quantities, I think it is a valuable starting point.

Unfortunately, their software implementation, described in:

De Laet T, Bellens S, Bruyninckx H, and De Schutter J (2013), Geometric Relations between Rigid Bodies: From Semantics to Software, IEEE Robotics & Automation Magazine, Vol. 20, No. 2, pp. 91-102. [11]

relies completely on templates and is not really user friendly, given that it does not provide the operator overloading for geometric operation that most user expect while manipulating geometric quantities. This is done for avoiding confusion in the use of the various operation, but it reduces the usability of the library. Given that programmers are lazy, and researcher programmers are even more lazy, we should try to incorporate semantics checking in the most not intrusive possible way.

Proof of concept

In pull request #28 I tried to implement the Position class (name taken from the Semantics for Standardization paper) following the requirements that I listed in the previous sections. A working Lua and Matlab interface, provided by SWIG, is also present in the proof of concept. Feedback are more than welcome.
If we are able to provide also a Orientation and Pose classes with some additional operation, we are already able to remove the custom class in Lua and use directly iDynTree classes.

[1] http://people.liralab.it/iron/Papers/journal/IvaldiFumagallietAl.pdf
[2] https://github.com/robotology/codyco-modules/blob/master/src/modules/graspAndStepDemo/lua/gas_funcs.lua
[3] https://github.com/iron76/bnt_time_varying
[4] https://github.com/swig/swig
[5] https://github.com/casadi/casadi/wiki/matlab
[6] http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
[7] http://www.cmake.org/cmake/help/v3.0/command/target_include_directories.html
[8] http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
[9] http://eigen.tuxfamily.org/index.php?title=FAQ#Where_in_my_program_are_temporary_objects_created.3F
[10] http://people.mech.kuleuven.be/~tdelaet/geometric_relations_semantics/geometric_relations_semantics_theory.pdf
[11] http://people.mech.kuleuven.be/~tdelaet/geometric_relations_semantics/geometric_relations_semantics_software.pdf
[12] casadi/casadi#176

[vpadois]

Thanks @traversaro for this detailed overview of the shortcomings of KDL for floating based systems. My knowledge of the implementation of core classes of KDL is too limited for me to have an enlightening technical opinion. My only question is : have you raised this issue to the Orocos/KDL core developers ?

[traversaro]

@vpadois When I started working on this topic I wrote if anyone was interested in this topics on the orocos mailing list, without receiving any response. I did not checked more recently anyway.

[nunoguedelha]

@traversaro I've uploaded a requirements document to my personal repo here: https://github.com/nunoguedelha/DevAnalysis/tree/feature/29/CompleteSemanticChecks/issue%2329_completeSemanticChecks
In the last section I address the 'assert()' handling through compile flags, depending on the build type, the bindings and the semantic checks activations. Could you please take a look and confirm this is aligned with our last discussion on the topic? Thanks

[nunoguedelha]

The idea would be to activate the semantic checks directly at the codyco-superbuild level and propagate it to iDynTree. Of course it can still be modified directly in iDynTree.

[traversaro]

@nunoguedelha Yes, it fits what we discussed.

[traversaro]

The KDL legacy classes have not been used for a long time, and have been removed in #697 . I think we can finally close this issue.