Add python bindings for adding ShapeFrames to CollisionGroup (#1490)

CHANGELOG.md

@@ -66,6 +66,7 @@
   * Added bindings for pointcloud and contact retrieval: [#1455](https://github.com/dartsim/dart/pull/1455)
   * Fixed TypeError from dartpy.dynamics.Node.getBodyNodePtr(): [#1463](https://github.com/dartsim/dart/pull/1463)
   * Added pybind/eigen.h to DistanceResult.cpp for read/write of eigen types: [#1480](https://github.com/dartsim/dart/pull/1480)
+  * Added bindings for adding ShapeFrames to CollisionGroup: [#1490](https://github.com/dartsim/dart/pull/1490)
 
 * Build and testing
 

python/dartpy/collision/CollisionGroup.cpp

@@ -71,9 +71,43 @@ void CollisionGroup(py::module& m)
           ::py::arg("shapeFrames"))
       .def(
           "addShapeFramesOf",
-          +[](dart::collision::CollisionGroup* self) {
-            self->addShapeFramesOf();
-          })
+          +[](dart::collision::CollisionGroup* self,
+              const dynamics::ShapeFrame* shapeFrame) {
+            self->addShapeFramesOf(shapeFrame);
+          },
+          ::py::arg("shapeFrame"),
+          "Adds a ShapeFrame")
+      .def(
+          "addShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              const std::vector<const dart::dynamics::ShapeFrame*>&
+                  shapeFrames) { self->addShapeFramesOf(shapeFrames); },
+          ::py::arg("shapeFrames"),
+          "Adds ShapeFrames")
+      .def(
+          "addShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              const dart::collision::CollisionGroup* otherGroup) {
+            self->addShapeFramesOf(otherGroup);
+          },
+          ::py::arg("otherGroup"),
+          "Adds ShapeFrames of other CollisionGroup")
+      .def(
+          "addShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              const dart::dynamics::BodyNode* body) {
+            self->addShapeFramesOf(body);
+          },
+          ::py::arg("body"),
+          "Adds ShapeFrames of BodyNode")
+      .def(
+          "addShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              const dart::dynamics::MetaSkeleton* skeleton) {
+            self->addShapeFramesOf(skeleton);
+          },
+          ::py::arg("skeleton"),
+          "Adds ShapeFrames of MetaSkeleton")
       .def(
           "subscribeTo",
           +[](dart::collision::CollisionGroup* self) { self->subscribeTo(); })
