complete implementation of humanoids planner with and w/o symbolics!