diff --git a/CHANGELOG.md b/CHANGELOG.md
index c2cb1c017129c..69b10c1bdc316 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,5 +1,17 @@
 ## DART 6
 
+### DART 6.1.0 (2016-XX-XX)
+
+* Misc improvements and bug fixes
+
+  * Added `virtual Shape::getType()` and deprecated `ShapeType Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724)
+
+### DART 6.0.1 (2016-XX-XX)
+
+* Collision detection
+
+  * Added warnings for unsupported shape pairs of DARTCollisionDetector: [#722](https://github.com/dartsim/dart/pull/722)
+
 ### DART 6.0.0 (2016-05-10)
 
 * Common data structures
@@ -9,7 +21,7 @@
   * Added `ShapeFrame` and `ShapeNode`: [#608](https://github.com/dartsim/dart/pull/608)
   * Added `BoundingBox`: [#547](https://github.com/dartsim/dart/pull/547), [#546](https://github.com/dartsim/dart/issues/546)
 
-* Kinematics features:
+* Kinematics
 
   * Added convenient functions for setting joint limits: [#703](https://github.com/dartsim/dart/pull/703)
   * Added more description on `InverseKinematics::solve()`: [#624](https://github.com/dartsim/dart/pull/624)
@@ -24,7 +36,7 @@
   * Fixed `InverseKinematics` when it's used with `FreeJoint` and `BallJoint`: [#683](https://github.com/dartsim/dart/pull/683)
   * Fixed ambiguous overload on `MetaSkeleton::getLinearJacobianDeriv`: [#628](https://github.com/dartsim/dart/pull/628), [#626](https://github.com/dartsim/dart/issues/626)
 
-* Dynamics features:
+* Dynamics
 
   * Added `get/setLCPSolver` functions to `ConstraintSolver`: [#633](https://github.com/dartsim/dart/pull/633)
   * Added `ServoMotorConstraint` as a preliminary implementation for `SERVO` actuator type: [#566](https://github.com/dartsim/dart/pull/566)
@@ -34,7 +46,7 @@
   * Fixed incorrect applying of joint constraint impulses: [#317](https://github.com/dartsim/dart/issues/317)
   * Deprecated `draw()` functions of dynamics classes: [#654](https://github.com/dartsim/dart/pull/654)
 
-* Collision detection features
+* Collision detection
 
   * Added `CollisionGroup` and refactored `CollisionDetector` to be more versatile: [#711](https://github.com/dartsim/dart/pull/711), [#704](https://github.com/dartsim/dart/pull/704), [#689](https://github.com/dartsim/dart/pull/689), [#631](https://github.com/dartsim/dart/pull/631), [#642](https://github.com/dartsim/dart/issues/642), [#20](https://github.com/dartsim/dart/issues/20)
   * Improved API for self collision checking options: [#718](https://github.com/dartsim/dart/pull/718), [#702](https://github.com/dartsim/dart/issues/702)
@@ -51,7 +63,7 @@
   * Moved `osgDart` under `dart::gui` namespace as `dart::gui::osg`: [#651](https://github.com/dartsim/dart/pull/651)
   * Fixed GlutWindow::screenshot(): [#623](https://github.com/dartsim/dart/pull/623), [#395](https://github.com/dartsim/dart/issues/395)
 
-* Simulation features
+* Simulation
 
   * Fixed `World::clone()` didn't clone the collision detector: [#658](https://github.com/dartsim/dart/pull/658)
   * Fixed bug of `World` concurrency: [#577](https://github.com/dartsim/dart/pull/577), [#576](https://github.com/dartsim/dart/issues/576)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index bf7265d1d7209..23a8c23f6ef48 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -44,7 +44,7 @@ endif()
 project(dart)
 
 set(DART_MAJOR_VERSION "6")
-set(DART_MINOR_VERSION "0")
+set(DART_MINOR_VERSION "1")
 set(DART_PATCH_VERSION "0")
 set(DART_VERSION "${DART_MAJOR_VERSION}.${DART_MINOR_VERSION}.${DART_PATCH_VERSION}")
 set(DART_PKG_DESC "Dynamic Animation and Robotics Toolkit.")
diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp
index f43692b81d06c..d6a8e8f337a15 100644
--- a/dart/collision/bullet/BulletCollisionDetector.cpp
+++ b/dart/collision/bullet/BulletCollisionDetector.cpp
@@ -403,100 +403,85 @@ btCollisionShape* BulletCollisionDetector::createBulletCollisionShape(
   using dynamics::MeshShape;
   using dynamics::SoftMeshShape;
 
+  const auto& shapeType = shape->getType();
   btCollisionShape* bulletCollisionShape = nullptr;
 
-  switch (shape->getShapeType())
+  if (BoxShape::getStaticType() == shapeType)
   {
-    case Shape::BOX:
-    {
-      assert(dynamic_cast<const BoxShape*>(shape.get()));
-
-      const auto box = static_cast<const BoxShape*>(shape.get());
-      const Eigen::Vector3d& size = box->getSize();
-
-      bulletCollisionShape = new btBoxShape(convertVector3(size*0.5));
+    assert(dynamic_cast<const BoxShape*>(shape.get()));
 
-      break;
-    }
-    case Shape::ELLIPSOID:
-    {
-      assert(dynamic_cast<const EllipsoidShape*>(shape.get()));
+    const auto box = static_cast<const BoxShape*>(shape.get());
+    const Eigen::Vector3d& size = box->getSize();
 
-      const auto ellipsoid = static_cast<const EllipsoidShape*>(shape.get());
-      const Eigen::Vector3d& size = ellipsoid->getSize();
+    bulletCollisionShape = new btBoxShape(convertVector3(size*0.5));
+  }
+  else if (EllipsoidShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const EllipsoidShape*>(shape.get()));
 
-      if (ellipsoid->isSphere())
-      {
-        bulletCollisionShape = new btSphereShape(size[0] * 0.5);
-      }
-      else
-      {
-        bulletCollisionShape = createBulletEllipsoidMesh(
-              size[0], size[1], size[2]);
-      }
+    const auto ellipsoid = static_cast<const EllipsoidShape*>(shape.get());
+    const Eigen::Vector3d& size = ellipsoid->getSize();
 
-      break;
-    }
-    case Shape::CYLINDER:
+    if (ellipsoid->isSphere())
     {
-      assert(dynamic_cast<const CylinderShape*>(shape.get()));
-
-      const auto cylinder = static_cast<const CylinderShape*>(shape.get());
-      const auto radius = cylinder->getRadius();
-      const auto height = cylinder->getHeight();
-      const auto size = btVector3(radius, radius, height * 0.5);
-
-      bulletCollisionShape = new btCylinderShapeZ(size);
-
-      break;
+      bulletCollisionShape = new btSphereShape(size[0] * 0.5);
     }
-    case Shape::PLANE:
+    else
     {
-      assert(dynamic_cast<const PlaneShape*>(shape.get()));
-
-      const auto plane = static_cast<const PlaneShape*>(shape.get());
-      const Eigen::Vector3d normal = plane->getNormal();
-      const double offset = plane->getOffset();
-
-      bulletCollisionShape = new btStaticPlaneShape(
-            convertVector3(normal), offset);
-
-      break;
+      bulletCollisionShape = createBulletEllipsoidMesh(
+            size[0], size[1], size[2]);
     }
-    case Shape::MESH:
-    {
-      assert(dynamic_cast<const MeshShape*>(shape.get()));
+  }
+  else if (CylinderShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const CylinderShape*>(shape.get()));
 
-      const auto shapeMesh = static_cast<const MeshShape*>(shape.get());
-      const auto scale = shapeMesh->getScale();
-      const auto mesh = shapeMesh->getMesh();
+    const auto cylinder = static_cast<const CylinderShape*>(shape.get());
+    const auto radius = cylinder->getRadius();
+    const auto height = cylinder->getHeight();
+    const auto size = btVector3(radius, radius, height * 0.5);
 
-      bulletCollisionShape = createBulletCollisionShapeFromAssimpScene(
-            scale, mesh);
+    bulletCollisionShape = new btCylinderShapeZ(size);
+  }
+  else if (PlaneShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const PlaneShape*>(shape.get()));
 
-      break;
-    }
-    case Shape::SOFT_MESH:
-    {
-      assert(dynamic_cast<const SoftMeshShape*>(shape.get()));
+    const auto plane = static_cast<const PlaneShape*>(shape.get());
+    const Eigen::Vector3d normal = plane->getNormal();
+    const double offset = plane->getOffset();
+
+    bulletCollisionShape = new btStaticPlaneShape(
+          convertVector3(normal), offset);
+  }
+  else if (MeshShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const MeshShape*>(shape.get()));
 
-      const auto softMeshShape = static_cast<const SoftMeshShape*>(shape.get());
-      const auto mesh = softMeshShape->getAssimpMesh();
+    const auto shapeMesh = static_cast<const MeshShape*>(shape.get());
+    const auto scale = shapeMesh->getScale();
+    const auto mesh = shapeMesh->getMesh();
 
-      bulletCollisionShape = createBulletCollisionShapeFromAssimpMesh(mesh);
+    bulletCollisionShape = createBulletCollisionShapeFromAssimpScene(
+          scale, mesh);
+  }
+  else if (SoftMeshShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const SoftMeshShape*>(shape.get()));
 
-      break;
-    }
-    default:
-    {
-      dterr << "[BulletCollisionObjectData::init] "
-            << "Attempting to create unsupported shape type '"
-            << shape->getShapeType() << "'.\n";
+    const auto softMeshShape = static_cast<const SoftMeshShape*>(shape.get());
+    const auto mesh = softMeshShape->getAssimpMesh();
 
-      bulletCollisionShape = new btSphereShape(0.1);
+    bulletCollisionShape = createBulletCollisionShapeFromAssimpMesh(mesh);
+  }
+  else
+  {
+    dterr << "[BulletCollisionDetector::createBulletCollisionShape] "
+          << "Attempting to create an unsupported shape type ["
+          << shapeType << "] Creating a sphere with 0.1 radius "
+          << "instead.\n";
 
-      break;
-    }
+    bulletCollisionShape = new btSphereShape(0.1);
   }
 
   return bulletCollisionShape;
diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp
index 4771287fb6aaa..49f7e5682919a 100644
--- a/dart/collision/dart/DARTCollide.cpp
+++ b/dart/collision/dart/DARTCollide.cpp
@@ -39,11 +39,11 @@
 
 #include <memory>
 
+#include "dart/math/Helpers.hpp"
 #include "dart/dynamics/BoxShape.hpp"
 #include "dart/dynamics/EllipsoidShape.hpp"
 #include "dart/dynamics/CylinderShape.hpp"
 #include "dart/dynamics/BodyNode.hpp"
-#include "dart/math/Helpers.hpp"
 
 namespace dart {
 namespace collision {
@@ -1258,83 +1258,71 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result)
   // TODO(JS): We could make the contact point computation as optional for
   // the case that we want only binary check.
 
-  const dynamics::ConstShapePtr& shape0 = o1->getShape();
-  const dynamics::ConstShapePtr& shape1 = o2->getShape();
-  const Eigen::Isometry3d& T0 = o1->getTransform();
-  const Eigen::Isometry3d& T1 = o2->getTransform();
+  const auto& shape1 = o1->getShape();
+  const auto& shape2 = o2->getShape();
 
-  dynamics::Shape::ShapeType LeftType = shape0->getShapeType();
-  dynamics::Shape::ShapeType RightType = shape1->getShapeType();
+  const auto& shapeType1 = shape1->getType();
+  const auto& shapeType2 = shape2->getType();
 
-  switch(LeftType)
-  {
-    case dynamics::Shape::BOX:
-    {
-      const dynamics::BoxShape* box0 = static_cast<const dynamics::BoxShape*>(shape0.get());
+  const Eigen::Isometry3d& T1 = o1->getTransform();
+  const Eigen::Isometry3d& T2 = o2->getTransform();
 
-      switch(RightType)
-      {
-        case dynamics::Shape::BOX:
-        {
-          const dynamics::BoxShape* box1 = static_cast<const dynamics::BoxShape*>(shape1.get());
-          return collideBoxBox(o1, o2,
-                               box0->getSize(), T0,
-                               box1->getSize(), T1, result);
-        }
-        case dynamics::Shape::ELLIPSOID:
-        {
-          const dynamics::EllipsoidShape* ellipsoid1 = static_cast<const dynamics::EllipsoidShape*>(shape1.get());
-          return collideBoxSphere(o1, o2,
-                                  box0->getSize(), T0,
-                                  ellipsoid1->getSize()[0] * 0.5, T1,
-              result);
-        }
-        default:
-        {
-          return false;
+  if (dynamics::BoxShape::getStaticType() == shapeType1)
+  {
+    const auto* box0 = static_cast<const dynamics::BoxShape*>(shape1.get());
 
-          break;
-        }
-      }
+    if (dynamics::BoxShape::getStaticType() == shapeType2)
+    {
+      const auto* box1
+          = static_cast<const dynamics::BoxShape*>(shape2.get());
 
-      break;
+      return collideBoxBox(o1, o2,
+                           box0->getSize(), T1,
+                           box1->getSize(), T2,
+                           result);
     }
-    case dynamics::Shape::ELLIPSOID:
+    else if (dynamics::EllipsoidShape::getStaticType() == shapeType2)
     {
-      const dynamics::EllipsoidShape* ellipsoid0 = static_cast<const dynamics::EllipsoidShape*>(shape0.get());
+      const auto* ellipsoid1
+          = static_cast<const dynamics::EllipsoidShape*>(shape2.get());
 
-      switch(RightType)
-      {
-        case dynamics::Shape::BOX:
-        {
-          const dynamics::BoxShape* box1 = static_cast<const dynamics::BoxShape*>(shape1.get());
-          return collideSphereBox(o1, o2,
-                                  ellipsoid0->getSize()[0] * 0.5, T0,
-                                  box1->getSize(), T1,
-                                  result);
-        }
-        case dynamics::Shape::ELLIPSOID:
-        {
-          const dynamics::EllipsoidShape* ellipsoid1 = static_cast<const dynamics::EllipsoidShape*>(shape1.get());
-          return collideSphereSphere(o1, o2,
-                                     ellipsoid0->getSize()[0] * 0.5, T0,
-                                     ellipsoid1->getSize()[0] * 0.5, T1,
-                                     result);
-        }
-        default:
-          return false;
+      return collideBoxSphere(o1, o2,
+                              box0->getSize(), T1,
+                              ellipsoid1->getSize()[0] * 0.5, T2,
+                              result);
+    }
+  }
+  else if (dynamics::EllipsoidShape::getStaticType() == shapeType1)
+  {
+    const auto* ellipsoid0
+        = static_cast<const dynamics::EllipsoidShape*>(shape1.get());
 
-          break;
-      }
+    if (dynamics::BoxShape::getStaticType() == shapeType2)
+    {
+      const auto* box1
+          = static_cast<const dynamics::BoxShape*>(shape2.get());
 
-      break;
+      return collideSphereBox(o1, o2,
+                              ellipsoid0->getSize()[0] * 0.5, T1,
+                              box1->getSize(), T2,
+                              result);
     }
-    default:
-      return false;
+    else if (dynamics::EllipsoidShape::getStaticType() == shapeType2)
+    {
+      const auto* ellipsoid1
+          = static_cast<const dynamics::EllipsoidShape*>(shape2.get());
 
-      break;
+      return collideSphereSphere(o1, o2,
+                                 ellipsoid0->getSize()[0] * 0.5, T1,
+                                 ellipsoid1->getSize()[0] * 0.5, T2,
+                                 result);
+    }
   }
 
+  dterr << "[DARTCollisionDetector] Attempting to check for an "
+        << "unsupported shape pair: [" << shape1->getType()
+        << "] - [" << shape2->getType() << "]. Returning false.\n";
+
   return false;
 }
 
diff --git a/dart/collision/dart/DARTCollide.hpp b/dart/collision/dart/DARTCollide.hpp
index 7633c0bc1e75c..37d26358e71c0 100644
--- a/dart/collision/dart/DARTCollide.hpp
+++ b/dart/collision/dart/DARTCollide.hpp
@@ -38,17 +38,8 @@
 #define DART_COLLISION_DART_DARTCOLLIDE_HPP_
 
 #include <vector>
-
 #include <Eigen/Dense>
-
 #include "dart/collision/CollisionDetector.hpp"
-#include "dart/dynamics/Shape.hpp"
-
-namespace dart {
-namespace dynamics {
-class Shape;
-}  // namespace dynamics
-}  // namespace dart
 
 namespace dart {
 namespace collision {
diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp
index aa743c263f156..124ae702f9819 100644
--- a/dart/collision/dart/DARTCollisionDetector.cpp
+++ b/dart/collision/dart/DARTCollisionDetector.cpp
@@ -42,6 +42,7 @@
 #include "dart/collision/dart/DARTCollisionObject.hpp"
 #include "dart/collision/dart/DARTCollisionGroup.hpp"
 #include "dart/dynamics/ShapeFrame.hpp"
+#include "dart/dynamics/BoxShape.hpp"
 #include "dart/dynamics/EllipsoidShape.hpp"
 
 namespace dart {
@@ -241,20 +242,22 @@ void warnUnsupportedShapeType(const dynamics::ShapeFrame* shapeFrame)
     return;
 
   const auto& shape = shapeFrame->getShape();
+  const auto& shapeType = shape->getType();
 
-  if (shape->getShapeType() == dynamics::Shape::BOX)
+  if (shapeType == dynamics::BoxShape::getStaticType())
     return;
 
-  if (shape->getShapeType() == dynamics::Shape::ELLIPSOID)
+  if (shapeType == dynamics::EllipsoidShape::getStaticType())
   {
     const auto& ellipsoid
         = std::static_pointer_cast<const dynamics::EllipsoidShape>(shape);
+
     if (ellipsoid->isSphere())
       return;
   }
 
-  dterr << "[DARTCollisionDetector] Attempting to create shape type '"
-        << shapeFrame->getShape()->getShapeType() << "' that is not supported "
+  dterr << "[DARTCollisionDetector] Attempting to create shape type ["
+        << shapeType << "] that is not supported "
         << "by DARTCollisionDetector. Currently, only BoxShape and "
         << "EllipsoidShape (only when all the radii are equal) are "
         << "supported. This shape will always get penetrated by other "
diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp
index 9a38b44f8130a..ba49ecd8fad74 100644
--- a/dart/collision/fcl/FCLCollisionDetector.cpp
+++ b/dart/collision/fcl/FCLCollisionDetector.cpp
@@ -826,132 +826,117 @@ FCLCollisionDetector::createFCLCollisionGeometry(
   using dynamics::SoftMeshShape;
 
   fcl::CollisionGeometry* geom = nullptr;
+  const auto& shapeType = shape->getType();
 
-  switch (shape->getShapeType())
+  if (BoxShape::getStaticType() == shapeType)
   {
-    case Shape::BOX:
-    {
-      assert(dynamic_cast<const BoxShape*>(shape.get()));
-
-      auto box = static_cast<const BoxShape*>(shape.get());
-      const Eigen::Vector3d& size = box->getSize();
+    assert(dynamic_cast<const BoxShape*>(shape.get()));
 
-      if (FCLCollisionDetector::PRIMITIVE == type)
-        geom = new fcl::Box(size[0], size[1], size[2]);
-      else
-        geom = createCube<fcl::OBBRSS>(size[0], size[1], size[2]);
+    auto box = static_cast<const BoxShape*>(shape.get());
+    const Eigen::Vector3d& size = box->getSize();
 
-      break;
-    }
-    case Shape::ELLIPSOID:
-    {
-      assert(dynamic_cast<const EllipsoidShape*>(shape.get()));
+    if (FCLCollisionDetector::PRIMITIVE == type)
+      geom = new fcl::Box(size[0], size[1], size[2]);
+    else
+      geom = createCube<fcl::OBBRSS>(size[0], size[1], size[2]);
+  }
+  else if (EllipsoidShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const EllipsoidShape*>(shape.get()));
 
-      auto ellipsoid = static_cast<const EllipsoidShape*>(shape.get());
-      const Eigen::Vector3d& size = ellipsoid->getSize();
+    auto ellipsoid = static_cast<const EllipsoidShape*>(shape.get());
+    const Eigen::Vector3d& size = ellipsoid->getSize();
 
-      if (FCLCollisionDetector::PRIMITIVE == type)
+    if (FCLCollisionDetector::PRIMITIVE == type)
+    {
+      if (ellipsoid->isSphere())
       {
-        if (ellipsoid->isSphere())
-        {
-          geom = new fcl::Sphere(size[0] * 0.5);
-        }
-        else
-        {
-#if FCL_VERSION_AT_LEAST(0,4,0)
-          geom = new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5));
-#else
-          geom = createEllipsoid<fcl::OBBRSS>(size[0], size[1], size[2]);
-#endif
-        }
+        geom = new fcl::Sphere(size[0] * 0.5);
       }
       else
       {
+#if FCL_VERSION_AT_LEAST(0,4,0)
+        geom = new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5));
+#else
         geom = createEllipsoid<fcl::OBBRSS>(size[0], size[1], size[2]);
+#endif
       }
-
-      break;
     }
-    case Shape::CYLINDER:
+    else
     {
-      assert(dynamic_cast<const CylinderShape*>(shape.get()));
+      geom = createEllipsoid<fcl::OBBRSS>(size[0], size[1], size[2]);
+    }
+  }
+  else if (CylinderShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const CylinderShape*>(shape.get()));
 
-      const auto cylinder = static_cast<const CylinderShape*>(shape.get());
-      const auto radius = cylinder->getRadius();
-      const auto height = cylinder->getHeight();
+    const auto cylinder = static_cast<const CylinderShape*>(shape.get());
+    const auto radius = cylinder->getRadius();
+    const auto height = cylinder->getHeight();
 
-      if (FCLCollisionDetector::PRIMITIVE == type)
-      {
-        geom = createCylinder<fcl::OBBRSS>(radius, radius, height, 16, 16);
-        // TODO(JS): We still need to use mesh for cylinder because FCL 0.4.0
-        // returns single contact point for cylinder yet. Once FCL support
-        // multiple contact points then above code will be replaced by:
-        // fclCollGeom.reset(new fcl::Cylinder(radius, height));
-      }
-      else
-      {
-        geom = createCylinder<fcl::OBBRSS>(radius, radius, height, 16, 16);
-      }
-
-      break;
+    if (FCLCollisionDetector::PRIMITIVE == type)
+    {
+      geom = createCylinder<fcl::OBBRSS>(radius, radius, height, 16, 16);
+      // TODO(JS): We still need to use mesh for cylinder because FCL 0.4.0
+      // returns single contact point for cylinder yet. Once FCL support
+      // multiple contact points then above code will be replaced by:
+      // fclCollGeom.reset(new fcl::Cylinder(radius, height));
     }
-    case Shape::PLANE:
+    else
     {
-      if (FCLCollisionDetector::PRIMITIVE == type)
-      {
-        assert(dynamic_cast<const PlaneShape*>(shape.get()));
-        auto                  plane = static_cast<const PlaneShape*>(shape.get());
-        const Eigen::Vector3d normal = plane->getNormal();
-        const double          offset = plane->getOffset();
-
-        geom = new fcl::Halfspace(FCLTypes::convertVector3(normal), offset);
-      }
-      else
-      {
-        geom = createCube<fcl::OBBRSS>(1000.0, 0.0, 1000.0);
-        dtwarn << "[FCLCollisionDetector] PlaneShape is not supported by "
-               << "FCLCollisionDetector. We create a thin box mesh insted, where "
-               << "the size is [1000 0 1000].\n";
-      }
-
-      break;
+      geom = createCylinder<fcl::OBBRSS>(radius, radius, height, 16, 16);
     }
-    case Shape::MESH:
+  }
+  else if (PlaneShape::getStaticType() == shapeType)
+  {
+    if (FCLCollisionDetector::PRIMITIVE == type)
     {
-      assert(dynamic_cast<const MeshShape*>(shape.get()));
-
-      auto shapeMesh = static_cast<const MeshShape*>(shape.get());
-      const Eigen::Vector3d& scale = shapeMesh->getScale();
-      auto aiScene = shapeMesh->getMesh();
-
-      geom = createMesh<fcl::OBBRSS>(scale[0], scale[1], scale[2], aiScene);
+      assert(dynamic_cast<const PlaneShape*>(shape.get()));
+      auto                  plane = static_cast<const PlaneShape*>(shape.get());
+      const Eigen::Vector3d normal = plane->getNormal();
+      const double          offset = plane->getOffset();
 
-      break;
+      geom = new fcl::Halfspace(FCLTypes::convertVector3(normal), offset);
     }
-    case Shape::SOFT_MESH:
+    else
     {
-      assert(dynamic_cast<const SoftMeshShape*>(shape.get()));
+      geom = createCube<fcl::OBBRSS>(1000.0, 0.0, 1000.0);
+      dtwarn << "[FCLCollisionDetector] PlaneShape is not supported by "
+             << "FCLCollisionDetector. We create a thin box mesh insted, where "
+             << "the size is [1000 0 1000].\n";
+    }
+  }
+  else if (MeshShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const MeshShape*>(shape.get()));
 
-      auto softMeshShape = static_cast<const SoftMeshShape*>(shape.get());
-      auto aiMesh = softMeshShape->getAssimpMesh();
+    auto shapeMesh = static_cast<const MeshShape*>(shape.get());
+    const Eigen::Vector3d& scale = shapeMesh->getScale();
+    auto aiScene = shapeMesh->getMesh();
 
-      geom = createSoftMesh<fcl::OBBRSS>(aiMesh);
+    geom = createMesh<fcl::OBBRSS>(scale[0], scale[1], scale[2], aiScene);
+  }
+  else if (SoftMeshShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const SoftMeshShape*>(shape.get()));
 
-      break;
-    }
-    default:
-    {
-      dterr << "[FCLCollisionDetector] Attempting to create unsupported shape "
-            << "type '" << shape->getShapeType() << "'.\n";
+    auto softMeshShape = static_cast<const SoftMeshShape*>(shape.get());
+    auto aiMesh = softMeshShape->getAssimpMesh();
 
-      return nullptr;
-    }
+    geom = createSoftMesh<fcl::OBBRSS>(aiMesh);
   }
-
-  if (geom)
-    return boost::shared_ptr<fcl::CollisionGeometry>(geom, deleter);
   else
-    return boost::shared_ptr<fcl::CollisionGeometry>();
+  {
+    dterr << "[FCLCollisionDetector::createFCLCollisionGeometry] "
+          << "Attempting to create an unsupported shape type ["
+          << shapeType << "] Creating a sphere with 0.1 radius "
+          << "instead.\n";
+
+    geom = createEllipsoid<fcl::OBBRSS>(0.1, 0.1, 0.1);
+  }
+
+  return boost::shared_ptr<fcl::CollisionGeometry>(geom, deleter);
 }
 
 //==============================================================================
diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp
index 164aeedb06d63..a5fb1abae075e 100644
--- a/dart/collision/fcl/FCLCollisionObject.cpp
+++ b/dart/collision/fcl/FCLCollisionObject.cpp
@@ -85,42 +85,39 @@ void FCLCollisionObject::updateEngineData()
 
   auto shape = mShapeFrame->getShape().get();
 
-  if (shape->getShapeType() == dynamics::Shape::SOFT_MESH)
+  // Update soft-body's vertices
+  if (shape->getType() == dynamics::SoftMeshShape::getStaticType())
   {
-    // Update soft-body's vertices
-    if (shape->getShapeType() == Shape::SOFT_MESH)
-    {
-      assert(dynamic_cast<const SoftMeshShape*>(shape));
-      auto softMeshShape = static_cast<const SoftMeshShape*>(shape);
+    assert(dynamic_cast<const SoftMeshShape*>(shape));
+    auto softMeshShape = static_cast<const SoftMeshShape*>(shape);
 
-      const aiMesh* mesh = softMeshShape->getAssimpMesh();
-      const_cast<SoftMeshShape*>(softMeshShape)->update();
-      // TODO(JS): update function be called by somewhere out of here.
+    const aiMesh* mesh = softMeshShape->getAssimpMesh();
+    const_cast<SoftMeshShape*>(softMeshShape)->update();
+    // TODO(JS): update function be called by somewhere out of here.
 
 #if FCL_VERSION_AT_LEAST(0,3,0)
-      auto collGeom = const_cast<fcl::CollisionGeometry*>(
+    auto collGeom = const_cast<fcl::CollisionGeometry*>(
           mFCLCollisionObject->collisionGeometry().get());
 #else
-      fcl::CollisionGeometry* collGeom
-          = const_cast<fcl::CollisionGeometry*>(
-              mFCLCollisionObject->getCollisionGeometry());
+    fcl::CollisionGeometry* collGeom
+        = const_cast<fcl::CollisionGeometry*>(
+          mFCLCollisionObject->getCollisionGeometry());
 #endif
-      assert(dynamic_cast<fcl::BVHModel<fcl::OBBRSS>*>(collGeom));
-      auto bvhModel = static_cast<fcl::BVHModel<fcl::OBBRSS>*>(collGeom);
+    assert(dynamic_cast<fcl::BVHModel<fcl::OBBRSS>*>(collGeom));
+    auto bvhModel = static_cast<fcl::BVHModel<fcl::OBBRSS>*>(collGeom);
 
-      bvhModel->beginUpdateModel();
-      for (auto j = 0u; j < mesh->mNumFaces; ++j)
+    bvhModel->beginUpdateModel();
+    for (auto i = 0u; i < mesh->mNumFaces; ++i)
+    {
+      fcl::Vec3f vertices[3];
+      for (auto j = 0u; j < 3; ++j)
       {
-        fcl::Vec3f vertices[3];
-        for (auto k = 0u; k < 3; ++k)
-        {
-          const auto& vertex = mesh->mVertices[mesh->mFaces[j].mIndices[k]];
-          vertices[k] = fcl::Vec3f(vertex.x, vertex.y, vertex.z);
-        }
-        bvhModel->updateTriangle(vertices[0], vertices[1], vertices[2]);
+        const auto& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]];
+        vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z);
       }
-      bvhModel->endUpdateModel();
+      bvhModel->updateTriangle(vertices[0], vertices[1], vertices[2]);
     }
+    bvhModel->endUpdateModel();
   }
 
   mFCLCollisionObject->setTransform(FCLTypes::convertTransform(getTransform()));
diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp
index b2845afe18aa5..0582fa77c9af3 100644
--- a/dart/constraint/SoftContactConstraint.cpp
+++ b/dart/constraint/SoftContactConstraint.cpp
@@ -44,6 +44,7 @@
 #include "dart/dynamics/SoftBodyNode.hpp"
 #include "dart/dynamics/Skeleton.hpp"
 #include "dart/dynamics/Shape.hpp"
+#include "dart/dynamics/SoftMeshShape.hpp"
 #include "dart/collision/CollisionObject.hpp"
 #include "dart/lcpsolver/lcp.h"
 
@@ -104,8 +105,8 @@ SoftContactConstraint::SoftContactConstraint(
   // Select colling point mass based on trimesh ID
   if (mSoftBodyNode1)
   {
-    if (contact.collisionObject1->getShape()->getShapeType()
-        == dynamics::Shape::SOFT_MESH)
+    if (contact.collisionObject1->getShape()->getType()
+        == dynamics::SoftMeshShape::getStaticType())
     {
       mPointMass1 = selectCollidingPointMass(mSoftBodyNode1, contact.point,
                                              contact.triID1);
@@ -114,8 +115,8 @@ SoftContactConstraint::SoftContactConstraint(
   }
   if (mSoftBodyNode2)
   {
-    if (contact.collisionObject2->getShape()->getShapeType()
-        == dynamics::Shape::SOFT_MESH)
+    if (contact.collisionObject2->getShape()->getType()
+        == dynamics::SoftMeshShape::getStaticType())
     {
       mPointMass2 = selectCollidingPointMass(mSoftBodyNode2, contact.point,
                                              contact.triID2);
diff --git a/dart/dynamics/BoxShape.cpp b/dart/dynamics/BoxShape.cpp
index dfe9880594c3a..fabef9ad0e4a0 100644
--- a/dart/dynamics/BoxShape.cpp
+++ b/dart/dynamics/BoxShape.cpp
@@ -55,6 +55,19 @@ BoxShape::BoxShape(const Eigen::Vector3d& _size)
 BoxShape::~BoxShape() {
 }
 
+//==============================================================================
+const std::string& BoxShape::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+const std::string& BoxShape::getStaticType()
+{
+  static const std::string type("BoxShape");
+  return type;
+}
+
 //==============================================================================
 double BoxShape::computeVolume(const Eigen::Vector3d& size)
 {
diff --git a/dart/dynamics/BoxShape.hpp b/dart/dynamics/BoxShape.hpp
index ecbb8018e6e92..5d07ae3dd6534 100644
--- a/dart/dynamics/BoxShape.hpp
+++ b/dart/dynamics/BoxShape.hpp
@@ -51,6 +51,12 @@ class BoxShape : public Shape {
   /// \brief Destructor.
   virtual ~BoxShape();
 
+  // Documentation inherited.
+  const std::string& getType() const;
+
+  /// Returns shape type for this class
+  static const std::string& getStaticType();
+
   /// \brief Set size of this box.
   void setSize(const Eigen::Vector3d& _size);
 
diff --git a/dart/dynamics/CylinderShape.cpp b/dart/dynamics/CylinderShape.cpp
index 076865fa7b084..d203c97063b0b 100644
--- a/dart/dynamics/CylinderShape.cpp
+++ b/dart/dynamics/CylinderShape.cpp
@@ -52,6 +52,19 @@ CylinderShape::CylinderShape(double _radius, double _height)
   updateVolume();
 }
 
+//==============================================================================
+const std::string& CylinderShape::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+const std::string& CylinderShape::getStaticType()
+{
+  static const std::string type("CylinderShape");
+  return type;
+}
+
 double CylinderShape::getRadius() const {
   return mRadius;
 }
diff --git a/dart/dynamics/CylinderShape.hpp b/dart/dynamics/CylinderShape.hpp
index ffd5b3a037565..ea2c8d07b134a 100644
--- a/dart/dynamics/CylinderShape.hpp
+++ b/dart/dynamics/CylinderShape.hpp
@@ -50,6 +50,12 @@ class CylinderShape : public Shape {
   /// \brief Constructor.
   CylinderShape(double _radius, double _height);
 
+  // Documentation inherited.
+  const std::string& getType() const;
+
+  /// Returns shape type for this class
+  static const std::string& getStaticType();
+
   /// \brief
   double getRadius() const;
 
diff --git a/dart/dynamics/EllipsoidShape.cpp b/dart/dynamics/EllipsoidShape.cpp
index 227dbcab045fc..a9e05ac82ffe7 100644
--- a/dart/dynamics/EllipsoidShape.cpp
+++ b/dart/dynamics/EllipsoidShape.cpp
@@ -50,6 +50,19 @@ EllipsoidShape::EllipsoidShape(const Eigen::Vector3d& _size)
 EllipsoidShape::~EllipsoidShape() {
 }
 
+//==============================================================================
+const std::string& EllipsoidShape::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+const std::string& EllipsoidShape::getStaticType()
+{
+  static const std::string type("EllipsoidShape");
+  return type;
+}
+
 void EllipsoidShape::setSize(const Eigen::Vector3d& _size) {
   assert(_size[0] > 0.0);
   assert(_size[1] > 0.0);
diff --git a/dart/dynamics/EllipsoidShape.hpp b/dart/dynamics/EllipsoidShape.hpp
index 141939a27cd5b..931f67904d05e 100644
--- a/dart/dynamics/EllipsoidShape.hpp
+++ b/dart/dynamics/EllipsoidShape.hpp
@@ -51,6 +51,12 @@ class EllipsoidShape : public Shape {
   /// \brief Destructor.
   virtual ~EllipsoidShape();
 
+  // Documentation inherited.
+  const std::string& getType() const;
+
+  /// Returns shape type for this class
+  static const std::string& getStaticType();
+
   /// \brief Set size of this box.
   void setSize(const Eigen::Vector3d& _size);
 
diff --git a/dart/dynamics/LineSegmentShape.cpp b/dart/dynamics/LineSegmentShape.cpp
index 751401fa62920..383c3f0b268af 100644
--- a/dart/dynamics/LineSegmentShape.cpp
+++ b/dart/dynamics/LineSegmentShape.cpp
@@ -80,6 +80,19 @@ LineSegmentShape::LineSegmentShape(const Eigen::Vector3d& _v1,
   mVariance = DYNAMIC_VERTICES;
 }
 
+//==============================================================================
+const std::string& LineSegmentShape::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+const std::string& LineSegmentShape::getStaticType()
+{
+  static const std::string type("LineSegmentShape");
+  return type;
+}
+
 //==============================================================================
 void LineSegmentShape::setThickness(float _thickness)
 {
diff --git a/dart/dynamics/LineSegmentShape.hpp b/dart/dynamics/LineSegmentShape.hpp
index 996cfce973596..d62e1060ff1fe 100644
--- a/dart/dynamics/LineSegmentShape.hpp
+++ b/dart/dynamics/LineSegmentShape.hpp
@@ -56,6 +56,12 @@ class LineSegmentShape : public Shape
   LineSegmentShape(const Eigen::Vector3d& _v1, const Eigen::Vector3d& _v2,
                    float _thickness = 1.0f);
 
+  // Documentation inherited.
+  const std::string& getType() const;
+
+  /// Returns shape type for this class
+  static const std::string& getStaticType();
+
   /// Set the line thickness/width for rendering
   void setThickness(float _thickness);
 
diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp
index 2e4c777ddc5c2..a8953ab4ff21a 100644
--- a/dart/dynamics/MeshShape.cpp
+++ b/dart/dynamics/MeshShape.cpp
@@ -152,6 +152,19 @@ MeshShape::~MeshShape() {
   delete mMesh;
 }
 
+//==============================================================================
+const std::string& MeshShape::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+const std::string& MeshShape::getStaticType()
+{
+  static const std::string type("MeshShape");
+  return type;
+}
+
 const aiScene* MeshShape::getMesh() const {
   return mMesh;
 }
diff --git a/dart/dynamics/MeshShape.hpp b/dart/dynamics/MeshShape.hpp
index 35ab8c7121b5f..3fb0513e1d48e 100644
--- a/dart/dynamics/MeshShape.hpp
+++ b/dart/dynamics/MeshShape.hpp
@@ -75,6 +75,12 @@ class MeshShape : public Shape {
   /// \brief Destructor.
   virtual ~MeshShape();
 
+  // Documentation inherited.
+  const std::string& getType() const;
+
+  /// Returns shape type for this class
+  static const std::string& getStaticType();
+
   /// \brief
   const aiScene* getMesh() const;
 
diff --git a/dart/dynamics/PlaneShape.cpp b/dart/dynamics/PlaneShape.cpp
index e8ea631bd2d15..b3479cfbb1ef6 100644
--- a/dart/dynamics/PlaneShape.cpp
+++ b/dart/dynamics/PlaneShape.cpp
@@ -56,6 +56,19 @@ PlaneShape::PlaneShape(const Eigen::Vector3d& _normal,
 {
 }
 
+//==============================================================================
+const std::string& PlaneShape::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+const std::string& PlaneShape::getStaticType()
+{
+  static const std::string type("PlaneShape");
+  return type;
+}
+
 //==============================================================================
 Eigen::Matrix3d PlaneShape::computeInertia(double /*mass*/) const
 {
diff --git a/dart/dynamics/PlaneShape.hpp b/dart/dynamics/PlaneShape.hpp
index a48b786716438..8f2f5c0f3c3c0 100644
--- a/dart/dynamics/PlaneShape.hpp
+++ b/dart/dynamics/PlaneShape.hpp
@@ -52,6 +52,12 @@ class PlaneShape : public Shape
   /// Constructor
   PlaneShape(const Eigen::Vector3d& _normal, const Eigen::Vector3d& _point);
 
+  // Documentation inherited.
+  const std::string& getType() const;
+
+  /// Returns shape type for this class
+  static const std::string& getStaticType();
+
   // Documentation inherited.
   Eigen::Matrix3d computeInertia(double mass) const override;
 
diff --git a/dart/dynamics/Shape.hpp b/dart/dynamics/Shape.hpp
index fa3e8d0d6ffc9..365509d6d4e07 100644
--- a/dart/dynamics/Shape.hpp
+++ b/dart/dynamics/Shape.hpp
@@ -42,18 +42,19 @@
 
 #include <Eigen/Dense>
 
-#include "dart/math/Geometry.hpp"
+#include "dart/common/Deprecated.hpp"
 #include "dart/common/Subject.hpp"
+#include "dart/math/Geometry.hpp"
 #include "dart/dynamics/SmartPointer.hpp"
 
 namespace dart {
 namespace dynamics {
-/// \brief
+
 class Shape : public virtual common::Subject
 {
 public:
-  // TODO(JS): We should not use ShapeType because this is not extendable.
-  /// \brief
+
+  /// \deprecated Deprecated in 6.1. Please use getType() instead.
   enum ShapeType {
     BOX,
     ELLIPSOID,
@@ -82,6 +83,9 @@ class Shape : public virtual common::Subject
   /// \brief Destructor
   virtual ~Shape();
 
+  /// Returns a string representing the shape type
+  virtual const std::string& getType() const = 0;
+
   /// \brief Get the bounding box of the shape in its local coordinate frame.
   ///        The dimension will be automatically determined by the sub-classes
   ///        such as BoxShape, EllipsoidShape, CylinderShape, and MeshShape.
@@ -102,7 +106,8 @@ class Shape : public virtual common::Subject
   /// \brief
   int getID() const;
 
-  /// \brief
+  /// \deprecated Deprecated in 6.1. Please use getType() instead.
+  DEPRECATED(6.1)
   ShapeType getShapeType() const;
 
   /// Set the data variance of this shape. Use the DataVariance to indicate what
@@ -155,7 +160,8 @@ class Shape : public virtual common::Subject
 
 private:
 
-  /// \brief Type of primitive
+  /// \deprecated Deprecated in 6.1. Please use getType() instead.
+  /// \brief Type of primitive shpae
   ShapeType mType;
 
 };
diff --git a/dart/dynamics/SoftMeshShape.cpp b/dart/dynamics/SoftMeshShape.cpp
index 07c300b26c475..aaf93341cddc2 100644
--- a/dart/dynamics/SoftMeshShape.cpp
+++ b/dart/dynamics/SoftMeshShape.cpp
@@ -61,6 +61,19 @@ SoftMeshShape::~SoftMeshShape()
   // Do nothing
 }
 
+//==============================================================================
+const std::string& SoftMeshShape::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+const std::string& SoftMeshShape::getStaticType()
+{
+  static const std::string type("SoftMeshShape");
+  return type;
+}
+
 const aiMesh* SoftMeshShape::getAssimpMesh() const
 {
   return mAssimpMesh.get();
diff --git a/dart/dynamics/SoftMeshShape.hpp b/dart/dynamics/SoftMeshShape.hpp
index ff145d8cef284..c4d066c163b42 100644
--- a/dart/dynamics/SoftMeshShape.hpp
+++ b/dart/dynamics/SoftMeshShape.hpp
@@ -59,6 +59,12 @@ class SoftMeshShape : public Shape
   /// \brief Destructor.
   virtual ~SoftMeshShape();
 
+  // Documentation inherited.
+  const std::string& getType() const;
+
+  /// Returns shape type for this class
+  static const std::string& getStaticType();
+
   /// \brief
   const aiMesh* getAssimpMesh() const;
 
diff --git a/dart/gui/OpenGLRenderInterface.cpp b/dart/gui/OpenGLRenderInterface.cpp
index 897315bb71912..672815c71c690 100644
--- a/dart/gui/OpenGLRenderInterface.cpp
+++ b/dart/gui/OpenGLRenderInterface.cpp
@@ -468,41 +468,24 @@ void OpenGLRenderInterface::compileList(dynamics::BodyNode* node)
 }
 
 //==============================================================================
-//FIXME: Use polymorphism instead of switch statements
-void OpenGLRenderInterface::compileList(dynamics::Shape* _shape) {
-    if(_shape == 0)
-        return;
-
-    switch(_shape->getShapeType()) {
-        case dynamics::Shape::BOX:
-            break;
-        case dynamics::Shape::CYLINDER:
-            break;
-        case dynamics::Shape::ELLIPSOID:
-            break;
-        case dynamics::Shape::PLANE:
-            dterr << "PLANE is not supported yet." << std::endl;
-            break;
-        case dynamics::Shape::SOFT_MESH:
-            // Do nothing
-            break;
-        case dynamics::Shape::MESH:
-        {
-            //FIXME: Separate these calls once BodyNode is refactored to contain
-            // both a col Shape and vis Shape.
-            dynamics::MeshShape* shapeMesh = dynamic_cast<dynamics::MeshShape*>(_shape);
-
-            if(shapeMesh == 0)
-                return;
-
-            shapeMesh->setDisplayList(compileList(shapeMesh->getScale(), shapeMesh->getMesh()));
-
-            break;
-        }
-        case dynamics::Shape::LINE_SEGMENT:
-            // Do nothing
-            break;
-    }
+void OpenGLRenderInterface::compileList(dynamics::Shape* shape)
+{
+  if (!shape)
+    return;
+
+  if (shape->getType() == dynamics::MeshShape::getStaticType())
+  {
+    assert(dynamic_cast<dynamics::MeshShape*>(shape));
+
+    auto* mesh = static_cast<dynamics::MeshShape*>(shape);
+    mesh->setDisplayList(compileList(mesh->getScale(), mesh->getMesh()));
+  }
+  else
+  {
+    dtwarn << "[OpenGLRenderInterface::compileList] Attempting to compile "
+           << "OpenGL list for an unsupported shape type ["
+           << shape->getType() << "].\n";
+  }
 }
 
 GLuint OpenGLRenderInterface::compileList(const Eigen::Vector3d& _scale, const aiScene* _mesh) {
diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp
index 63ba9c4534b55..e5ffec490f699 100644
--- a/dart/gui/SimWindow.cpp
+++ b/dart/gui/SimWindow.cpp
@@ -407,64 +407,50 @@ void SimWindow::drawShape(const dynamics::Shape* shape,
   using dynamics::SoftMeshShape;
   using dynamics::LineSegmentShape;
 
-  switch (shape->getShapeType())
-  {
-    case Shape::BOX:
-    {
-      const auto& box = static_cast<const BoxShape*>(shape);
-      mRI->drawCube(box->getSize());
-
-      break;
-    }
-    case Shape::ELLIPSOID:
-    {
-      const auto& ellipsoid = static_cast<const EllipsoidShape*>(shape);
-      mRI->drawEllipsoid(ellipsoid->getSize());
-
-      break;
-    }
-    case Shape::CYLINDER:
-    {
-      const auto& cylinder = static_cast<const CylinderShape*>(shape);
-      mRI->drawCylinder(cylinder->getRadius(), cylinder->getHeight());
-
-      break;
-    }
-    case Shape::MESH:
-    {
-      const auto& mesh = static_cast<const MeshShape*>(shape);
-
-      glDisable(GL_COLOR_MATERIAL); // Use mesh colors to draw
-
-      if (mesh->getDisplayList())
-        mRI->drawList(mesh->getDisplayList());
-      else
-        mRI->drawMesh(mesh->getScale(), mesh->getMesh());
+  const auto& shapeType = shape->getType();
 
-      break;
-    }
-    case Shape::SOFT_MESH:
-    {
-      const auto& softMesh = static_cast<const SoftMeshShape*>(shape);
-      mRI->drawSoftMesh(softMesh->getAssimpMesh());
+  if (BoxShape::getStaticType() == shapeType)
+  {
+    const auto* box = static_cast<const BoxShape*>(shape);
+    mRI->drawCube(box->getSize());
+  }
+  else if (EllipsoidShape::getStaticType() == shapeType)
+  {
+    const auto* ellipsoid = static_cast<const EllipsoidShape*>(shape);
+    mRI->drawEllipsoid(ellipsoid->getSize());
+  }
+  else if (CylinderShape::getStaticType() == shapeType)
+  {
+    const auto* cylinder = static_cast<const CylinderShape*>(shape);
+    mRI->drawCylinder(cylinder->getRadius(), cylinder->getHeight());
+  }
+  else if (MeshShape::getStaticType() == shapeType)
+  {
+    const auto& mesh = static_cast<const MeshShape*>(shape);
 
-      break;
-    }
-    case Shape::LINE_SEGMENT:
-    {
-      const auto& lineSegmentShape
-          = static_cast<const LineSegmentShape*>(shape);
-      mRI->drawLineSegments(lineSegmentShape->getVertices(),
-                            lineSegmentShape->getConnections());
+    glDisable(GL_COLOR_MATERIAL); // Use mesh colors to draw
 
-      break;
-    }
-    default:
-    {
-      dterr << "[SimWindow::drawShape] Attempting to draw unsupported shape "
-            << "type '" << shape->getShapeType() << "'.\n";
-      break;
-    }
+    if (mesh->getDisplayList())
+      mRI->drawList(mesh->getDisplayList());
+    else
+      mRI->drawMesh(mesh->getScale(), mesh->getMesh());
+  }
+  else if (SoftMeshShape::getStaticType() == shapeType)
+  {
+    const auto& softMesh = static_cast<const SoftMeshShape*>(shape);
+    mRI->drawSoftMesh(softMesh->getAssimpMesh());
+  }
+  else if (LineSegmentShape::getStaticType() == shapeType)
+  {
+    const auto& lineSegmentShape
+        = static_cast<const LineSegmentShape*>(shape);
+    mRI->drawLineSegments(lineSegmentShape->getVertices(),
+                          lineSegmentShape->getConnections());
+  }
+  else
+  {
+    dterr << "[SimWindow::drawShape] Attempting to draw an unsupported shape "
+          << "type [" << shapeType << "].\n";
   }
 
   glDisable(GL_COLOR_MATERIAL);
diff --git a/dart/gui/osg/ShapeFrameNode.cpp b/dart/gui/osg/ShapeFrameNode.cpp
index a83c27c587869..cf8f0f3d00d03 100644
--- a/dart/gui/osg/ShapeFrameNode.cpp
+++ b/dart/gui/osg/ShapeFrameNode.cpp
@@ -173,88 +173,74 @@ void ShapeFrameNode::createShapeNode(
 
   mShapeNode = nullptr;
 
-  switch(shape->getShapeType())
-  {
-    case Shape::BOX:
-    {
-      std::shared_ptr<BoxShape> bs =
-          std::dynamic_pointer_cast<BoxShape>(shape);
-      if(bs)
-        mShapeNode = new render::BoxShapeNode(bs, this);
-      else
-        warnAboutUnsuccessfulCast("BoxShape", mShapeFrame->getName());
-      break;
-    }
-
-    case Shape::ELLIPSOID:
-    {
-      std::shared_ptr<EllipsoidShape> es =
-          std::dynamic_pointer_cast<EllipsoidShape>(shape);
-      if(es)
-        mShapeNode = new render::EllipsoidShapeNode(es, this);
-      else
-        warnAboutUnsuccessfulCast("EllipsoidShape", mShapeFrame->getName());
-      break;
-    }
-
-    case Shape::CYLINDER:
-    {
-      std::shared_ptr<CylinderShape> cs =
-          std::dynamic_pointer_cast<CylinderShape>(shape);
-      if(cs)
-        mShapeNode = new render::CylinderShapeNode(cs, this);
-      else
-        warnAboutUnsuccessfulCast("CylinderShape", mShapeFrame->getName());
-      break;
-    }
-
-    case Shape::PLANE:
-    {
-      std::shared_ptr<PlaneShape> ps =
-          std::dynamic_pointer_cast<PlaneShape>(shape);
-      if(ps)
-        mShapeNode = new render::PlaneShapeNode(ps, this);
-      else
-        warnAboutUnsuccessfulCast("PlaneShape", mShapeFrame->getName());
-      break;
-    }
-
-    case Shape::MESH:
-    {
-      std::shared_ptr<MeshShape> ms =
-          std::dynamic_pointer_cast<MeshShape>(shape);
-      if(ms)
-        mShapeNode = new render::MeshShapeNode(ms, this);
-      else
-        warnAboutUnsuccessfulCast("MeshShape", mShapeFrame->getName());
-      break;
-    }
-
-    case Shape::SOFT_MESH:
-    {
-      std::shared_ptr<SoftMeshShape> sms =
-          std::dynamic_pointer_cast<SoftMeshShape>(shape);
-      if(sms)
-        mShapeNode = new render::SoftMeshShapeNode(sms, this);
-      else
-        warnAboutUnsuccessfulCast("SoftMeshShape", mShapeFrame->getName());
-      break;
-    }
+  const auto& shapeType = shape->getType();
 
-    case Shape::LINE_SEGMENT:
-    {
-      std::shared_ptr<LineSegmentShape> lss =
-          std::dynamic_pointer_cast<LineSegmentShape>(shape);
-      if(lss)
-        mShapeNode = new render::LineSegmentShapeNode(lss, this);
-      else
-        warnAboutUnsuccessfulCast("LineSegmentShape", mShapeFrame->getName());
-      break;
-    }
-
-    default:
-      mShapeNode = new render::WarningShapeNode(shape, this);
-      break;
+  if(BoxShape::getStaticType() == shapeType)
+  {
+    std::shared_ptr<BoxShape> bs =
+        std::dynamic_pointer_cast<BoxShape>(shape);
+    if(bs)
+      mShapeNode = new render::BoxShapeNode(bs, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
+  else if(EllipsoidShape::getStaticType() == shapeType)
+  {
+    std::shared_ptr<EllipsoidShape> es =
+        std::dynamic_pointer_cast<EllipsoidShape>(shape);
+    if(es)
+      mShapeNode = new render::EllipsoidShapeNode(es, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
+  else if(CylinderShape::getStaticType() == shapeType)
+  {
+    std::shared_ptr<CylinderShape> cs =
+        std::dynamic_pointer_cast<CylinderShape>(shape);
+    if(cs)
+      mShapeNode = new render::CylinderShapeNode(cs, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
+  else if(PlaneShape::getStaticType() == shapeType)
+  {
+    std::shared_ptr<PlaneShape> ps =
+        std::dynamic_pointer_cast<PlaneShape>(shape);
+    if(ps)
+      mShapeNode = new render::PlaneShapeNode(ps, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
+  else if(MeshShape::getStaticType() == shapeType)
+  {
+    std::shared_ptr<MeshShape> ms =
+        std::dynamic_pointer_cast<MeshShape>(shape);
+    if(ms)
+      mShapeNode = new render::MeshShapeNode(ms, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
+  else if(SoftMeshShape::getStaticType() == shapeType)
+  {
+    std::shared_ptr<SoftMeshShape> sms =
+        std::dynamic_pointer_cast<SoftMeshShape>(shape);
+    if(sms)
+      mShapeNode = new render::SoftMeshShapeNode(sms, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
+  else if(LineSegmentShape::getStaticType() == shapeType)
+  {
+    std::shared_ptr<LineSegmentShape> lss =
+        std::dynamic_pointer_cast<LineSegmentShape>(shape);
+    if(lss)
+      mShapeNode = new render::LineSegmentShapeNode(lss, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
+  else
+  {
+    mShapeNode = new render::WarningShapeNode(shape, this);
   }
 
   if(nullptr == mShapeNode)
diff --git a/dart/gui/osg/render/WarningShapeNode.cpp b/dart/gui/osg/render/WarningShapeNode.cpp
index e85fe8e86d189..e7b7fc964c9e7 100644
--- a/dart/gui/osg/render/WarningShapeNode.cpp
+++ b/dart/gui/osg/render/WarningShapeNode.cpp
@@ -50,7 +50,7 @@ WarningShapeNode::WarningShapeNode(std::shared_ptr<dart::dynamics::Shape> shape,
                                    ShapeFrameNode* parent)
   : ShapeNode(shape, parent, this)
 {
-  dtwarn << "Shape type (" << shape->getShapeType()
+  dtwarn << "Shape type (" << shape->getType()
          << ") found in Entity '" << parent->getName()
          << "' is not currently supported by dart::gui::osg, "
          << "and will not be rendered\n";
diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp
index 8feac46876dea..08960adcf018f 100644
--- a/dart/utils/SkelParser.cpp
+++ b/dart/utils/SkelParser.cpp
@@ -629,8 +629,8 @@ void readAspects(
       {
         for (auto& shapeNode : bodyNode->getShapeNodes())
         {
-          auto shapeType = shapeNode->getShape()->getShapeType();
-          if (dynamics::Shape::SOFT_MESH == shapeType)
+          const auto& shapeType = shapeNode->getShape()->getType();
+          if (dynamics::SoftMeshShape::getStaticType() == shapeType)
             continue;
 
           auto mass = bodyNode->getMass();
diff --git a/package.xml b/package.xml
index 27a17e70fa157..aa52996f05c3d 100644
--- a/package.xml
+++ b/package.xml
@@ -4,7 +4,7 @@
        a Catkin workspace. Catkin is not required to build DART. For more
        information, see: http://ros.org/reps/rep-0136.html -->
   <name>dart</name>
-  <version>6.0.0</version>
+  <version>6.1.0</version>
   <description>
     DART (Dynamic Animation and Robotics Toolkit) is a collaborative,
     cross-platform, open source library created by the Georgia Tech Graphics