@@ -92,9 +126,43 @@ void CollisionGroup(py::module& m)
           ::py::arg("shapeFrames"))
       .def(
           "removeShapeFramesOf",
-          +[](dart::collision::CollisionGroup* self) {
-            self->removeShapeFramesOf();
-          })
+          +[](dart::collision::CollisionGroup* self,
+              const dynamics::ShapeFrame* shapeFrame) {
+            self->removeShapeFramesOf(shapeFrame);
+          },
+          ::py::arg("shapeFrame"),
+          "Removes a ShapeFrame")
+      .def(
+          "removeShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              const std::vector<const dart::dynamics::ShapeFrame*>&
+                  shapeFrames) { self->removeShapeFramesOf(shapeFrames); },
+          ::py::arg("shapeFrames"),
+          "Removes ShapeFrames")
+      .def(
+          "removeShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              const dart::collision::CollisionGroup* otherGroup) {
+            self->removeShapeFramesOf(otherGroup);
+          },
+          ::py::arg("otherGroup"),
+          "Removes ShapeFrames of other CollisionGroup")
+      .def(
+          "removeShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              const dart::dynamics::BodyNode* body) {
+            self->removeShapeFramesOf(body);
+          },
+          ::py::arg("body"),
+          "Removes ShapeFrames of BodyNode")
+      .def(
+          "removeShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              const dart::dynamics::MetaSkeleton* skeleton) {
+            self->removeShapeFramesOf(skeleton);
+          },
+          ::py::arg("skeleton"),
+          "Removes ShapeFrames of MetaSkeleton")
       .def(
           "removeAllShapeFrames",
           +[](dart::collision::CollisionGroup* self) {
@@ -116,14 +184,16 @@ void CollisionGroup(py::module& m)
           "collide",
           +[](dart::collision::CollisionGroup* self) -> bool {
             return self->collide();
-          })
+          },
+          "Performs collision check within this CollisionGroup")
       .def(
           "collide",
           +[](dart::collision::CollisionGroup* self,
               const dart::collision::CollisionOption& option) -> bool {
             return self->collide(option);
           },
-          ::py::arg("option"))
+          ::py::arg("option"),
+          "Performs collision check within this CollisionGroup")
       .def(
           "collide",
           +[](dart::collision::CollisionGroup* self,
@@ -132,7 +202,38 @@ void CollisionGroup(py::module& m)
             return self->collide(option, result);
           },
           ::py::arg("option"),
-          ::py::arg("result"))
+          ::py::arg("result"),
+          "Performs collision check within this CollisionGroup")
+      .def(
+          "collide",
+          +[](dart::collision::CollisionGroup* self,
+              dart::collision::CollisionGroup* otherGroup) -> bool {
+            return self->collide(otherGroup);
+          },
+          ::py::arg("otherGroup"),
+          "Perform collision check against other CollisionGroup")
+      .def(
+          "collide",
+          +[](dart::collision::CollisionGroup* self,
+              dart::collision::CollisionGroup* otherGroup,
+              const dart::collision::CollisionOption& option) -> bool {
+            return self->collide(otherGroup, option);
+          },
+          ::py::arg("otherGroup"),
+          ::py::arg("option"),
+          "Perform collision check against other CollisionGroup")
+      .def(
+          "collide",
+          +[](dart::collision::CollisionGroup* self,
+              dart::collision::CollisionGroup* otherGroup,
+              const dart::collision::CollisionOption& option,
+              dart::collision::CollisionResult* result) -> bool {
+            return self->collide(otherGroup, option, result);
+          },
+          ::py::arg("otherGroup"),
+          ::py::arg("option"),
+          ::py::arg("result"),
+          "Perform collision check against other CollisionGroup")
       .def(
           "distance",
           +[](dart::collision::CollisionGroup* self) -> double {

python/dartpy/dynamics/InverseKinematics.cpp

@@ -32,6 +32,7 @@
 
 #include <dart/dart.hpp>
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 #include "eigen_geometry_pybind.h"
 #include "eigen_pybind.h"
 

python/dartpy/gui/osg/Viewer.cpp

@@ -327,6 +327,11 @@ void Viewer(py::module& m)
       .def(
           "run",
           +[](dart::gui::osg::Viewer* self) -> int { return self->run(); })
+      .def("frame", +[](dart::gui::osg::Viewer* self) { self->frame(); })
+      .def(
+          "frame",
+          +[](dart::gui::osg::Viewer* self,
+              double simulationTime) { self->frame(simulationTime); })
       .def(
           "setUpViewInWindow",
           +[](dart::gui::osg::Viewer* self,

python/tests/unit/collision/test_collision.py

@@ -21,6 +21,7 @@ def collision_groups_tester(cd):
     group = cd.createCollisionGroup()
     group.addShapeFrame(simple_frame1)
     group.addShapeFrame(simple_frame2)
+    assert group.getNumShapeFrames() is 2
 
     #
     #    ( s1,s2 )              collision!
@@ -51,6 +52,54 @@ def collision_groups_tester(cd):
     assert result.isCollision()
     assert result.getNumContacts() is not 0
 
+    # Repeat the same test with BodyNodes instead of SimpleFrames
+
+    group.removeAllShapeFrames()
+    assert group.getNumShapeFrames() is 0
+
+    skel1 = dart.dynamics.Skeleton()
+    skel2 = dart.dynamics.Skeleton()
+
+    [joint1, body1] = skel1.createFreeJointAndBodyNodePair(None)
+    [joint2, body2] = skel2.createFreeJointAndBodyNodePair(None)
+
+    shape_node1 = body1.createShapeNode(sphere1)
+    shape_node1.createVisualAspect()
+    shape_node1.createCollisionAspect()
+
+    shape_node2 = body2.createShapeNode(sphere2)
+    shape_node2.createVisualAspect()
+    shape_node2.createCollisionAspect()
+
+    group.addShapeFramesOf(body1)
+    group.addShapeFramesOf(body2)
+
+    assert group.getNumShapeFrames() is 2
+
+    assert group.collide()
+
+    joint2.setPosition(3, 3)
+    assert not group.collide()
+
+    # Repeat the same test with BodyNodes and two groups
+
+    joint2.setPosition(3, 0)
+
+    group.removeAllShapeFrames()
+    assert group.getNumShapeFrames() is 0
+    group2 = cd.createCollisionGroup()
+
+    group.addShapeFramesOf(body1)
+    group2.addShapeFramesOf(body2)
+
+    assert group.getNumShapeFrames() is 1
+    assert group2.getNumShapeFrames() is 1
+
+    assert group.collide(group2)
+
+    joint2.setPosition(3, 3)
+    assert not group.collide(group2)
+
 
 def test_collision_groups():
     cd = dart.collision.FCLCollisionDetector()