Add C++ functions to FootTrajectoryGeneration for more efficient frame conversion + Control from yaml + Again a few changes for RPY to quaternion conversion