Add Factory class and collision detector factory (#864)

CMakeLists.txt

@@ -309,8 +309,14 @@ if(TINYXML_FOUND AND TINYXML2_FOUND AND BULLET_FOUND)
     endif(DART_VERBOSE)
 
     # Add tutorial targets
-    dart_add_tutorial(tutorials/tutorialBiped dart-utils-urdf dart-gui)
-    dart_add_tutorial(tutorials/tutorialBiped-Finished dart-utils-urdf dart-gui)
+    if(HAVE_BULLET_COLLISION)
+      dart_add_tutorial(tutorials/tutorialBiped dart-utils-urdf dart-gui dart-collision-bullet)
+      dart_add_tutorial(tutorials/tutorialBiped-Finished dart-utils-urdf dart-gui dart-collision-bullet)
+    else()
+      dart_add_tutorial(tutorials/tutorialBiped dart-utils-urdf dart-gui)
+      dart_add_tutorial(tutorials/tutorialBiped-Finished dart-utils-urdf dart-gui)
+    endif()
+
     dart_add_tutorial(tutorials/tutorialCollisions dart-utils-urdf dart-gui)
     dart_add_tutorial(tutorials/tutorialCollisions-Finished dart-utils-urdf dart-gui)
     dart_add_tutorial(tutorials/tutorialDominoes dart-utils-urdf dart-gui)

dart/CMakeLists.txt

@@ -52,7 +52,7 @@ install(FILES ${DART_CONFIG_HPP_OUT} DESTINATION include/dart)
 # dart-optimizer-ipopt - {dart}, optimizer/ipopt, [ipopt]
 # dart-optimizer-nlopt - {dart}, optimizer/nlopt, [nlopt]
 # dart-collision-bullet - {dart}, collision/bullet, [bullet]
-# dart-utils - {dart-collision-bullet}, utils, utils/sdf, [tinyxml, tinyxml2]
+# dart-utils - {dart}, utils, utils/sdf, [tinyxml, tinyxml2]
 # dart-utils-urdf - {dart-utils}, utils/urdf, [urdfdom]
 # dart-gui - {dart}, gui, [opengl, glut]
 # dart-gui-osg - {dart-gui}, gui/osg, gui/osg/render, [openscenegraph]
@@ -97,11 +97,11 @@ if(FLANN_FOUND)
   add_subdirectory(planning)
 endif()
 
-if(TINYXML_FOUND AND TINYXML2_FOUND AND BULLET_FOUND)
+if(TINYXML_FOUND AND TINYXML2_FOUND)
   add_subdirectory(utils)
 endif()
 
-if(OPENGL_FOUND AND GLUT_FOUND AND BULLET_FOUND)
+if(OPENGL_FOUND AND GLUT_FOUND)
   add_subdirectory(gui)
 endif()
 

dart/collision/CollisionDetector.cpp

@@ -42,6 +42,12 @@
 namespace dart {
 namespace collision {
 
+//==============================================================================
+CollisionDetector::Factory* CollisionDetector::getFactory()
+{
+  return SingletonFactory::getSingletonPtr();
+}
+
 //==============================================================================
 std::shared_ptr<CollisionGroup>
 CollisionDetector::createCollisionGroupAsSharedPtr()

dart/collision/CollisionDetector.hpp

@@ -37,6 +37,7 @@
 
 #include <Eigen/Dense>
 
+#include "dart/common/Factory.hpp"
 #include "dart/collision/Contact.hpp"
 #include "dart/collision/CollisionOption.hpp"
 #include "dart/collision/CollisionResult.hpp"
@@ -57,6 +58,18 @@ class CollisionDetector : public std::enable_shared_from_this<CollisionDetector>
   friend class CollisionObject;
   friend class CollisionGroup;
 
+  using Factory = common::Factory<
+      std::string, CollisionDetector, std::shared_ptr<CollisionDetector>>;
+
+  using SingletonFactory = common::Singleton<Factory>;
+
+  template <typename Derived>
+  using Registrar = common::FactoryRegistrar<
+      std::string, CollisionDetector, Derived, std::shared_ptr<CollisionDetector>>;
+
+  /// Returns the singleton factory.
+  static Factory* getFactory();
+
   /// Destructor
   virtual ~CollisionDetector() = default;
 

dart/collision/bullet/BulletCollisionDetector.cpp

@@ -80,6 +80,14 @@ btCollisionShape* createBulletCollisionShapeFromAssimpMesh(const aiMesh* mesh);
 
 } // anonymous namespace
 
+//==============================================================================
+BulletCollisionDetector::Registrar<BulletCollisionDetector>
+BulletCollisionDetector::mRegistrar{
+  BulletCollisionDetector::getStaticType(),
+  []() -> std::shared_ptr<dart::collision::BulletCollisionDetector> {
+      return dart::collision::BulletCollisionDetector::create();
+  }};
+
 //==============================================================================
 std::shared_ptr<BulletCollisionDetector> BulletCollisionDetector::create()
 {

dart/collision/bullet/BulletCollisionDetector.hpp

@@ -125,6 +125,7 @@ class BulletCollisionDetector : public CollisionDetector
 
   std::unique_ptr<BulletCollisionGroup> mGroupForFiltering;
 
+  static Registrar<BulletCollisionDetector> mRegistrar;
 };
 
 }  // namespace collision

dart/collision/dart/DARTCollisionDetector.cpp

@@ -57,7 +57,13 @@ void postProcess(CollisionObject* o1, CollisionObject* o2, const CollisionOption
 
 } // anonymous namespace
 
-
+//==============================================================================
+DARTCollisionDetector::Registrar<DARTCollisionDetector>
+DARTCollisionDetector::mRegistrar{
+  DARTCollisionDetector::getStaticType(),
+  []() -> std::shared_ptr<dart::collision::DARTCollisionDetector> {
+      return dart::collision::DARTCollisionDetector::create();
+  }};
 
 //==============================================================================
 std::shared_ptr<DARTCollisionDetector> DARTCollisionDetector::create()

dart/collision/dart/DARTCollisionDetector.hpp

@@ -93,6 +93,8 @@ class DARTCollisionDetector : public CollisionDetector
   std::unique_ptr<CollisionObject> createCollisionObject(
       const dynamics::ShapeFrame* shapeFrame) override;
 
+private:
+  static Registrar<DARTCollisionDetector> mRegistrar;
 };
 
 }  // namespace collision

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -645,7 +645,13 @@ fcl::BVHModel<BV>* createSoftMesh(const aiMesh* _mesh)
 
 } // anonymous namespace
 
