Updated geometry library, added AnyGeometry support to RobotWithGeometry