heron ocp works. now i need to figure out how enforce joint efforts. then it needs to be made into mpc (still simulation). then we need replace the mobile_base_model. then we need to define the cart and run with that
