Add basic support for additional bounding boxes at different heights

The idea here is to have terrain points which might intersect the
upper body when walking generate additional c-space obstacles based
on a conservative outer approximation of the volume occupied by
the robot while walking
better error handling
more reasonable robot shape