-
+//==============================================================================
+FCLCollisionDetector::Registrar<FCLCollisionDetector>
+FCLCollisionDetector::mRegistrar{
+  FCLCollisionDetector::getStaticType(),
+  []() -> std::shared_ptr<dart::collision::FCLCollisionDetector> {
+      return dart::collision::FCLCollisionDetector::create();
+  }};
 
 //==============================================================================
 std::shared_ptr<FCLCollisionDetector> FCLCollisionDetector::create()

dart/collision/fcl/FCLCollisionDetector.hpp

@@ -187,6 +187,7 @@ class FCLCollisionDetector : public CollisionDetector
 
   ShapeMap mShapeMap;
 
+  static Registrar<FCLCollisionDetector> mRegistrar;
 };
 
 }  // namespace collision

dart/collision/ode/OdeCollisionDetector.cpp

@@ -96,6 +96,14 @@ struct OdeCollisionCallbackData
 
 } // anonymous namespace
 
+//==============================================================================
+OdeCollisionDetector::Registrar<OdeCollisionDetector>
+OdeCollisionDetector::mRegistrar{
+  OdeCollisionDetector::getStaticType(),
+  []() -> std::shared_ptr<dart::collision::OdeCollisionDetector> {
+      return dart::collision::OdeCollisionDetector::create();
+  }};
+
 //==============================================================================
 std::shared_ptr<OdeCollisionDetector> OdeCollisionDetector::create()
 {

dart/collision/ode/OdeCollisionDetector.hpp

@@ -114,6 +114,7 @@ class OdeCollisionDetector : public CollisionDetector
 
 private:
   dContactGeom contactCollisions[MAX_COLLIDE_RETURNS];
+  static Registrar<OdeCollisionDetector> mRegistrar;
 };
 
 }  // namespace collision

dart/common/Factory.hpp

