Towards Robust Planning for High-DoF Robots in Human Environments: The Role of Optimization