Add an accessor to non const collision geometry in CollisionObject class.

include/fcl/collision_object.h

@@ -310,6 +310,12 @@ public:
     return cgeom_const;
   }
 
+  /// @brief get geometry from the object instance
+  const boost::shared_ptr<CollisionGeometry>& collisionGeometry()
+  {
+    return cgeom;
+  }
+
   /// @brief get object's cost density
   FCL_REAL getCostDensity() const
   {