Fix waist, add debug + rational for message handling.
 - waist type is now MatrixHomogeneous and not ml::Vector anymore.
 - add traces
 - messages are better handled based on timestamped and seq id.
trajectory.cpp is not a plugin.