Continuing to integrate basic_planner
Basic torque rate planner ported to ros
z-alpha planner setup but not completed