From 2c7f3b98c2eecea50821116967c75ad5c58b7c60 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 19 May 2019 23:30:47 -0700 Subject: [PATCH 1/3] dartpy: Adding shapes --- python/dartpy/common/Observer.cpp | 47 ++ python/dartpy/common/module.cpp | 2 + python/dartpy/dynamics/Shape.cpp | 526 ++++++++++++++++++ python/dartpy/dynamics/module.cpp | 4 + python/dartpy/gui/osg/DragAndDrop.cpp | 278 +++++++++ python/dartpy/gui/osg/Viewer.cpp | 67 +++ python/dartpy/gui/osg/module.cpp | 4 + .../operational_space_control/main.py | 6 + 8 files changed, 934 insertions(+) create mode 100644 python/dartpy/common/Observer.cpp create mode 100644 python/dartpy/dynamics/Shape.cpp create mode 100644 python/dartpy/gui/osg/DragAndDrop.cpp diff --git a/python/dartpy/common/Observer.cpp b/python/dartpy/common/Observer.cpp new file mode 100644 index 0000000000000..d867e18ade8e6 --- /dev/null +++ b/python/dartpy/common/Observer.cpp @@ -0,0 +1,47 @@ +/* + * 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 +#include + +namespace dart { +namespace python { + +void Observer(pybind11::module& m) +{ + ::pybind11:: + class_>( + m, "Observer"); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/common/module.cpp b/python/dartpy/common/module.cpp index a79d4017598a1..97016fa5d75d2 100644 --- a/python/dartpy/common/module.cpp +++ b/python/dartpy/common/module.cpp @@ -35,6 +35,7 @@ namespace dart { namespace python { +void Observer(pybind11::module& sm); void Subject(pybind11::module& sm); void Uri(pybind11::module& sm); @@ -42,6 +43,7 @@ void dart_common(pybind11::module& m) { auto sm = m.def_submodule("common"); + Observer(sm); Subject(sm); Uri(sm); } diff --git a/python/dartpy/dynamics/Shape.cpp b/python/dartpy/dynamics/Shape.cpp new file mode 100644 index 0000000000000..b4f39f5b64bab --- /dev/null +++ b/python/dartpy/dynamics/Shape.cpp @@ -0,0 +1,526 @@ +/* + * 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 +#include +#include "eigen_geometry_pybind.h" +#include "eigen_pybind.h" + +namespace dart { +namespace python { + +void Shape(pybind11::module& m) +{ + ::pybind11::class_< + dart::dynamics::Shape, + dart::common::Subject, + std::shared_ptr>(m, "Shape") + .def( + "getType", + +[](const dart::dynamics::Shape* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getBoundingBox", + +[](const dart::dynamics::Shape* self) + -> const dart::math::BoundingBox& { + return self->getBoundingBox(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "computeInertia", + +[](const dart::dynamics::Shape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def( + "computeInertiaFromDensity", + +[](const dart::dynamics::Shape* self, + double density) -> Eigen::Matrix3d { + return self->computeInertiaFromDensity(density); + }, + ::pybind11::arg("density")) + .def( + "computeInertiaFromMass", + +[](const dart::dynamics::Shape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertiaFromMass(mass); }, + ::pybind11::arg("mass")) + .def( + "getVolume", + +[](const dart::dynamics::Shape* self) -> double { + return self->getVolume(); + }) + .def( + "getID", + +[](const dart::dynamics::Shape* self) -> std::size_t { + return self->getID(); + }) + .def( + "setDataVariance", + +[](dart::dynamics::Shape* self, unsigned int _variance) { + self->setDataVariance(_variance); + }, + ::pybind11::arg("variance")) + .def( + "addDataVariance", + +[](dart::dynamics::Shape* self, unsigned int _variance) { + self->addDataVariance(_variance); + }, + ::pybind11::arg("variance")) + .def( + "removeDataVariance", + +[](dart::dynamics::Shape* self, unsigned int _variance) { + self->removeDataVariance(_variance); + }, + ::pybind11::arg("variance")) + .def( + "getDataVariance", + +[](const dart::dynamics::Shape* self) -> unsigned int { + return self->getDataVariance(); + }) + .def( + "checkDataVariance", + +[](const dart::dynamics::Shape* self, + dart::dynamics::Shape::DataVariance type) -> bool { + return self->checkDataVariance(type); + }, + ::pybind11::arg("type")) + .def( + "refreshData", + +[](dart::dynamics::Shape* self) { self->refreshData(); }) + .def( + "notifyAlphaUpdated", + +[](dart::dynamics::Shape* self, double alpha) { + self->notifyAlphaUpdated(alpha); + }, + ::pybind11::arg("alpha")) + .def( + "notifyColorUpdated", + +[](dart::dynamics::Shape* self, const Eigen::Vector4d& color) { + self->notifyColorUpdated(color); + }, + ::pybind11::arg("color")) + .def( + "incrementVersion", + +[](dart::dynamics::Shape* self) -> std::size_t { + return self->incrementVersion(); + }) + .def_readonly( + "onVersionChanged", &dart::dynamics::Shape::onVersionChanged); + + ::pybind11::class_< + dart::dynamics::BoxShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "BoxShape") + .def(::pybind11::init(), ::pybind11::arg("size")) + .def( + "getType", + +[](const dart::dynamics::BoxShape* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setSize", + +[](dart::dynamics::BoxShape* self, const Eigen::Vector3d& _size) { + self->setSize(_size); + }, + ::pybind11::arg("size")) + .def( + "getSize", + +[](const dart::dynamics::BoxShape* self) -> const Eigen::Vector3d& { + return self->getSize(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "computeInertia", + +[](const dart::dynamics::BoxShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::BoxShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def_static( + "computeVolume", + +[](const Eigen::Vector3d& size) -> double { + return dart::dynamics::BoxShape::computeVolume(size); + }, + ::pybind11::arg("size")) + .def_static( + "computeInertiaOf", + +[](const Eigen::Vector3d& size, double mass) -> Eigen::Matrix3d { + return dart::dynamics::BoxShape::computeInertia(size, mass); + }, + ::pybind11::arg("size"), + ::pybind11::arg("mass")); + + ::pybind11::class_< + dart::dynamics::ConeShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "ConeShape") + .def( + ::pybind11::init(), + ::pybind11::arg("radius"), + ::pybind11::arg("height")) + .def( + "getType", + +[](const dart::dynamics::ConeShape* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getRadius", + +[](const dart::dynamics::ConeShape* self) -> double { + return self->getRadius(); + }) + .def( + "setRadius", + +[](dart::dynamics::ConeShape* self, double radius) { + self->setRadius(radius); + }, + ::pybind11::arg("radius")) + .def( + "getHeight", + +[](const dart::dynamics::ConeShape* self) -> double { + return self->getHeight(); + }) + .def( + "setHeight", + +[](dart::dynamics::ConeShape* self, double height) { + self->setHeight(height); + }, + ::pybind11::arg("height")) + .def( + "computeInertia", + +[](const dart::dynamics::ConeShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::ConeShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def_static( + "computeVolume", + +[](double radius, double height) -> double { + return dart::dynamics::ConeShape::computeVolume(radius, height); + }, + ::pybind11::arg("radius"), + ::pybind11::arg("height")) + .def_static( + "computeInertiaOf", + +[](double radius, double height, double mass) -> Eigen::Matrix3d { + return dart::dynamics::ConeShape::computeInertia( + radius, height, mass); + }, + ::pybind11::arg("radius"), + ::pybind11::arg("height"), + ::pybind11::arg("mass")); + + ::pybind11::class_< + dart::dynamics::MeshShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "MeshShape") + .def( + ::pybind11::init(), + ::pybind11::arg("scale"), + ::pybind11::arg("mesh")) + .def( + ::pybind11::init< + const Eigen::Vector3d&, + const aiScene*, + const dart::common::Uri&>(), + ::pybind11::arg("scale"), + ::pybind11::arg("mesh"), + ::pybind11::arg("uri")) + .def( + ::pybind11::init< + const Eigen::Vector3d&, + const aiScene*, + const dart::common::Uri&, + dart::common::ResourceRetrieverPtr>(), + ::pybind11::arg("scale"), + ::pybind11::arg("mesh"), + ::pybind11::arg("uri"), + ::pybind11::arg("resourceRetriever")) + .def( + "getType", + +[](const dart::dynamics::MeshShape* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def("update", +[](dart::dynamics::MeshShape* self) { self->update(); }) + .def( + "notifyAlphaUpdated", + +[](dart::dynamics::MeshShape* self, double alpha) { + self->notifyAlphaUpdated(alpha); + }, + ::pybind11::arg("alpha")) + .def( + "setMesh", + +[](dart::dynamics::MeshShape* self, const aiScene* mesh) { + self->setMesh(mesh); + }, + ::pybind11::arg("mesh")) + .def( + "setMesh", + +[](dart::dynamics::MeshShape* self, + const aiScene* mesh, + const std::string& path) { self->setMesh(mesh, path); }, + ::pybind11::arg("mesh"), + ::pybind11::arg("path")) + .def( + "setMesh", + +[](dart::dynamics::MeshShape* self, + const aiScene* mesh, + const std::string& path, + dart::common::ResourceRetrieverPtr resourceRetriever) { + self->setMesh(mesh, path, resourceRetriever); + }, + ::pybind11::arg("mesh"), + ::pybind11::arg("path"), + ::pybind11::arg("resourceRetriever")) + .def( + "setMesh", + +[](dart::dynamics::MeshShape* self, + const aiScene* mesh, + const dart::common::Uri& path) { self->setMesh(mesh, path); }, + ::pybind11::arg("mesh"), + ::pybind11::arg("path")) + .def( + "setMesh", + +[](dart::dynamics::MeshShape* self, + const aiScene* mesh, + const dart::common::Uri& path, + dart::common::ResourceRetrieverPtr resourceRetriever) { + self->setMesh(mesh, path, resourceRetriever); + }, + ::pybind11::arg("mesh"), + ::pybind11::arg("path"), + ::pybind11::arg("resourceRetriever")) + .def( + "getMeshUri", + +[](const dart::dynamics::MeshShape* self) -> std::string { + return self->getMeshUri(); + }) + .def( + "getMeshUri2", + +[](const dart::dynamics::MeshShape* self) + -> const dart::common::Uri& { return self->getMeshUri2(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getMeshPath", + +[](const dart::dynamics::MeshShape* self) -> const std::string& { + return self->getMeshPath(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getResourceRetriever", + +[](dart::dynamics::MeshShape* self) + -> dart::common::ResourceRetrieverPtr { + return self->getResourceRetriever(); + }) + .def( + "setScale", + +[](dart::dynamics::MeshShape* self, const Eigen::Vector3d& scale) { + self->setScale(scale); + }, + ::pybind11::arg("scale")) + .def( + "getScale", + +[](const dart::dynamics::MeshShape* self) -> const Eigen::Vector3d& { + return self->getScale(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setColorMode", + +[](dart::dynamics::MeshShape* self, + dart::dynamics::MeshShape::ColorMode mode) { + self->setColorMode(mode); + }, + ::pybind11::arg("mode")) + .def( + "getColorMode", + +[](const dart::dynamics::MeshShape* self) + -> dart::dynamics::MeshShape::ColorMode { + return self->getColorMode(); + }) + .def( + "setColorIndex", + +[](dart::dynamics::MeshShape* self, int index) { + self->setColorIndex(index); + }, + ::pybind11::arg("index")) + .def( + "getColorIndex", + +[](const dart::dynamics::MeshShape* self) -> int { + return self->getColorIndex(); + }) + .def( + "getDisplayList", + +[](const dart::dynamics::MeshShape* self) -> int { + return self->getDisplayList(); + }) + .def( + "setDisplayList", + +[](dart::dynamics::MeshShape* self, int index) { + self->setDisplayList(index); + }, + ::pybind11::arg("index")) + .def( + "computeInertia", + +[](const dart::dynamics::MeshShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::MeshShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); + + auto attr = m.attr("MeshShape"); + + ::pybind11::enum_(attr, "ColorMode") + .value( + "MATERIAL_COLOR", + dart::dynamics::MeshShape::ColorMode::MATERIAL_COLOR) + .value("COLOR_INDEX", dart::dynamics::MeshShape::ColorMode::COLOR_INDEX) + .value("SHAPE_COLOR", dart::dynamics::MeshShape::ColorMode::SHAPE_COLOR) + .export_values(); + + ::pybind11::class_< + dart::dynamics::LineSegmentShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "LineSegmentShape") + .def(::pybind11::init<>()) + .def(::pybind11::init(), ::pybind11::arg("thickness")) + .def( + ::pybind11::init(), + ::pybind11::arg("v1"), + ::pybind11::arg("v2")) + .def( + ::pybind11:: + init(), + ::pybind11::arg("v1"), + ::pybind11::arg("v2"), + ::pybind11::arg("thickness")) + .def( + "getType", + +[](const dart::dynamics::LineSegmentShape* self) + -> const std::string& { return self->getType(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setThickness", + +[](dart::dynamics::LineSegmentShape* self, float _thickness) { + self->setThickness(_thickness); + }, + ::pybind11::arg("thickness")) + .def( + "getThickness", + +[](const dart::dynamics::LineSegmentShape* self) -> float { + return self->getThickness(); + }) + .def( + "addVertex", + +[](dart::dynamics::LineSegmentShape* self, const Eigen::Vector3d& _v) + -> std::size_t { return self->addVertex(_v); }, + ::pybind11::arg("v")) + .def( + "addVertex", + +[](dart::dynamics::LineSegmentShape* self, + const Eigen::Vector3d& _v, + std::size_t _parent) -> std::size_t { + return self->addVertex(_v, _parent); + }, + ::pybind11::arg("v"), + ::pybind11::arg("parent")) + .def( + "removeVertex", + +[](dart::dynamics::LineSegmentShape* self, std::size_t _idx) { + self->removeVertex(_idx); + }, + ::pybind11::arg("idx")) + .def( + "setVertex", + +[](dart::dynamics::LineSegmentShape* self, + std::size_t _idx, + const Eigen::Vector3d& _v) { self->setVertex(_idx, _v); }, + ::pybind11::arg("idx"), + ::pybind11::arg("v")) + .def( + "getVertex", + +[](const dart::dynamics::LineSegmentShape* self, std::size_t _idx) + -> const Eigen::Vector3d& { return self->getVertex(_idx); }, + ::pybind11::return_value_policy::reference_internal, + ::pybind11::arg("idx")) + .def( + "addConnection", + +[](dart::dynamics::LineSegmentShape* self, + std::size_t _idx1, + std::size_t _idx2) { self->addConnection(_idx1, _idx2); }, + ::pybind11::arg("idx1"), + ::pybind11::arg("idx2")) + .def( + "removeConnection", + +[](dart::dynamics::LineSegmentShape* self, + std::size_t _vertexIdx1, + std::size_t _vertexIdx2) { + self->removeConnection(_vertexIdx1, _vertexIdx2); + }, + ::pybind11::arg("vertexIdx1"), + ::pybind11::arg("vertexIdx2")) + .def( + "removeConnection", + +[](dart::dynamics::LineSegmentShape* self, + std::size_t _connectionIdx) { + self->removeConnection(_connectionIdx); + }, + ::pybind11::arg("connectionIdx")) + .def( + "computeInertia", + +[](const dart::dynamics::LineSegmentShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::LineSegmentShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/module.cpp b/python/dartpy/dynamics/module.cpp index 55b0ea7440053..d502d9c7b6197 100644 --- a/python/dartpy/dynamics/module.cpp +++ b/python/dartpy/dynamics/module.cpp @@ -35,6 +35,8 @@ namespace dart { namespace python { +void Shape(pybind11::module& sm); + void Entity(pybind11::module& sm); void Frame(pybind11::module& sm); void ShapeFrame(pybind11::module& sm); @@ -60,6 +62,8 @@ void dart_dynamics(pybind11::module& m) { auto sm = m.def_submodule("dynamics"); + Shape(sm); + Entity(sm); Frame(sm); ShapeFrame(sm); diff --git a/python/dartpy/gui/osg/DragAndDrop.cpp b/python/dartpy/gui/osg/DragAndDrop.cpp new file mode 100644 index 0000000000000..7500929b4a690 --- /dev/null +++ b/python/dartpy/gui/osg/DragAndDrop.cpp @@ -0,0 +1,278 @@ +/* + * 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 +#include +#include +#include + +PYBIND11_DECLARE_HOLDER_TYPE(T, ::osg::ref_ptr, true); + +namespace py = pybind11; + +namespace dart { +namespace python { + +void DragAndDrop(pybind11::module& m) +{ + ::pybind11::class_< + dart::gui::osg::DragAndDrop, + dart::common::Observer, + dart::common::Subject, + ::std::shared_ptr>(m, "DragAndDrop") + .def("update", +[](dart::gui::osg::DragAndDrop* self) { self->update(); }) + .def( + "setObstructable", + +[](dart::gui::osg::DragAndDrop* self, bool _obstructable) { + self->setObstructable(_obstructable); + }, + ::pybind11::arg("obstructable")) + .def( + "isObstructable", + +[](const dart::gui::osg::DragAndDrop* self) -> bool { + return self->isObstructable(); + }) + .def("move", +[](dart::gui::osg::DragAndDrop* self) { self->move(); }) + .def( + "saveState", + +[](dart::gui::osg::DragAndDrop* self) { self->saveState(); }) + .def( + "release", + +[](dart::gui::osg::DragAndDrop* self) { self->release(); }) + .def( + "getConstrainedDx", + +[](const dart::gui::osg::DragAndDrop* self) -> Eigen::Vector3d { + return self->getConstrainedDx(); + }) + .def( + "getConstrainedRotation", + +[](const dart::gui::osg::DragAndDrop* self) -> Eigen::AngleAxisd { + return self->getConstrainedRotation(); + }) + .def( + "unconstrain", + +[](dart::gui::osg::DragAndDrop* self) { self->unconstrain(); }) + .def( + "constrainToLine", + +[](dart::gui::osg::DragAndDrop* self, const Eigen::Vector3d& slope) { + self->constrainToLine(slope); + }, + ::pybind11::arg("slope")) + .def( + "constrainToPlane", + +[](dart::gui::osg::DragAndDrop* self, + const Eigen::Vector3d& normal) { + self->constrainToPlane(normal); + }, + ::pybind11::arg("normal")) + .def( + "isMoving", + +[](const dart::gui::osg::DragAndDrop* self) -> bool { + return self->isMoving(); + }) + .def( + "setRotationOption", + +[](dart::gui::osg::DragAndDrop* self, + dart::gui::osg::DragAndDrop::RotationOption option) { + self->setRotationOption(option); + }, + ::pybind11::arg("option")) + .def( + "getRotationOption", + +[](const dart::gui::osg::DragAndDrop* self) + -> dart::gui::osg::DragAndDrop::RotationOption { + return self->getRotationOption(); + }) + .def( + "setRotationModKey", + +[](dart::gui::osg::DragAndDrop* self, + osgGA::GUIEventAdapter::ModKeyMask rotationModKey) { + self->setRotationModKey(rotationModKey); + }, + ::pybind11::arg("rotationModKey")) + .def( + "getRotationModKey", + +[](const dart::gui::osg::DragAndDrop* self) + -> osgGA::GUIEventAdapter::ModKeyMask { + return self->getRotationModKey(); + }); + + auto attr = m.attr("DragAndDrop"); + + ::pybind11::enum_( + attr, "RotationOption") + .value( + "HOLD_MODKEY", + dart::gui::osg::DragAndDrop::RotationOption::HOLD_MODKEY) + .value( + "ALWAYS_ON", dart::gui::osg::DragAndDrop::RotationOption::ALWAYS_ON) + .value( + "ALWAYS_OFF", dart::gui::osg::DragAndDrop::RotationOption::ALWAYS_OFF) + .export_values(); + + ::pybind11::class_< + dart::gui::osg::SimpleFrameDnD, + dart::gui::osg::DragAndDrop, + ::std::shared_ptr>(m, "SimpleFrameDnD") + .def( + ::pybind11:: + init(), + ::pybind11::arg("viewer"), + ::pybind11::arg("frame")) + .def("move", +[](dart::gui::osg::SimpleFrameDnD* self) { self->move(); }) + .def("saveState", +[](dart::gui::osg::SimpleFrameDnD* self) { + self->saveState(); + }); + + ::pybind11::class_< + dart::gui::osg::SimpleFrameShapeDnD, + dart::gui::osg::SimpleFrameDnD, + std::shared_ptr>( + m, "SimpleFrameShapeDnD") + .def( + ::pybind11::init< + dart::gui::osg::Viewer*, + dart::dynamics::SimpleFrame*, + dart::dynamics::Shape*>(), + ::pybind11::arg("viewer"), + ::pybind11::arg("frame"), + ::pybind11::arg("shape")) + .def("update", +[](dart::gui::osg::SimpleFrameShapeDnD* self) { + self->update(); + }); + + ::pybind11::class_< + dart::gui::osg::BodyNodeDnD, + dart::gui::osg::DragAndDrop, + std::shared_ptr>(m, "BodyNodeDnD") + .def( + ::pybind11:: + init(), + ::pybind11::arg("viewer"), + ::pybind11::arg("bn")) + .def( + ::pybind11:: + init(), + ::pybind11::arg("viewer"), + ::pybind11::arg("bn"), + ::pybind11::arg("useExternalIK")) + .def( + ::pybind11::init< + dart::gui::osg::Viewer*, + dart::dynamics::BodyNode*, + bool, + bool>(), + ::pybind11::arg("viewer"), + ::pybind11::arg("bn"), + ::pybind11::arg("useExternalIK"), + ::pybind11::arg("useWholeBody")) + .def("update", +[](dart::gui::osg::BodyNodeDnD* self) { self->update(); }) + .def("move", +[](dart::gui::osg::BodyNodeDnD* self) { self->move(); }) + .def( + "saveState", + +[](dart::gui::osg::BodyNodeDnD* self) { self->saveState(); }) + .def( + "release", + +[](dart::gui::osg::BodyNodeDnD* self) { self->release(); }) + .def( + "useExternalIK", + +[](dart::gui::osg::BodyNodeDnD* self, bool external) { + self->useExternalIK(external); + }, + ::pybind11::arg("external")) + .def( + "isUsingExternalIK", + +[](const dart::gui::osg::BodyNodeDnD* self) -> bool { + return self->isUsingExternalIK(); + }) + .def( + "useWholeBody", + +[](dart::gui::osg::BodyNodeDnD* self, bool wholeBody) { + self->useWholeBody(wholeBody); + }, + ::pybind11::arg("wholeBody")) + .def( + "isUsingWholeBody", + +[](const dart::gui::osg::BodyNodeDnD* self) -> bool { + return self->isUsingWholeBody(); + }) + .def( + "setPreserveOrientationModKey", + +[](dart::gui::osg::BodyNodeDnD* self, + osgGA::GUIEventAdapter::ModKeyMask modkey) { + self->setPreserveOrientationModKey(modkey); + }, + ::pybind11::arg("modkey")) + .def( + "getPreserveOrientationModKey", + +[](const dart::gui::osg::BodyNodeDnD* self) + -> osgGA::GUIEventAdapter::ModKeyMask { + return self->getPreserveOrientationModKey(); + }) + .def( + "setJointRestrictionModKey", + +[](dart::gui::osg::BodyNodeDnD* self, + osgGA::GUIEventAdapter::ModKeyMask modkey) { + self->setJointRestrictionModKey(modkey); + }, + ::pybind11::arg("modkey")) + .def( + "getJointRestrictionModKey", + +[](const dart::gui::osg::BodyNodeDnD* self) + -> osgGA::GUIEventAdapter::ModKeyMask { + return self->getJointRestrictionModKey(); + }); + + ::pybind11::class_< + dart::gui::osg::InteractiveFrameDnD, + dart::gui::osg::DragAndDrop, + std::shared_ptr>( + m, "InteractiveFrameDnD") + .def( + ::pybind11::init< + dart::gui::osg::Viewer*, + dart::gui::osg::InteractiveFrame*>(), + ::pybind11::arg("viewer"), + ::pybind11::arg("frame")) + .def( + "update", + +[](dart::gui::osg::InteractiveFrameDnD* self) { self->update(); }) + .def( + "move", + +[](dart::gui::osg::InteractiveFrameDnD* self) { self->move(); }) + .def("saveState", +[](dart::gui::osg::InteractiveFrameDnD* self) { + self->saveState(); + }); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/gui/osg/Viewer.cpp b/python/dartpy/gui/osg/Viewer.cpp index 93f7d5e40d6d6..0c5b32bd43313 100644 --- a/python/dartpy/gui/osg/Viewer.cpp +++ b/python/dartpy/gui/osg/Viewer.cpp @@ -219,6 +219,73 @@ void Viewer(pybind11::module& m) +[](const dart::gui::osg::Viewer* self) -> bool { return self->isAllowingSimulation(); }) + .def( + "enableDragAndDrop", + +[](dart::gui::osg::Viewer* self, + dart::dynamics::Entity* entity) -> dart::gui::osg::DragAndDrop* { + return self->enableDragAndDrop(entity); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("entity")) + .def( + "enableDragAndDrop", + +[](dart::gui::osg::Viewer* self, dart::dynamics::SimpleFrame* frame) + -> dart::gui::osg::SimpleFrameDnD* { + return self->enableDragAndDrop(frame); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("frame")) + .def( + "enableDragAndDrop", + +[](dart::gui::osg::Viewer* self, + dart::dynamics::SimpleFrame* frame, + dart::dynamics::Shape* shape) + -> dart::gui::osg::SimpleFrameShapeDnD* { + return self->enableDragAndDrop(frame, shape); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("frame"), + ::pybind11::arg("shape")) + .def( + "enableDragAndDrop", + +[](dart::gui::osg::Viewer* self, dart::dynamics::BodyNode* bodyNode) + -> dart::gui::osg::BodyNodeDnD* { + return self->enableDragAndDrop(bodyNode); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("bodyNode")) + .def( + "enableDragAndDrop", + +[](dart::gui::osg::Viewer* self, + dart::dynamics::BodyNode* bodyNode, + bool useExternalIK) -> dart::gui::osg::BodyNodeDnD* { + return self->enableDragAndDrop(bodyNode, useExternalIK); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("bodyNode"), + ::pybind11::arg("useExternalIK")) + .def( + "enableDragAndDrop", + +[](dart::gui::osg::Viewer* self, + dart::dynamics::BodyNode* bodyNode, + bool useExternalIK, + bool useWholeBody) -> dart::gui::osg::BodyNodeDnD* { + return self->enableDragAndDrop( + bodyNode, useExternalIK, useWholeBody); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("bodyNode"), + ::pybind11::arg("useExternalIK"), + ::pybind11::arg("useWholeBody")) + .def( + "enableDragAndDrop", + +[](dart::gui::osg::Viewer* self, + dart::gui::osg::InteractiveFrame* frame) + -> dart::gui::osg::InteractiveFrameDnD* { + return self->enableDragAndDrop(frame); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("frame")) .def( "disableDragAndDrop", +[](dart::gui::osg::Viewer* self, dart::gui::osg::DragAndDrop* _dnd) diff --git a/python/dartpy/gui/osg/module.cpp b/python/dartpy/gui/osg/module.cpp index 9a6af7a2cd909..ef82b5662b4e7 100644 --- a/python/dartpy/gui/osg/module.cpp +++ b/python/dartpy/gui/osg/module.cpp @@ -42,6 +42,8 @@ void Viewer(pybind11::module& sm); void ViewerAttachment(pybind11::module& sm); void GridVisual(pybind11::module& sm); +void DragAndDrop(pybind11::module& sm); + void dart_gui_osg(pybind11::module& m) { auto sm = m.def_submodule("osg"); @@ -52,6 +54,8 @@ void dart_gui_osg(pybind11::module& m) Viewer(sm); ViewerAttachment(sm); GridVisual(sm); + + DragAndDrop(sm); } } // namespace python diff --git a/python/examples/operational_space_control/main.py b/python/examples/operational_space_control/main.py index a0cfb32709092..ea7fa441fb359 100644 --- a/python/examples/operational_space_control/main.py +++ b/python/examples/operational_space_control/main.py @@ -63,6 +63,12 @@ def main(): viewer = dart.gui.osg.Viewer() viewer.addWorldNode(node) + target = dart.dynamics.SimpleFrame() + world.addSimpleFrame(target) + viewer.enableDragAndDrop(target) + ee = kr5.getBodyNode('palm') + viewer.enableDragAndDrop(ee) + # Grid settings grid = dart.gui.osg.GridVisual() grid.setPlaneType(dart.gui.osg.GridVisual.PlaneType.ZX) From 88498b42a5ee2acb0226f4991606541f81825cf6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 20 May 2019 07:20:40 -0700 Subject: [PATCH 2/3] dartpy: Add shapes and ShapeFrame aspects --- python/dartpy/dynamics/Shape.cpp | 548 ++++++++++++++++++++++ python/dartpy/dynamics/ShapeFrame.cpp | 201 ++++++++ python/tests/unit/dynamics/test_aspect.py | 15 + 3 files changed, 764 insertions(+) create mode 100644 python/tests/unit/dynamics/test_aspect.py diff --git a/python/dartpy/dynamics/Shape.cpp b/python/dartpy/dynamics/Shape.cpp index b4f39f5b64bab..6e97481138068 100644 --- a/python/dartpy/dynamics/Shape.cpp +++ b/python/dartpy/dynamics/Shape.cpp @@ -421,6 +421,496 @@ void Shape(pybind11::module& m) .value("SHAPE_COLOR", dart::dynamics::MeshShape::ColorMode::SHAPE_COLOR) .export_values(); + ::pybind11::class_< + dart::dynamics::ArrowShape, + dart::dynamics::MeshShape, + std::shared_ptr>(m, "ArrowShape") + .def( + ::pybind11::init(), + ::pybind11::arg("tail"), + ::pybind11::arg("head")) + .def( + ::pybind11::init< + const Eigen::Vector3d&, + const Eigen::Vector3d&, + const dart::dynamics::ArrowShape::Properties&>(), + ::pybind11::arg("tail"), + ::pybind11::arg("head"), + ::pybind11::arg("properties")) + .def( + ::pybind11::init< + const Eigen::Vector3d&, + const Eigen::Vector3d&, + const dart::dynamics::ArrowShape::Properties&, + const Eigen::Vector4d&>(), + ::pybind11::arg("tail"), + ::pybind11::arg("head"), + ::pybind11::arg("properties"), + ::pybind11::arg("color")) + .def( + ::pybind11::init< + const Eigen::Vector3d&, + const Eigen::Vector3d&, + const dart::dynamics::ArrowShape::Properties&, + const Eigen::Vector4d&, + std::size_t>(), + ::pybind11::arg("tail"), + ::pybind11::arg("head"), + ::pybind11::arg("properties"), + ::pybind11::arg("color"), + ::pybind11::arg("resolution")) + .def( + "setPositions", + +[](dart::dynamics::ArrowShape* self, + const Eigen::Vector3d& _tail, + const Eigen::Vector3d& _head) { + self->setPositions(_tail, _head); + }, + ::pybind11::arg("tail"), + ::pybind11::arg("head")) + .def( + "getTail", + +[](const dart::dynamics::ArrowShape* self) + -> const Eigen::Vector3d& { return self->getTail(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getHead", + +[](const dart::dynamics::ArrowShape* self) + -> const Eigen::Vector3d& { return self->getHead(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setProperties", + +[](dart::dynamics::ArrowShape* self, + const dart::dynamics::ArrowShape::Properties& _properties) { + self->setProperties(_properties); + }, + ::pybind11::arg("properties")) + .def( + "notifyColorUpdated", + +[](dart::dynamics::ArrowShape* self, const Eigen::Vector4d& _color) { + self->notifyColorUpdated(_color); + }, + ::pybind11::arg("color")) + .def( + "getProperties", + +[](const dart::dynamics::ArrowShape* self) + -> const dart::dynamics::ArrowShape::Properties& { + return self->getProperties(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "configureArrow", + +[](dart::dynamics::ArrowShape* self, + const Eigen::Vector3d& _tail, + const Eigen::Vector3d& _head, + const dart::dynamics::ArrowShape::Properties& _properties) { + self->configureArrow(_tail, _head, _properties); + }, + ::pybind11::arg("tail"), + ::pybind11::arg("head"), + ::pybind11::arg("properties")); + + ::pybind11::class_( + m, "ArrowShapeProperties") + .def(::pybind11::init<>()) + .def(::pybind11::init(), ::pybind11::arg("radius")) + .def( + ::pybind11::init(), + ::pybind11::arg("radius"), + ::pybind11::arg("headRadiusScale")) + .def( + ::pybind11::init(), + ::pybind11::arg("radius"), + ::pybind11::arg("headRadiusScale"), + ::pybind11::arg("headLengthScale")) + .def( + ::pybind11::init(), + ::pybind11::arg("radius"), + ::pybind11::arg("headRadiusScale"), + ::pybind11::arg("headLengthScale"), + ::pybind11::arg("minHeadLength")) + .def( + ::pybind11::init(), + ::pybind11::arg("radius"), + ::pybind11::arg("headRadiusScale"), + ::pybind11::arg("headLengthScale"), + ::pybind11::arg("minHeadLength"), + ::pybind11::arg("maxHeadLength")) + .def( + ::pybind11::init(), + ::pybind11::arg("radius"), + ::pybind11::arg("headRadiusScale"), + ::pybind11::arg("headLengthScale"), + ::pybind11::arg("minHeadLength"), + ::pybind11::arg("maxHeadLength"), + ::pybind11::arg("doubleArrow")) + .def_readwrite( + "mRadius", &dart::dynamics::ArrowShape::Properties::mRadius) + .def_readwrite( + "mHeadRadiusScale", + &dart::dynamics::ArrowShape::Properties::mHeadRadiusScale) + .def_readwrite( + "mHeadLengthScale", + &dart::dynamics::ArrowShape::Properties::mHeadLengthScale) + .def_readwrite( + "mMinHeadLength", + &dart::dynamics::ArrowShape::Properties::mMinHeadLength) + .def_readwrite( + "mMaxHeadLength", + &dart::dynamics::ArrowShape::Properties::mMaxHeadLength) + .def_readwrite( + "mDoubleArrow", + &dart::dynamics::ArrowShape::Properties::mDoubleArrow); + + ::pybind11::class_< + dart::dynamics::PlaneShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "PlaneShape") + .def( + ::pybind11::init(), + ::pybind11::arg("normal"), + ::pybind11::arg("offset")) + .def( + ::pybind11::init(), + ::pybind11::arg("normal"), + ::pybind11::arg("point")) + .def( + "getType", + +[](const dart::dynamics::PlaneShape* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "computeInertia", + +[](const dart::dynamics::PlaneShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def( + "setNormal", + +[](dart::dynamics::PlaneShape* self, + const Eigen::Vector3d& _normal) { self->setNormal(_normal); }, + ::pybind11::arg("normal")) + .def( + "getNormal", + +[](const dart::dynamics::PlaneShape* self) + -> const Eigen::Vector3d& { return self->getNormal(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setOffset", + +[](dart::dynamics::PlaneShape* self, double _offset) { + self->setOffset(_offset); + }, + ::pybind11::arg("offset")) + .def( + "getOffset", + +[](const dart::dynamics::PlaneShape* self) -> double { + return self->getOffset(); + }) + .def( + "setNormalAndOffset", + +[](dart::dynamics::PlaneShape* self, + const Eigen::Vector3d& _normal, + double _offset) { self->setNormalAndOffset(_normal, _offset); }, + ::pybind11::arg("normal"), + ::pybind11::arg("offset")) + .def( + "setNormalAndPoint", + +[](dart::dynamics::PlaneShape* self, + const Eigen::Vector3d& _normal, + const Eigen::Vector3d& _point) { + self->setNormalAndPoint(_normal, _point); + }, + ::pybind11::arg("normal"), + ::pybind11::arg("point")) + .def( + "computeDistance", + +[](const dart::dynamics::PlaneShape* self, + const Eigen::Vector3d& _point) -> double { + return self->computeDistance(_point); + }, + ::pybind11::arg("point")) + .def( + "computeSignedDistance", + +[](const dart::dynamics::PlaneShape* self, + const Eigen::Vector3d& _point) -> double { + return self->computeSignedDistance(_point); + }, + ::pybind11::arg("point")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::PlaneShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); + + ::pybind11::class_< + dart::dynamics::SphereShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "SphereShape") + .def(::pybind11::init(), ::pybind11::arg("radius")) + .def( + "getType", + +[](const dart::dynamics::SphereShape* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setRadius", + +[](dart::dynamics::SphereShape* self, double radius) { + self->setRadius(radius); + }, + ::pybind11::arg("radius")) + .def( + "getRadius", + +[](const dart::dynamics::SphereShape* self) -> double { + return self->getRadius(); + }) + .def( + "computeInertia", + +[](const dart::dynamics::SphereShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::SphereShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def_static( + "computeVolumeOf", + +[](double radius) -> double { + return dart::dynamics::SphereShape::computeVolume(radius); + }, + ::pybind11::arg("radius")) + .def_static( + "computeInertiaOf", + +[](double radius, double mass) -> Eigen::Matrix3d { + return dart::dynamics::SphereShape::computeInertia(radius, mass); + }, + ::pybind11::arg("radius"), + ::pybind11::arg("mass")); + + ::pybind11::class_< + dart::dynamics::CapsuleShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "CapsuleShape") + .def( + ::pybind11::init(), + ::pybind11::arg("radius"), + ::pybind11::arg("height")) + .def( + "getType", + +[](const dart::dynamics::CapsuleShape* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getRadius", + +[](const dart::dynamics::CapsuleShape* self) -> double { + return self->getRadius(); + }) + .def( + "setRadius", + +[](dart::dynamics::CapsuleShape* self, double radius) { + self->setRadius(radius); + }, + ::pybind11::arg("radius")) + .def( + "getHeight", + +[](const dart::dynamics::CapsuleShape* self) -> double { + return self->getHeight(); + }) + .def( + "setHeight", + +[](dart::dynamics::CapsuleShape* self, double height) { + self->setHeight(height); + }, + ::pybind11::arg("height")) + .def( + "computeInertia", + +[](const dart::dynamics::CapsuleShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::CapsuleShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def_static( + "computeVolumeOf", + +[](double radius, double height) -> double { + return dart::dynamics::CapsuleShape::computeVolume(radius, height); + }, + ::pybind11::arg("radius"), + ::pybind11::arg("height")) + .def_static( + "computeInertiaOf", + +[](double radius, double height, double mass) -> Eigen::Matrix3d { + return dart::dynamics::CapsuleShape::computeInertia( + radius, height, mass); + }, + ::pybind11::arg("radius"), + ::pybind11::arg("height"), + ::pybind11::arg("mass")); + + ::pybind11::class_< + dart::dynamics::CylinderShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "CylinderShape") + .def( + ::pybind11::init(), + ::pybind11::arg("radius"), + ::pybind11::arg("height")) + .def( + "getType", + +[](const dart::dynamics::CylinderShape* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getRadius", + +[](const dart::dynamics::CylinderShape* self) -> double { + return self->getRadius(); + }) + .def( + "setRadius", + +[](dart::dynamics::CylinderShape* self, double _radius) { + self->setRadius(_radius); + }, + ::pybind11::arg("radius")) + .def( + "getHeight", + +[](const dart::dynamics::CylinderShape* self) -> double { + return self->getHeight(); + }) + .def( + "setHeight", + +[](dart::dynamics::CylinderShape* self, double _height) { + self->setHeight(_height); + }, + ::pybind11::arg("height")) + .def( + "computeInertia", + +[](const dart::dynamics::CylinderShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::CylinderShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def_static( + "computeVolumeOf", + +[](double radius, double height) -> double { + return dart::dynamics::CylinderShape::computeVolume(radius, height); + }, + ::pybind11::arg("radius"), + ::pybind11::arg("height")) + .def_static( + "computeInertiaOf", + +[](double radius, double height, double mass) -> Eigen::Matrix3d { + return dart::dynamics::CylinderShape::computeInertia( + radius, height, mass); + }, + ::pybind11::arg("radius"), + ::pybind11::arg("height"), + ::pybind11::arg("mass")); + + ::pybind11::class_< + dart::dynamics::SoftMeshShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "SoftMeshShape") + .def( + ::pybind11::init(), + ::pybind11::arg("softBodyNode")) + .def( + "getType", + +[](const dart::dynamics::SoftMeshShape* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "update", + +[](dart::dynamics::SoftMeshShape* self) { self->update(); }) + .def( + "computeInertia", + +[](const dart::dynamics::SoftMeshShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::SoftMeshShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); + + ::pybind11::class_< + dart::dynamics::EllipsoidShape, + dart::dynamics::Shape, + std::shared_ptr>(m, "EllipsoidShape") + .def( + ::pybind11::init(), + ::pybind11::arg("diameters")) + .def( + "getType", + +[](const dart::dynamics::EllipsoidShape* self) + -> const std::string& { return self->getType(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setDiameters", + +[](dart::dynamics::EllipsoidShape* self, + const Eigen::Vector3d& diameters) { + self->setDiameters(diameters); + }, + ::pybind11::arg("diameters")) + .def( + "getDiameters", + +[](const dart::dynamics::EllipsoidShape* self) + -> const Eigen::Vector3d& { return self->getDiameters(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setRadii", + +[](dart::dynamics::EllipsoidShape* self, + const Eigen::Vector3d& radii) { self->setRadii(radii); }, + ::pybind11::arg("radii")) + .def( + "getRadii", + +[](const dart::dynamics::EllipsoidShape* self) + -> const Eigen::Vector3d { return self->getRadii(); }) + .def( + "computeInertia", + +[](const dart::dynamics::EllipsoidShape* self, double mass) + -> Eigen::Matrix3d { return self->computeInertia(mass); }, + ::pybind11::arg("mass")) + .def( + "isSphere", + +[](const dart::dynamics::EllipsoidShape* self) -> bool { + return self->isSphere(); + }) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::EllipsoidShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def_static( + "computeVolumeOf", + +[](const Eigen::Vector3d& diameters) -> double { + return dart::dynamics::EllipsoidShape::computeVolume(diameters); + }, + ::pybind11::arg("diameters")) + .def_static( + "computeInertiaOf", + +[](const Eigen::Vector3d& diameters, + double mass) -> Eigen::Matrix3d { + return dart::dynamics::EllipsoidShape::computeInertia( + diameters, mass); + }, + ::pybind11::arg("diameters"), + ::pybind11::arg("mass")); + ::pybind11::class_< dart::dynamics::LineSegmentShape, dart::dynamics::Shape, @@ -520,6 +1010,64 @@ void Shape(pybind11::module& m) return dart::dynamics::LineSegmentShape::getStaticType(); }, ::pybind11::return_value_policy::reference_internal); + + ::pybind11::class_< + dart::dynamics::MultiSphereConvexHullShape, + dart::dynamics::Shape, + std::shared_ptr>( + m, "MultiSphereConvexHullShape") + .def( + ::pybind11::init< + const dart::dynamics::MultiSphereConvexHullShape::Spheres&>(), + ::pybind11::arg("spheres")) + .def( + "getType", + +[](const dart::dynamics::MultiSphereConvexHullShape* self) + -> const std::string& { return self->getType(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "addSpheres", + +[](dart::dynamics::MultiSphereConvexHullShape* self, + const dart::dynamics::MultiSphereConvexHullShape::Spheres& + spheres) { self->addSpheres(spheres); }, + ::pybind11::arg("spheres")) + .def( + "addSphere", + +[](dart::dynamics::MultiSphereConvexHullShape* self, + const dart::dynamics::MultiSphereConvexHullShape::Sphere& + sphere) { self->addSphere(sphere); }, + ::pybind11::arg("sphere")) + .def( + "addSphere", + +[](dart::dynamics::MultiSphereConvexHullShape* self, + double radius, + const Eigen::Vector3d& position) { + self->addSphere(radius, position); + }, + ::pybind11::arg("radius"), + ::pybind11::arg("position")) + .def( + "removeAllSpheres", + +[](dart::dynamics::MultiSphereConvexHullShape* self) { + self->removeAllSpheres(); + }) + .def( + "getNumSpheres", + +[](const dart::dynamics::MultiSphereConvexHullShape* self) + -> std::size_t { return self->getNumSpheres(); }) + .def( + "computeInertia", + +[](const dart::dynamics::MultiSphereConvexHullShape* self, + double mass) -> Eigen::Matrix3d { + return self->computeInertia(mass); + }, + ::pybind11::arg("mass")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::MultiSphereConvexHullShape::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); } } // namespace python diff --git a/python/dartpy/dynamics/ShapeFrame.cpp b/python/dartpy/dynamics/ShapeFrame.cpp index bb9358df4fd91..6aceeb2e9e168 100644 --- a/python/dartpy/dynamics/ShapeFrame.cpp +++ b/python/dartpy/dynamics/ShapeFrame.cpp @@ -89,6 +89,21 @@ void ShapeFrame(pybind11::module& m) +[](const dart::dynamics::ShapeFrame* self) -> bool { return self->hasVisualAspect(); }) + .def( + "getVisualAspect", + +[](dart::dynamics::ShapeFrame* self) + -> dart::dynamics::VisualAspect* { + return self->getVisualAspect(); + }, + pybind11::return_value_policy::reference_internal) + .def( + "getVisualAspect", + +[](dart::dynamics::ShapeFrame* self, + bool createIfNull) -> dart::dynamics::VisualAspect* { + return self->getVisualAspect(createIfNull); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("createIfNull")) .def( "setVisualAspect", +[](dart::dynamics::ShapeFrame* self, @@ -96,6 +111,13 @@ void ShapeFrame(pybind11::module& m) self->setVisualAspect(aspect); }, ::pybind11::arg("aspect")) + .def( + "createVisualAspect", + +[](dart::dynamics::ShapeFrame* self) + -> dart::dynamics::VisualAspect* { + return self->createVisualAspect(); + }, + pybind11::return_value_policy::reference_internal) .def( "removeVisualAspect", +[](dart::dynamics::ShapeFrame* self) { self->removeVisualAspect(); }) @@ -110,6 +132,35 @@ void ShapeFrame(pybind11::module& m) +[](const dart::dynamics::ShapeFrame* self) -> bool { return self->hasCollisionAspect(); }) + .def( + "getCollisionAspect", + +[](dart::dynamics::ShapeFrame* self) + -> dart::dynamics::CollisionAspect* { + return self->getCollisionAspect(); + }, + pybind11::return_value_policy::reference_internal) + .def( + "getCollisionAspect", + +[](dart::dynamics::ShapeFrame* self, + bool createIfNull) -> dart::dynamics::CollisionAspect* { + return self->getCollisionAspect(createIfNull); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("createIfNull")) + .def( + "setCollisionAspect", + +[](dart::dynamics::ShapeFrame* self, + const dart::dynamics::CollisionAspect* aspect) { + self->setCollisionAspect(aspect); + }, + ::pybind11::arg("aspect")) + .def( + "createCollisionAspect", + +[](dart::dynamics::ShapeFrame* self) + -> dart::dynamics::CollisionAspect* { + return self->createCollisionAspect(); + }, + pybind11::return_value_policy::reference_internal) .def( "setCollisionAspect", +[](dart::dynamics::ShapeFrame* self, @@ -133,6 +184,35 @@ void ShapeFrame(pybind11::module& m) +[](const dart::dynamics::ShapeFrame* self) -> bool { return self->hasDynamicsAspect(); }) + .def( + "getDynamicsAspect", + +[](dart::dynamics::ShapeFrame* self) + -> dart::dynamics::DynamicsAspect* { + return self->getDynamicsAspect(); + }, + pybind11::return_value_policy::reference_internal) + .def( + "getDynamicsAspect", + +[](dart::dynamics::ShapeFrame* self, + bool createIfNull) -> dart::dynamics::DynamicsAspect* { + return self->getDynamicsAspect(createIfNull); + }, + pybind11::return_value_policy::reference_internal, + ::pybind11::arg("createIfNull")) + .def( + "setDynamicsAspect", + +[](dart::dynamics::ShapeFrame* self, + const dart::dynamics::DynamicsAspect* aspect) { + self->setDynamicsAspect(aspect); + }, + ::pybind11::arg("aspect")) + .def( + "createDynamicsAspect", + +[](dart::dynamics::ShapeFrame* self) + -> dart::dynamics::DynamicsAspect* { + return self->createDynamicsAspect(); + }, + pybind11::return_value_policy::reference_internal) .def( "setDynamicsAspect", +[](dart::dynamics::ShapeFrame* self, @@ -154,6 +234,127 @@ void ShapeFrame(pybind11::module& m) .def("isShapeNode", +[](const dart::dynamics::ShapeFrame* self) -> bool { return self->isShapeNode(); }); + + ::pybind11::class_(m, "VisualAspect") + .def(::pybind11::init<>()) + .def( + ::pybind11::init< + const dart::common::detail::AspectWithVersionedProperties< + dart::common::CompositeTrackingAspect< + dart::dynamics::ShapeFrame>, + dart::dynamics::VisualAspect, + dart::dynamics::detail::VisualAspectProperties, + dart::dynamics::ShapeFrame, + &dart::common::detail::NoOp>::PropertiesData&>(), + ::pybind11::arg("properties")) + .def( + "setRGBA", + +[](dart::dynamics::VisualAspect* self, + const Eigen::Vector4d& color) { self->setRGBA(color); }, + ::pybind11::arg("color")) + .def( + "setHidden", + +[](dart::dynamics::VisualAspect* self, const bool& value) { + self->setHidden(value); + }, + ::pybind11::arg("value")) + .def( + "setShadowed", + +[](dart::dynamics::VisualAspect* self, const bool& value) { + self->setShadowed(value); + }, + ::pybind11::arg("value")) + .def( + "setColor", + +[](dart::dynamics::VisualAspect* self, + const Eigen::Vector3d& color) { self->setColor(color); }, + ::pybind11::arg("color")) + .def( + "setColor", + +[](dart::dynamics::VisualAspect* self, + const Eigen::Vector4d& color) { self->setColor(color); }, + ::pybind11::arg("color")) + .def( + "setRGB", + +[](dart::dynamics::VisualAspect* self, const Eigen::Vector3d& rgb) { + self->setRGB(rgb); + }, + ::pybind11::arg("rgb")) + .def( + "setAlpha", + +[](dart::dynamics::VisualAspect* self, const double alpha) { + self->setAlpha(alpha); + }, + ::pybind11::arg("alpha")) + .def( + "getColor", + +[](const dart::dynamics::VisualAspect* self) -> Eigen::Vector3d { + return self->getColor(); + }) + .def( + "getRGB", + +[](const dart::dynamics::VisualAspect* self) -> Eigen::Vector3d { + return self->getRGB(); + }) + .def( + "getAlpha", + +[](const dart::dynamics::VisualAspect* self) -> double { + return self->getAlpha(); + }) + .def("hide", +[](dart::dynamics::VisualAspect* self) { self->hide(); }) + .def("show", +[](dart::dynamics::VisualAspect* self) { self->show(); }) + .def("isHidden", +[](const dart::dynamics::VisualAspect* self) -> bool { + return self->isHidden(); + }); + + ::pybind11::class_(m, "CollisionAspect") + .def(::pybind11::init<>()) + .def( + ::pybind11::init< + const dart::common::detail::AspectWithVersionedProperties< + dart::common::CompositeTrackingAspect< + dart::dynamics::ShapeFrame>, + dart::dynamics::CollisionAspect, + dart::dynamics::detail::CollisionAspectProperties, + dart::dynamics::ShapeFrame, + &dart::common::detail::NoOp>::PropertiesData&>(), + ::pybind11::arg("properties")) + .def( + "setCollidable", + +[](dart::dynamics::CollisionAspect* self, const bool& value) { + self->setCollidable(value); + }, + ::pybind11::arg("value")) + .def( + "isCollidable", + +[](const dart::dynamics::CollisionAspect* self) -> bool { + return self->isCollidable(); + }); + + ::pybind11::class_(m, "DynamicsAspect") + .def(::pybind11::init<>()) + .def( + ::pybind11::init< + const dart::common::detail::AspectWithVersionedProperties< + dart::common::CompositeTrackingAspect< + dart::dynamics::ShapeFrame>, + dart::dynamics::DynamicsAspect, + dart::dynamics::detail::DynamicsAspectProperties, + dart::dynamics::ShapeFrame, + &dart::common::detail::NoOp>::PropertiesData&>(), + ::pybind11::arg("properties")) + .def( + "setFrictionCoeff", + +[](dart::dynamics::DynamicsAspect* self, const double& value) { + self->setFrictionCoeff(value); + }, + ::pybind11::arg("value")) + .def( + "setRestitutionCoeff", + +[](dart::dynamics::DynamicsAspect* self, const double& value) { + self->setRestitutionCoeff(value); + }, + ::pybind11::arg("value")); } } // namespace python diff --git a/python/tests/unit/dynamics/test_aspect.py b/python/tests/unit/dynamics/test_aspect.py new file mode 100644 index 0000000000000..3baf56edd8603 --- /dev/null +++ b/python/tests/unit/dynamics/test_aspect.py @@ -0,0 +1,15 @@ +import pytest +import dartpy as dart + + +def test_simple_frame(): + shape_frame = dart.dynamics.SimpleFrame() + assert not shape_frame.hasVisualAspect() + assert shape_frame.getVisualAspect() is None + assert shape_frame.getVisualAspect(False) is None + visual = shape_frame.createVisualAspect() + assert visual is not None + + +if __name__ == "__main__": + pytest.main() From ad602e54628ba2c37dbc55e7e4da07529f86ecfc Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 20 May 2019 07:21:38 -0700 Subject: [PATCH 3/3] Update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 97dd185ad7566..6b7c2e7a59ff1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -43,6 +43,7 @@ * Added optimizer APIs with GradientDescentSolver and NloptSolver: [#1325](https://github.com/dartsim/dart/pull/1325) * Added SimpleFrame: [#1326](https://github.com/dartsim/dart/pull/1326) * Added basic inverse kinematics APIs: [#1327](https://github.com/dartsim/dart/pull/1327) + * Added shapes and ShapeFrame aspects: [#1328](https://github.com/dartsim/dart/pull/1328) ### [DART 6.8.4 (2019-05-03)](https://github.com/dartsim/dart/milestone/56?closed=1)