Balancing QP

Balancing algorithm using QP optimization, with 4-legs and 3-legs.

First Balancing QP, MPC

Balancing algorithms using QP and MPC are first made, but needs some improvement.
It still bouncing.