Continue work on C++ control + Rework joystick to full c++ + Tweak joystick values + Automatic switch to 4 stance + Position control by joystick

Add possibility of NLE feedback in MPC + Add nice(-20) priority + Add lots of debug output in QP WBC

Rename h_v_bis into h_v_windowed + Remove h_v_windowed_filt + Adjust which data is sent to planner and mpc

Switch updateState function to a C++ version included in Estimator class, small difference in result due to some sort of numerical error

Finish switching to full C++ for the whole-body control part + Error is no longer an attribute of the wbc class

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