From 88dfe70a0728bf2a74777875567b52197c0ab527 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 27 Aug 2020 20:33:54 -0700 Subject: [PATCH] Add python bindings for adding ShapeFrames to CollisionGroup (#1490) --- CHANGELOG.md | 1 + python/dartpy/collision/CollisionGroup.cpp | 119 ++++++++++++++++-- python/dartpy/dynamics/InverseKinematics.cpp | 1 + python/dartpy/gui/osg/Viewer.cpp | 5 + python/tests/unit/collision/test_collision.py | 49 ++++++++ 5 files changed, 166 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b4f61b71653f4..654269498b7d7 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/python/dartpy/collision/CollisionGroup.cpp b/python/dartpy/collision/CollisionGroup.cpp index 8ffbeeb28027f..822a6393b9d12 100644 --- a/python/dartpy/collision/CollisionGroup.cpp +++ b/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& + 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& + 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 { diff --git a/python/dartpy/dynamics/InverseKinematics.cpp b/python/dartpy/dynamics/InverseKinematics.cpp index e6c7b9343248e..b3b90f0601e1f 100644 --- a/python/dartpy/dynamics/InverseKinematics.cpp +++ b/python/dartpy/dynamics/InverseKinematics.cpp @@ -32,6 +32,7 @@ #include #include +#include #include "eigen_geometry_pybind.h" #include "eigen_pybind.h" diff --git a/python/dartpy/gui/osg/Viewer.cpp b/python/dartpy/gui/osg/Viewer.cpp index c10cb419ed5c2..cedbaf7b81bae 100644 --- a/python/dartpy/gui/osg/Viewer.cpp +++ b/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, diff --git a/python/tests/unit/collision/test_collision.py b/python/tests/unit/collision/test_collision.py index e56b910a7434c..575fbdcb4e884 100644 --- a/python/tests/unit/collision/test_collision.py +++ b/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()