LowLevelPlanner

New foot z trajectory
other fixes