[dartpy] Add DART, Bullet, Ode collision detectors (#1339)

* dartpy: Add DART, Bullet, Ode collision detectors * Update changelog * Remove numpy dependency, which is not necessary * Use namespace alias for pybind11 as py * dartpy: Add test for changing collision detector of world
dartsim · May 26, 2019 · 82e39c1 · 82e39c1
1 parent d9038fe
commit 82e39c1
Show file tree

Hide file tree

Showing 92 changed files with 3,466 additions and 2,788 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -51,6 +51,7 @@
   * Added constraint APIs: [#1333](https://github.com/dartsim/dart/pull/1333)
   * Added BallJoint, RevoluteJoint, joint properties, and chain tutorial (incomplete): [#1335](https://github.com/dartsim/dart/pull/1335)
   * Added all the joints: [#1337](https://github.com/dartsim/dart/pull/1337)
+  * Added DART, Bullet, Ode collision detectors: [#1339](https://github.com/dartsim/dart/pull/1339)
 
 ### [DART 6.8.5 (2019-05-26)](https://github.com/dartsim/dart/milestone/57?closed=1)
 

diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp
@@ -158,7 +158,7 @@ class CollisionDetector : public std::enable_shared_from_this<CollisionDetector>
       const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
       DistanceResult* result = nullptr) = 0;
 
-  /// Perform raycast to a collision group.
+  /// Performs raycast to a collision group.
   ///
   /// \param[in] group The collision group the ray will be casted onto.
   /// \param[in] from The start point of the ray in world coordinates.

diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp
@@ -220,6 +220,19 @@ double CollisionGroup::distance(
   return mCollisionDetector->distance(this, otherGroup, option, result);
 }
 
+//==============================================================================
+bool CollisionGroup::raycast(
+    const Eigen::Vector3d& from,
+    const Eigen::Vector3d& to,
+    const RaycastOption& option,
+    RaycastResult* result)
+{
+  if(mUpdateAutomatically)
+    update();
+
+  return mCollisionDetector->raycast(this, from, to, option, result);
+}
+
 //==============================================================================
 void CollisionGroup::setAutomaticUpdate(const bool automatic)
 {

diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp
@@ -41,6 +41,8 @@
 #include "dart/collision/CollisionResult.hpp"
 #include "dart/collision/DistanceOption.hpp"
 #include "dart/collision/DistanceResult.hpp"
+#include "dart/collision/RaycastOption.hpp"
+#include "dart/collision/RaycastResult.hpp"
 #include "dart/common/Observer.hpp"
 #include "dart/dynamics/SmartPointer.hpp"
 
@@ -268,6 +270,19 @@ class CollisionGroup
       const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
       DistanceResult* result = nullptr);
 
+  /// Performs raycast to this collision group.
+  ///
+  /// \param[in] from The start point of the ray in world coordinates.
+  /// \param[in] to The end point of the ray in world coordinates.
+  /// \param[in] option The raycast option.
+  /// \param[in] result The raycast result.
+  /// \return True if the ray hit an collision object.
+  bool raycast(
+      const Eigen::Vector3d& from,
+      const Eigen::Vector3d& to,
+      const RaycastOption& option = RaycastOption(),
+      RaycastResult* result = nullptr);
+
   /// Set whether this CollisionGroup will automatically check for updates.
   void setAutomaticUpdate(bool automatic = true);
 

diff --git a/python/dartpy/CMakeLists.txt b/python/dartpy/CMakeLists.txt
@@ -71,6 +71,9 @@ endif()
 if(TARGET dart-collision-bullet)
   target_link_libraries(dartpy PUBLIC dart-collision-bullet)
 endif()
+if(TARGET dart-collision-ode)
+  target_link_libraries(dartpy PUBLIC dart-collision-ode)
+endif()
 
 set_target_properties(dartpy
   PROPERTIES

diff --git a/python/dartpy/collision/BulletCollisionDetector.cpp b/python/dartpy/collision/BulletCollisionDetector.cpp
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2011-2019, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * 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.
+ */
+
+#include <dart/config.hpp>
+
+#if HAVE_BULLET
+
+#include <dart/collision/bullet/bullet.hpp>
+#include <pybind11/pybind11.h>
+
+namespace py = pybind11;
+
+namespace dart {
+namespace python {
+
+void BulletCollisionDetector(py::module& m)
+{
+  ::py::class_<
+      dart::collision::BulletCollisionDetector,
+      std::shared_ptr<dart::collision::BulletCollisionDetector>,
+      dart::collision::CollisionDetector>(m, "BulletCollisionDetector")
+      .def(::py::init(
+          +[]() -> std::shared_ptr<dart::collision::BulletCollisionDetector> {
+            return dart::collision::BulletCollisionDetector::create();
+          }))
+      .def(
+          "cloneWithoutCollisionObjects",
+          +[](const dart::collision::BulletCollisionDetector* self)
+              -> std::shared_ptr<dart::collision::CollisionDetector> {
+            return self->cloneWithoutCollisionObjects();
+          })
+      .def(
+          "getType",
+          +[](const dart::collision::BulletCollisionDetector* self)
+              -> const std::string& { return self->getType(); },
+          ::py::return_value_policy::reference_internal)
+      .def(
+          "createCollisionGroup",
+          +[](dart::collision::BulletCollisionDetector* self)
+              -> std::unique_ptr<dart::collision::CollisionGroup> {
+            return self->createCollisionGroup();
+          })
+      .def_static(
+          "getStaticType",
+          +[]() -> const std::string& {
+            return dart::collision::BulletCollisionDetector::getStaticType();
+          },
+          ::py::return_value_policy::reference_internal);
+}
+
+} // namespace python
+} // namespace dart
+
+#endif // HAVE_BULLET
diff --git a/python/dartpy/collision/BulletCollisionGroup.cpp b/python/dartpy/collision/BulletCollisionGroup.cpp
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2011-2019, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * 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.
+ */
+
+#include <dart/config.hpp>
+
+#if HAVE_BULLET
+
+#include <dart/collision/bullet/bullet.hpp>
+#include <pybind11/pybind11.h>
+
+namespace py = pybind11;
+
+namespace dart {
+namespace python {
+
+void BulletCollisionGroup(py::module& m)
+{
+  ::py::class_<
+      dart::collision::BulletCollisionGroup,
+      dart::collision::CollisionGroup,
+      std::shared_ptr<dart::collision::BulletCollisionGroup>>(
+      m, "BulletCollisionGroup")
+      .def(
+          ::py::init<const dart::collision::CollisionDetectorPtr&>(),
+          ::py::arg("collisionDetector"));
+}
+
+} // namespace python
+} // namespace dart
+
+#endif // HAVE_BULLET
diff --git a/python/dartpy/collision/CollisionDetector.cpp b/python/dartpy/collision/CollisionDetector.cpp
@@ -33,12 +33,14 @@
 #include <dart/dart.hpp>
 #include <pybind11/pybind11.h>
 
+namespace py = pybind11;
+
 namespace dart {
 namespace python {
 
-void CollisionDetector(pybind11::module& m)
+void CollisionDetector(py::module& m)
 {
-  ::pybind11::class_<
+  ::py::class_<
       dart::collision::CollisionDetector,
       std::shared_ptr<dart::collision::CollisionDetector> >(
       m, "CollisionDetector")
@@ -52,7 +54,7 @@ void CollisionDetector(pybind11::module& m)
           "getType",
           +[](const dart::collision::CollisionDetector* self)
               -> const std::string& { return self->getType(); },
-          ::pybind11::return_value_policy::reference_internal)
+          ::py::return_value_policy::reference_internal)
       .def(
           "createCollisionGroup",
           +[](dart::collision::CollisionDetector* self)

diff --git a/python/dartpy/collision/CollisionGroup.cpp b/python/dartpy/collision/CollisionGroup.cpp
@@ -33,12 +33,14 @@
 #include <dart/dart.hpp>
 #include <pybind11/pybind11.h>
 
+namespace py = pybind11;
+
 namespace dart {
 namespace python {
 
-void CollisionGroup(pybind11::module& m)
+void CollisionGroup(py::module& m)
 {
-  ::pybind11::class_<
+  ::py::class_<
       dart::collision::CollisionGroup,
       std::shared_ptr<dart::collision::CollisionGroup> >(m, "CollisionGroup")
       .def(
@@ -59,13 +61,13 @@ void CollisionGroup(pybind11::module& m)
               const dart::dynamics::ShapeFrame* shapeFrame) {
             self->addShapeFrame(shapeFrame);
           },
-          ::pybind11::arg("shapeFrame"))
+          ::py::arg("shapeFrame"))
       .def(
           "addShapeFrames",
           +[](dart::collision::CollisionGroup* self,
               const std::vector<const dart::dynamics::ShapeFrame*>&
                   shapeFrames) { self->addShapeFrames(shapeFrames); },
-          ::pybind11::arg("shapeFrames"))
+          ::py::arg("shapeFrames"))
       .def(
           "addShapeFramesOf",
           +[](dart::collision::CollisionGroup* self) {
@@ -80,13 +82,13 @@ void CollisionGroup(pybind11::module& m)
               const dart::dynamics::ShapeFrame* shapeFrame) {
             self->removeShapeFrame(shapeFrame);
           },
-          ::pybind11::arg("shapeFrame"))
+          ::py::arg("shapeFrame"))
       .def(
           "removeShapeFrames",
           +[](dart::collision::CollisionGroup* self,
               const std::vector<const dart::dynamics::ShapeFrame*>&
                   shapeFrames) { self->removeShapeFrames(shapeFrames); },
-          ::pybind11::arg("shapeFrames"))
+          ::py::arg("shapeFrames"))
       .def(
           "removeShapeFramesOf",
           +[](dart::collision::CollisionGroup* self) {
@@ -103,7 +105,7 @@ void CollisionGroup(pybind11::module& m)
               const dart::dynamics::ShapeFrame* shapeFrame) -> bool {
             return self->hasShapeFrame(shapeFrame);
           },
-          ::pybind11::arg("shapeFrame"))
+          ::py::arg("shapeFrame"))
       .def(
           "getNumShapeFrames",
           +[](const dart::collision::CollisionGroup* self) -> std::size_t {
@@ -120,16 +122,70 @@ void CollisionGroup(pybind11::module& m)
               const dart::collision::CollisionOption& option) -> bool {
             return self->collide(option);
           },
-          ::pybind11::arg("option"))
+          ::py::arg("option"))
       .def(
           "collide",
           +[](dart::collision::CollisionGroup* self,
               const dart::collision::CollisionOption& option,
               dart::collision::CollisionResult* result) -> bool {
             return self->collide(option, result);
           },
-          ::pybind11::arg("option"),
-          ::pybind11::arg("result"))
+          ::py::arg("option"),
+          ::py::arg("result"))
+      .def(
+          "distance",
+          +[](dart::collision::CollisionGroup* self) -> double {
+            return self->distance();
+          })
+      .def(
+          "distance",
+          +[](dart::collision::CollisionGroup* self,
+              const dart::collision::DistanceOption& option) -> double {
+            return self->distance(option);
+          },
+          ::py::arg("option"))
+      .def(
+          "distance",
+          +[](dart::collision::CollisionGroup* self,
+              const dart::collision::DistanceOption& option,
+              dart::collision::DistanceResult* result) -> double {
+            return self->distance(option, result);
+          },
+          ::py::arg("option"),
+          ::py::arg("result"))
+      .def(
+          "raycast",
+          +[](dart::collision::CollisionGroup* self,
+              const Eigen::Vector3d& from,
+              const Eigen::Vector3d& to) -> bool {
+            return self->raycast(from, to);
+          },
+          ::py::arg("from"),
+          ::py::arg("to"))
+      .def(
+          "raycast",
+          +[](dart::collision::CollisionGroup* self,
+              const Eigen::Vector3d& from,
+              const Eigen::Vector3d& to,
+              const dart::collision::RaycastOption& option) -> bool {
+            return self->raycast(from, to, option);
+          },
+          ::py::arg("from"),
+          ::py::arg("to"),
+          ::py::arg("option"))
+      .def(
+          "raycast",
+          +[](dart::collision::CollisionGroup* self,
+              const Eigen::Vector3d& from,
+              const Eigen::Vector3d& to,
+              const dart::collision::RaycastOption& option,
+              dart::collision::RaycastResult* result) -> bool {
+            return self->raycast(from, to, option, result);
+          },
+          ::py::arg("from"),
+          ::py::arg("to"),
+          ::py::arg("option"),
+          ::py::arg("result"))
       .def(
           "setAutomaticUpdate",
           +[](dart::collision::CollisionGroup* self) {
@@ -140,7 +196,7 @@ void CollisionGroup(pybind11::module& m)
           +[](dart::collision::CollisionGroup* self, bool automatic) {
             self->setAutomaticUpdate(automatic);
           },
-          ::pybind11::arg("automatic"))
+          ::py::arg("automatic"))
       .def(
           "getAutomaticUpdate",
           +[](const dart::collision::CollisionGroup* self) -> bool {