Path Optimization for Nonholonomic Systems: Application to Reactive Obstacle Avoidance and Path Planning