@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2017, Graphics Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2017, Personal Robotics Lab, Carnegie Mellon University
+ * All rights reserved.
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_COMMON_FACTORY_HPP_
+#define DART_COMMON_FACTORY_HPP_
+
+#include <functional>
+#include <unordered_map>
+#include <memory>
+
+#include "dart/common/StlHelpers.hpp"
+#include "dart/common/Singleton.hpp"
+
+namespace dart {
+namespace common {
+
+/// Implementation of the Abstract Factory Pattern.
+///
+/// Example:
+/// \code
+/// using CdFactory = Factory<std::string, CollisionDetector>;
+///
+/// auto factory = CdFactory();
+/// factory.registerCreator<FclCollisionDetector>("fcl");
+/// auto fclCd = CdFactory::create("fcl");
+/// \endcode
+template <typename KeyT,
+          typename BaseT,
+          typename HeldT = std::shared_ptr<BaseT>,
+          typename... Args>
+class Factory
+{
+public:
+  struct EnumClassHash;
+
+  using This = Factory<KeyT, BaseT, HeldT>;
+  using Creator = std::function<HeldT(Args...)>;
+  template <typename Key>
+  using HashType = typename std::conditional<
+      std::is_enum<Key>::value, EnumClassHash, std::hash<Key>>::type;
+  using CreatorMap = std::unordered_map<KeyT, Creator, HashType<KeyT>>;
+
+  /// Default constructor.
+  Factory() = default;
+
+  /// Destructor
+  virtual ~Factory() = default;
+
+  /// Registers a object creator function with a key.
+  void registerCreator(const KeyT& key, Creator creator);
+
+  /// Registers the default object creator function with a key.
+  template <typename Derived>
+  void registerCreator(const KeyT& key);
+
+  /// Unregisters the object creator function that is registered with a key. Do
+  /// nothing if there is no creator function associated with the key.
+  void unregisterCreator(const KeyT& key);
+
+  /// Unregisters all the object creator functions.
+  void unregisterAllCreators();
+
+  /// Returns true if an object creator function is registered with the key.
+  /// Otherwise, returns false.
+  bool canCreate(const KeyT& key);
+
+  /// Creates an object of the class that is registered with a key. Returns
+  /// nullptr if there is no object creator function associated with the key.
+  HeldT create(const KeyT& key, Args&&... args);
+  // TODO(JS): Add create() for creating smart_pointers
+  // (see: https://github.com/dartsim/dart/pull/845)
+
+private:
+  /// Object creator function map.
+  CreatorMap mCreatorMap;
+};
+
+/// Helper class to register a object creator function to the Singleton.
+template <typename KeyT,
+          typename BaseT,
+          typename DerivedT,
+          typename HeldT = std::shared_ptr<BaseT>,
+          typename... Args>
+class FactoryRegistrar final
+{
+public:
+  using This = FactoryRegistrar<KeyT, BaseT, DerivedT, HeldT>;
+  using FactoryType = Factory<KeyT, BaseT, HeldT, Args...>;
+  using SingletonFactory = Singleton<FactoryType>;
+  using Creator = typename FactoryType::Creator;
+
+  /// Constructor. Interanlly, this constructor registers Derived class with
+  /// the key and the default creator function.
+  FactoryRegistrar(const KeyT& key, Creator creator);
+
+  /// Constructor. Interanlly, this constructor registers Derived class with
+  /// the key and the default creator function.
+  explicit FactoryRegistrar(const KeyT& key);
+};
+
+} // namespace common
+} // namespace dart
+
+#include "dart/common/detail/Factory-impl.hpp"
+
+#endif // DART_COMMON_FACTORY_HPP_

dart/common/Singleton.hpp

@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2017, Graphics Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2017, Personal Robotics Lab, Carnegie Mellon University
+ * All rights reserved.
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_COMMON_SINGLETON_HPP_
+#define DART_COMMON_SINGLETON_HPP_
+
+namespace dart {
+namespace common {
+
+/// Singleton template class
+///
+/// \note This singleton is not thread safe. For use of thread safe singleton,
+/// use static initialization as:
+///
+/// // Singletone class Engine
+/// class Engine : public Singleton<Engine> {};
+///
+/// // Call before main() and use theT only instead of calling getSingleton()
+/// static T& theT = T::getSingleton();
+template<typename T>
+class Singleton
+{
+public:
+  /// Returns reference of the singleton
+  template <typename... Args>
+  static T& getSingleton(Args... _args);
+
+  /// Returns pointer of the singleton
+  template <typename ... Args>
+  static T* getSingletonPtr(Args... _args);
+
+protected:
+  /// Constructor
+  Singleton() = default;
+
+  /// Destructor
+  virtual ~Singleton() = default;
+
+private:
+  /// Don't implement copy constructor
+  Singleton(const T&) = delete;
+
+  /// Don't assignment operator
+  const T& operator=(const T&) = delete;
+
+private:
+  /// Singleton instance
+  static T* mInstance;
+};
+
+} // namespace common
+} // namespace dart
+
+#include "dart/common/detail/Singleton-impl.hpp"
+
+#endif // DART_COMMON_SINGLETON_HPP_