Add replanning.
Resolve barrett env obstacles conflict.