Collaborative motion planning of humanoid robots