Add python bindings for pointcloud and contact retrieval (#1455)

CHANGELOG.md

@@ -57,6 +57,7 @@
   * Added bindings for BallJoint::convertToPositions(): [#1408](https://github.com/dartsim/dart/pull/1408)
   * Fixed typos in Skeleton: [#1392](https://github.com/dartsim/dart/pull/1392)
   * Fixed enabling drag and drop for InteractiveFrame: [#1432](https://github.com/dartsim/dart/pull/1432)
+  * Added bindings for pointcloud and contact retrieval: [#1455](https://github.com/dartsim/dart/pull/1455)
 
 * Build and testing
 

python/dartpy/collision/CollisionResult.cpp

@@ -32,6 +32,7 @@
 
 #include <dart/dart.hpp>
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
 namespace py = pybind11;
 
@@ -47,6 +48,19 @@ void CollisionResult(py::module& m)
           +[](const dart::collision::CollisionResult* self) -> std::size_t {
             return self->getNumContacts();
           })
+      .def(
+          "getContact",
+          +[](dart::collision::CollisionResult* self, std::size_t index)
+              -> dart::collision::Contact& { return self->getContact(index); },
+          ::py::arg("index"),
+          ::py::return_value_policy::reference)
+      .def(
+          "getContacts",
+          +[](const dart::collision::CollisionResult* self)
+              -> const std::vector<dart::collision::Contact>& {
+            return self->getContacts();
+          },
+          ::py::return_value_policy::reference)
       .def(
           "inCollision",
           +[](const dart::collision::CollisionResult* self,
@@ -66,9 +80,9 @@ void CollisionResult(py::module& m)
           +[](const dart::collision::CollisionResult* self) -> bool {
             return self->isCollision();
           })
-      .def("clear", +[](dart::collision::CollisionResult* self) {
-        self->clear();
-      });
+      .def(
+          "clear",
+          +[](dart::collision::CollisionResult* self) { self->clear(); });
 }
 
 } // namespace python

python/dartpy/dynamics/Shape.cpp

@@ -32,6 +32,8 @@
 
 #include <dart/dart.hpp>
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
 #include "eigen_geometry_pybind.h"
 #include "eigen_pybind.h"
 
@@ -322,7 +324,8 @@ void Shape(py::module& m)
             return self->getType();
           },
           ::py::return_value_policy::reference_internal)
-      .def("update", +[](dart::dynamics::MeshShape* self) { self->update(); })
+      .def(
+          "update", +[](dart::dynamics::MeshShape* self) { self->update(); })
       .def(
           "notifyAlphaUpdated",
           +[](dart::dynamics::MeshShape* self, double alpha) {
@@ -683,6 +686,164 @@ void Shape(py::module& m)
             return dart::dynamics::PlaneShape::getStaticType();
           },
           ::py::return_value_policy::reference_internal);
+  ::py::class_<
+      dart::dynamics::PointCloudShape,
+      dart::dynamics::Shape,
+      std::shared_ptr<dart::dynamics::PointCloudShape>>
+      pointCloudShape(m, "PointCloudShape");
+
+  pointCloudShape.def(::py::init<double>(), ::py::arg("visualSize") = 0.01)
+      .def(
+          "getType",
+          +[](const dart::dynamics::PointCloudShape* self)
+              -> const std::string& { return self->getType(); },
+          ::py::return_value_policy::reference_internal)
+      .def(
+          "computeInertia",
+          +[](const dart::dynamics::PointCloudShape* self, double mass)
+              -> Eigen::Matrix3d { return self->computeInertia(mass); },
+          ::py::arg("mass"))
+      .def(
+          "reserve",
+          +[](dart::dynamics::PointCloudShape* self, std::size_t size) -> void {
+            return self->reserve(size);
+          },
+          ::py::arg("size"))
+      .def(
+          "addPoint",
+          +[](dart::dynamics::PointCloudShape* self,
+              const Eigen::Vector3d& point) -> void {
+            return self->addPoint(point);
+          },
+          ::py::arg("point"))
+      .def(
+          "addPoint",
+          +[](dart::dynamics::PointCloudShape* self,
+              const std::vector<Eigen::Vector3d>& points) -> void {
+            return self->addPoint(points);
+          },
+          ::py::arg("points"))
+      .def(
+          "setPoint",
+          +[](dart::dynamics::PointCloudShape* self,
+              const std::vector<Eigen::Vector3d>& points) -> void {
+            return self->setPoint(points);
+          },
+          ::py::arg("points"))
+      .def(
+          "getPoints",
+          +[](const dart::dynamics::PointCloudShape* self)
+              -> const std::vector<Eigen::Vector3d>& {
+            return self->getPoints();
+          },
+          ::py::return_value_policy::reference_internal)
+      .def(
+          "getNumPoints",
+          +[](const dart::dynamics::PointCloudShape* self) -> std::size_t {
+            return self->getNumPoints();
+          })
+      .def(
+          "removeAllPoints",
+          +[](dart::dynamics::PointCloudShape* self) -> void {
+            return self->removeAllPoints();
+          })
+      .def(
+          "setPointShapeType",
+          +[](dart::dynamics::PointCloudShape* self,
+              dart::dynamics::PointCloudShape::PointShapeType type) -> void {
+            return self->setPointShapeType(type);
+          },
+          ::py::arg("type"))
+      .def(
+          "getPointShapeType",
+          +[](const dart::dynamics::PointCloudShape* self)
+              -> dart::dynamics::PointCloudShape::PointShapeType {
+            return self->getPointShapeType();
+          })
+      .def(
+          "setColorMode",
+          +[](dart::dynamics::PointCloudShape* self,
+              dart::dynamics::PointCloudShape::ColorMode mode) -> void {
+            return self->setColorMode(mode);
+          },
+          ::py::arg("mode"))
+      .def(
+          "getColorMode",
+          +[](const dart::dynamics::PointCloudShape* self)
+              -> dart::dynamics::PointCloudShape::ColorMode {
+            return self->getColorMode();
+          })
+      .def(
+          "setOverallColor",
+          +[](dart::dynamics::PointCloudShape* self,
+              const Eigen::Vector4d& color) -> void {
+            return self->setOverallColor(color);
+          },
+          ::py::arg("color"))
+      .def(
+          "getOverallColor",
+          +[](const dart::dynamics::PointCloudShape* self) -> Eigen::Vector4d {
+            return self->getOverallColor();
+          })
+      .def(
+          "setColors",
+          +[](dart::dynamics::PointCloudShape* self,
+              const std::vector<
+                  Eigen::Vector4d,
+                  Eigen::aligned_allocator<Eigen::Vector4d>>& colors) -> void {
+            return self->setColors(colors);
+          },
+          ::py::arg("colors"))
+      .def(
+          "getColors",
+          +[](const dart::dynamics::PointCloudShape* self)
+              -> const std::vector<
+                  Eigen::Vector4d,
+                  Eigen::aligned_allocator<Eigen::Vector4d>>& {
+            return self->getColors();
+          })
+      .def(
+          "setVisualSize",
+          +[](dart::dynamics::PointCloudShape* self, double size) -> void {
+            return self->setVisualSize(size);
+          },
+          ::py::arg("size"))
+      .def(
+          "getVisualSize",
+          +[](const dart::dynamics::PointCloudShape* self) -> double {
+            return self->getVisualSize();
+          })
+      .def(
+          "notifyColorUpdated",
+          +[](dart::dynamics::PointCloudShape* self,
+              const Eigen::Vector4d& color) {
+            self->notifyColorUpdated(color);
+          },
+          ::py::arg("color"));
+
+  ::py::enum_<dart::dynamics::PointCloudShape::ColorMode>(
+      pointCloudShape, "ColorMode")
+      .value(
+          "USE_SHAPE_COLOR",
+          dart::dynamics::PointCloudShape::ColorMode::USE_SHAPE_COLOR)
+      .value(
+          "BIND_OVERALL",
+          dart::dynamics::PointCloudShape::ColorMode::BIND_OVERALL)
+      .value(
+          "BIND_PER_POINT",
+          dart::dynamics::PointCloudShape::ColorMode::BIND_PER_POINT)
+      .export_values();
+
+  ::py::enum_<dart::dynamics::PointCloudShape::PointShapeType>(
+      pointCloudShape, "PointShapeType")
+      .value("BOX", dart::dynamics::PointCloudShape::PointShapeType::BOX)
+      .value(
+          "BILLBOARD_SQUARE",
+          dart::dynamics::PointCloudShape::PointShapeType::BILLBOARD_SQUARE)
+      .value(
+          "BILLBOARD_CIRCLE",
+          dart::dynamics::PointCloudShape::PointShapeType::BILLBOARD_CIRCLE)
+      .export_values();
 
   ::py::class_<
       dart::dynamics::SphereShape,

python/examples/contacts_pointcloud/main.py

@@ -0,0 +1,112 @@
+import dartpy as dart
+
+"""
+Displays Contacts between a Robot arm in a GUI as a pointcloud
+
+Demonstrates the following dartpy functionality:
+- Extracting contact points from simulation
+- Drawing of a point cloud (with variable data during simulation)
+- Hooking functionality to be executed in the viewer run() function
+"""
+
+
+class ContactVisualizingNode(dart.gui.osg.RealTimeWorldNode):
+    """ WorldNode subclass, which extracts the contacts of the last simulation
+    step and adds them to a pointloud for visualization.
+
+    Note: Simulation must run for the contacts to be displayed.
+
+    """
+
+    def __init__(self, world, pointCloudShape):
+        """
+        Input:
+        ------
+        world : the dart.simulation.World object to visualize
+        pointCloudShape : dart.dynamics.PointCloudShape to set contacts in
+                          before each display refresh
+        """
+        super(ContactVisualizingNode, self).__init__(world)
+        self._pointCloudShape = pointCloudShape
+
+    def customPreRefresh(self):
+        # here, we overload the function to add contact points to the
+        # pointcloud before each GUI refresh step
+        contactPoints = self._getListOfContactPoints()
+        self._pointCloudShape.setPoint(contactPoints)
+
+    def _getListOfContactPoints(self):
+        return [
+            c.point
+            for c in self.getWorld().getLastCollisionResult().getContacts()
+        ]
+
+
+def setAlpha(skeleton, alphaValue):
+    """ setAlpha(skeleton : dartpy.dynamics.Skeleton, alphaValue : float) -> None
+
+    sets the transparency (alpha) value for each body node of the skeleton,
+    assuming that the VisualAspect is contained within the first ShapeNode
+    by convention.
+
+    Input:
+    ------
+    skeleton : The skeleton to set transparency values for
+    alphaValue: [0,1] transparency, where 1 is fully visible
+    """
+    for bodyNode in skeleton.getBodyNodes():
+        bodyNode.getShapeNode(0).getVisualAspect().setAlpha(alphaValue)
+
+
+def main():
+    world = dart.simulation.World()
+
+    # load KR5 robot and ground plane, set transparent:
+    urdfParser = dart.utils.DartLoader()
+    kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf")
+    # set transparency, so that "inner" contact points will be visible
+    setAlpha(kr5, 0.3)
+    ground = urdfParser.parseSkeleton("dart://sample/urdf/KR5/ground.urdf")
+    setAlpha(ground, 0.3)
+    world.addSkeleton(kr5)
+    world.addSkeleton(ground)
+    world.setGravity([0, -9.81, 0])
+
+    # add point cloud shape for visualizing contacts
+    pointCloudShape = dart.dynamics.PointCloudShape(0.02)
+
+    # Since contact points may change during execution, dynamic data variance
+    # is assumed for the pointcloud of contacts. Otherwise, OSG will not render
+    # the new points.
+    pointCloudShape.setDataVariance(dart.dynamics.Shape.DataVariance.DYNAMIC)
+
+    pointCloudSimpleFrame = dart.dynamics.SimpleFrame(
+        dart.dynamics.Frame.World(), "ContactsVisualization"
+    )
+    pointCloudSimpleFrame.setShape(pointCloudShape)
+    pcVisualAspect = pointCloudSimpleFrame.createVisualAspect()
+    pcVisualAspect.setRGBA([0.7, 0, 0, 1])
+    world.addSimpleFrame(pointCloudSimpleFrame)
+
+    # Create world node and add it to viewer
+    node = ContactVisualizingNode(world, pointCloudShape)
+
+    # create a viewer with background color (red, green, blue, alpha), here: white
+    viewer = dart.gui.osg.Viewer([1.0, 1.0, 1.0, 1.0])
+    viewer.addWorldNode(node)
+
+    # Grid settings
+    grid = dart.gui.osg.GridVisual()
+    grid.setPlaneType(dart.gui.osg.GridVisual.PlaneType.ZX)
+    grid.setOffset([0, -0.55, 0])
+    viewer.addAttachment(grid)
+
+    viewer.setUpViewInWindow(0, 0, 640, 480)
+    viewer.setCameraHomePosition(
+        [2.0, 1.0, 2.0], [0.00, 0.00, 0.00], [-0.24, 0.94, -0.25]
+    )
+    viewer.run()
+
+
+if __name__ == "__main__":
+    main()