Added failure condition for missing collision geometry description
Added functions for managing collision objects in the planning scene