Optimization-based whole-body motion planning for humanoid robots