Constrained Whole Body Motion Planning in Task Configuration and Time