diff --git a/CHANGELOG.md b/CHANGELOG.md index c6175ca403035..b5d497c3bb6b6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -38,6 +38,7 @@ * Added grid visual: [#1318](https://github.com/dartsim/dart/pull/1318) * Added ReferentialSkeleton, Linkage, and Chain: [#1321](https://github.com/dartsim/dart/pull/1321) * Enabled WorldNode classes to overload virtual functions in Python: [#1322](https://github.com/dartsim/dart/pull/1322) + * Added JacobianNode and operational space controller example: [#1323](https://github.com/dartsim/dart/pull/1323) ### [DART 6.8.4 (2019-05-03)](https://github.com/dartsim/dart/milestone/56?closed=1) diff --git a/python/dartpy/common/module.cpp b/python/dartpy/common/module.cpp index 2c9005d8d2f15..a79d4017598a1 100644 --- a/python/dartpy/common/module.cpp +++ b/python/dartpy/common/module.cpp @@ -35,14 +35,14 @@ namespace dart { namespace python { -// void Subject(pybind11::module& sm); +void Subject(pybind11::module& sm); void Uri(pybind11::module& sm); void dart_common(pybind11::module& m) { auto sm = m.def_submodule("common"); - // Subject(sm); + Subject(sm); Uri(sm); } diff --git a/python/dartpy/dynamics/BodyNode.cpp b/python/dartpy/dynamics/BodyNode.cpp index e2f908180d469..205ce2800b8f0 100644 --- a/python/dartpy/dynamics/BodyNode.cpp +++ b/python/dartpy/dynamics/BodyNode.cpp @@ -35,13 +35,215 @@ #include "eigen_geometry_pybind.h" #include "eigen_pybind.h" +PYBIND11_DECLARE_HOLDER_TYPE(T, dart::dynamics::TemplateBodyNodePtr, true); + namespace dart { namespace python { void BodyNode(pybind11::module& m) { - ::pybind11::class_( - m, "BodyNode") + ::pybind11::class_< + dart::dynamics::TemplatedJacobianNode, + dart::dynamics::JacobianNode, + std::shared_ptr< + dart::dynamics::TemplatedJacobianNode>>( + m, "TemplatedJacobianBodyNode") + .def( + "getJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobian(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset) -> dart::math::Jacobian { + return self->getJacobian(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobian(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getWorldJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset) -> dart::math::Jacobian { + return self->getWorldJacobian(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getLinearJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self) -> dart::math::LinearJacobian { + return self->getLinearJacobian(); + }) + .def( + "getLinearJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::LinearJacobian { + return self->getLinearJacobian(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getLinearJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset) -> dart::math::LinearJacobian { + return self->getLinearJacobian(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getLinearJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::LinearJacobian { + return self->getLinearJacobian(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getAngularJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self) -> dart::math::AngularJacobian { + return self->getAngularJacobian(); + }) + .def( + "getAngularJacobian", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::AngularJacobian { + return self->getAngularJacobian(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobianSpatialDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobianSpatialDeriv(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobianSpatialDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset) -> dart::math::Jacobian { + return self->getJacobianSpatialDeriv(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getJacobianSpatialDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobianSpatialDeriv(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobianClassicDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobianClassicDeriv(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobianClassicDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset) -> dart::math::Jacobian { + return self->getJacobianClassicDeriv(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getJacobianClassicDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobianClassicDeriv(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getLinearJacobianDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self) -> dart::math::LinearJacobian { + return self->getLinearJacobianDeriv(); + }) + .def( + "getLinearJacobianDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::LinearJacobian { + return self->getLinearJacobianDeriv(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getLinearJacobianDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset) -> dart::math::LinearJacobian { + return self->getLinearJacobianDeriv(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getLinearJacobianDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::LinearJacobian { + return self->getLinearJacobianDeriv(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getAngularJacobianDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self) -> dart::math::AngularJacobian { + return self->getAngularJacobianDeriv(); + }) + .def( + "getAngularJacobianDeriv", + +[](const dart::dynamics::TemplatedJacobianNode< + dart::dynamics::BodyNode>* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::AngularJacobian { + return self->getAngularJacobianDeriv(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")); + + ::pybind11::class_< + dart::dynamics::BodyNode, + dart::dynamics::TemplatedJacobianNode, + dart::dynamics::Frame, + dart::dynamics::BodyNodePtr>(m, "BodyNode") .def( "setAllNodeStates", +[](dart::dynamics::BodyNode* self, @@ -102,14 +304,14 @@ void BodyNode(pybind11::module& m) dart::dynamics::Node::State, std::default_delete< dart::dynamics::Node:: - State> > >, + State>>>, std::default_delete< dart::common::CloneableVector< std::unique_ptr< dart::dynamics::Node::State, std::default_delete< dart::dynamics::Node:: - State> > > > >, + State>>>>>, std::less, std::allocator > >, + State>>>, std::default_delete< dart::common::CloneableVector< std::unique_ptr< @@ -127,7 +329,7 @@ void BodyNode(pybind11::module& m) State, std::default_delete< dart::dynamics::Node:: - State> > > > > > > > >, + State>>>>>>>>>, &dart::dynamics::detail::setAllNodeStates, &dart::dynamics::detail::getAllNodeStates>, dart::common::ProxyCloneable< @@ -141,7 +343,7 @@ void BodyNode(pybind11::module& m) dart::dynamics::Node::Properties, std::default_delete< dart::dynamics::Node:: - Properties> > >, + Properties>>>, std::default_delete< dart::common::CloneableVector< std::unique_ptr< @@ -149,7 +351,7 @@ void BodyNode(pybind11::module& m) Properties, std::default_delete< dart::dynamics::Node:: - Properties> > > > >, + Properties>>>>>, std::less, std::allocator > >, + Properties>>>, std::default_delete< dart::common::CloneableVector< std::unique_ptr< @@ -168,10 +370,10 @@ void BodyNode(pybind11::module& m) Properties, std::default_delete< dart::dynamics::Node:: - Properties> > > > > > > > >, + Properties>>>>>>>>>, &dart::dynamics::detail::setAllNodeProperties, &dart::dynamics::detail:: - getAllNodeProperties> > > >::AspectState& + getAllNodeProperties>>>>::AspectState& state) { self->setAspectState(state); }, ::pybind11::arg("state")) .def( diff --git a/python/dartpy/dynamics/Entity.cpp b/python/dartpy/dynamics/Entity.cpp new file mode 100644 index 0000000000000..4cb5f87147d81 --- /dev/null +++ b/python/dartpy/dynamics/Entity.cpp @@ -0,0 +1,109 @@ +/* + * 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 Entity(pybind11::module& m) +{ + ::pybind11::class_< + dart::dynamics::Entity, + dart::common::Subject, + std::shared_ptr>(m, "Entity") + .def( + "setName", + +[](dart::dynamics::Entity* self, const std::string& name) + -> const std::string& { return self->setName(name); }, + ::pybind11::return_value_policy::reference_internal, + ::pybind11::arg("name")) + .def( + "getName", + +[](const dart::dynamics::Entity* self) -> const std::string& { + return self->getName(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "descendsFrom", + +[](const dart::dynamics::Entity* self, + const dart::dynamics::Frame* someFrame) -> bool { + return self->descendsFrom(someFrame); + }, + ::pybind11::arg("someFrame")) + .def( + "isFrame", + +[](const dart::dynamics::Entity* self) -> bool { + return self->isFrame(); + }) + .def( + "isQuiet", + +[](const dart::dynamics::Entity* self) -> bool { + return self->isQuiet(); + }) + // .def("notifyTransformUpdate", +[](dart::dynamics::Entity *self) { + // self->notifyTransformUpdate(); }) + .def( + "dirtyTransform", + +[](dart::dynamics::Entity* self) { self->dirtyTransform(); }) + .def( + "needsTransformUpdate", + +[](const dart::dynamics::Entity* self) -> bool { + return self->needsTransformUpdate(); + }) + // .def("notifyVelocityUpdate", +[](dart::dynamics::Entity *self) { + // self->notifyVelocityUpdate(); }) + .def( + "dirtyVelocity", + +[](dart::dynamics::Entity* self) { self->dirtyVelocity(); }) + .def( + "needsVelocityUpdate", + +[](const dart::dynamics::Entity* self) -> bool { + return self->needsVelocityUpdate(); + }) + // .def("notifyAccelerationUpdate", +[](dart::dynamics::Entity *self) { + // self->notifyAccelerationUpdate(); }) + .def( + "dirtyAcceleration", + +[](dart::dynamics::Entity* self) { self->dirtyAcceleration(); }) + .def( + "needsAccelerationUpdate", + +[](const dart::dynamics::Entity* self) -> bool { + return self->needsAccelerationUpdate(); + }); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/Frame.cpp b/python/dartpy/dynamics/Frame.cpp index 8b23c10a36fa2..bce61b9bc4ae2 100644 --- a/python/dartpy/dynamics/Frame.cpp +++ b/python/dartpy/dynamics/Frame.cpp @@ -40,18 +40,33 @@ namespace python { void Frame(pybind11::module& m) { - ::pybind11::class_(m, "Frame") + ::pybind11::class_< + dart::dynamics::Frame, + dart::dynamics::Entity, + std::shared_ptr >(m, "Frame") + .def( + "getRelativeTransform", + +[](const dart::dynamics::Frame* self) -> Eigen::Isometry3d { + return self->getRelativeTransform(); + }) + .def( + "getWorldTransform", + +[](const dart::dynamics::Frame* self) -> Eigen::Isometry3d { + return self->getWorldTransform(); + }) .def( "getTransform", +[](const dart::dynamics::Frame* self) -> Eigen::Isometry3d { + // std::cout << "[DEBUG] tf: \n" + // << self->getTransform().matrix() << + // std::endl; return self->getTransform(); }) .def( "getTransform", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _withRespectTo) - -> Eigen::Isometry3d { - return self->getTransform(_withRespectTo); + const dart::dynamics::Frame* withRespectTo) -> Eigen::Isometry3d { + return self->getTransform(withRespectTo); }, ::pybind11::arg("withRespectTo")) .def( @@ -64,30 +79,33 @@ void Frame(pybind11::module& m) }, ::pybind11::arg("withRespectTo"), ::pybind11::arg("inCoordinatesOf")) + .def( + "getSpatialVelocity", + +[](const dart::dynamics::Frame* self) -> Eigen::Vector6d { + return self->getSpatialVelocity(); + }) .def( "getSpatialVelocity", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector6d { - return self->getSpatialVelocity(_relativeTo, _inCoordinatesOf); + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector6d { + return self->getSpatialVelocity(relativeTo, inCoordinatesOf); }, ::pybind11::arg("relativeTo"), ::pybind11::arg("inCoordinatesOf")) .def( "getSpatialVelocity", - +[](const dart::dynamics::Frame* self, const Eigen::Vector3d& _offset) - -> Eigen::Vector6d { return self->getSpatialVelocity(_offset); }, + +[](const dart::dynamics::Frame* self, const Eigen::Vector3d& offset) + -> Eigen::Vector6d { return self->getSpatialVelocity(offset); }, ::pybind11::arg("offset")) .def( "getSpatialVelocity", +[](const dart::dynamics::Frame* self, - const Eigen::Vector3d& _offset, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector6d { + const Eigen::Vector3d& offset, + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector6d { return self->getSpatialVelocity( - _offset, _relativeTo, _inCoordinatesOf); + offset, relativeTo, inCoordinatesOf); }, ::pybind11::arg("offset"), ::pybind11::arg("relativeTo"), @@ -100,43 +118,40 @@ void Frame(pybind11::module& m) .def( "getLinearVelocity", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d { - return self->getLinearVelocity(_relativeTo); + const dart::dynamics::Frame* relativeTo) -> Eigen::Vector3d { + return self->getLinearVelocity(relativeTo); }, ::pybind11::arg("relativeTo")) .def( "getLinearVelocity", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector3d { - return self->getLinearVelocity(_relativeTo, _inCoordinatesOf); + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector3d { + return self->getLinearVelocity(relativeTo, inCoordinatesOf); }, ::pybind11::arg("relativeTo"), ::pybind11::arg("inCoordinatesOf")) .def( "getLinearVelocity", - +[](const dart::dynamics::Frame* self, const Eigen::Vector3d& _offset) - -> Eigen::Vector3d { return self->getLinearVelocity(_offset); }, + +[](const dart::dynamics::Frame* self, const Eigen::Vector3d& offset) + -> Eigen::Vector3d { return self->getLinearVelocity(offset); }, ::pybind11::arg("offset")) .def( "getLinearVelocity", +[](const dart::dynamics::Frame* self, - const Eigen::Vector3d& _offset, - const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d { - return self->getLinearVelocity(_offset, _relativeTo); + const Eigen::Vector3d& offset, + const dart::dynamics::Frame* relativeTo) -> Eigen::Vector3d { + return self->getLinearVelocity(offset, relativeTo); }, ::pybind11::arg("offset"), ::pybind11::arg("relativeTo")) .def( "getLinearVelocity", +[](const dart::dynamics::Frame* self, - const Eigen::Vector3d& _offset, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector3d { - return self->getLinearVelocity( - _offset, _relativeTo, _inCoordinatesOf); + const Eigen::Vector3d& offset, + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector3d { + return self->getLinearVelocity(offset, relativeTo, inCoordinatesOf); }, ::pybind11::arg("offset"), ::pybind11::arg("relativeTo"), @@ -149,46 +164,48 @@ void Frame(pybind11::module& m) .def( "getAngularVelocity", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d { - return self->getAngularVelocity(_relativeTo); + const dart::dynamics::Frame* relativeTo) -> Eigen::Vector3d { + return self->getAngularVelocity(relativeTo); }, ::pybind11::arg("relativeTo")) .def( "getAngularVelocity", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector3d { - return self->getAngularVelocity(_relativeTo, _inCoordinatesOf); + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector3d { + return self->getAngularVelocity(relativeTo, inCoordinatesOf); }, ::pybind11::arg("relativeTo"), ::pybind11::arg("inCoordinatesOf")) + .def( + "getSpatialAcceleration", + +[](const dart::dynamics::Frame* self) -> Eigen::Vector6d { + return self->getSpatialAcceleration(); + }) .def( "getSpatialAcceleration", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector6d { - return self->getSpatialAcceleration(_relativeTo, _inCoordinatesOf); + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector6d { + return self->getSpatialAcceleration(relativeTo, inCoordinatesOf); }, ::pybind11::arg("relativeTo"), ::pybind11::arg("inCoordinatesOf")) .def( "getSpatialAcceleration", +[](const dart::dynamics::Frame* self, - const Eigen::Vector3d& _offset) -> Eigen::Vector6d { - return self->getSpatialAcceleration(_offset); + const Eigen::Vector3d& offset) -> Eigen::Vector6d { + return self->getSpatialAcceleration(offset); }, ::pybind11::arg("offset")) .def( "getSpatialAcceleration", +[](const dart::dynamics::Frame* self, - const Eigen::Vector3d& _offset, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector6d { + const Eigen::Vector3d& offset, + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector6d { return self->getSpatialAcceleration( - _offset, _relativeTo, _inCoordinatesOf); + offset, relativeTo, inCoordinatesOf); }, ::pybind11::arg("offset"), ::pybind11::arg("relativeTo"), @@ -201,45 +218,43 @@ void Frame(pybind11::module& m) .def( "getLinearAcceleration", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d { - return self->getLinearAcceleration(_relativeTo); + const dart::dynamics::Frame* relativeTo) -> Eigen::Vector3d { + return self->getLinearAcceleration(relativeTo); }, ::pybind11::arg("relativeTo")) .def( "getLinearAcceleration", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector3d { - return self->getLinearAcceleration(_relativeTo, _inCoordinatesOf); + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector3d { + return self->getLinearAcceleration(relativeTo, inCoordinatesOf); }, ::pybind11::arg("relativeTo"), ::pybind11::arg("inCoordinatesOf")) .def( "getLinearAcceleration", +[](const dart::dynamics::Frame* self, - const Eigen::Vector3d& _offset) -> Eigen::Vector3d { - return self->getLinearAcceleration(_offset); + const Eigen::Vector3d& offset) -> Eigen::Vector3d { + return self->getLinearAcceleration(offset); }, ::pybind11::arg("offset")) .def( "getLinearAcceleration", +[](const dart::dynamics::Frame* self, - const Eigen::Vector3d& _offset, - const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d { - return self->getLinearAcceleration(_offset, _relativeTo); + const Eigen::Vector3d& offset, + const dart::dynamics::Frame* relativeTo) -> Eigen::Vector3d { + return self->getLinearAcceleration(offset, relativeTo); }, ::pybind11::arg("offset"), ::pybind11::arg("relativeTo")) .def( "getLinearAcceleration", +[](const dart::dynamics::Frame* self, - const Eigen::Vector3d& _offset, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector3d { + const Eigen::Vector3d& offset, + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector3d { return self->getLinearAcceleration( - _offset, _relativeTo, _inCoordinatesOf); + offset, relativeTo, inCoordinatesOf); }, ::pybind11::arg("offset"), ::pybind11::arg("relativeTo"), @@ -252,17 +267,16 @@ void Frame(pybind11::module& m) .def( "getAngularAcceleration", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d { - return self->getAngularAcceleration(_relativeTo); + const dart::dynamics::Frame* relativeTo) -> Eigen::Vector3d { + return self->getAngularAcceleration(relativeTo); }, ::pybind11::arg("relativeTo")) .def( "getAngularAcceleration", +[](const dart::dynamics::Frame* self, - const dart::dynamics::Frame* _relativeTo, - const dart::dynamics::Frame* _inCoordinatesOf) - -> Eigen::Vector3d { - return self->getAngularAcceleration(_relativeTo, _inCoordinatesOf); + const dart::dynamics::Frame* relativeTo, + const dart::dynamics::Frame* inCoordinatesOf) -> Eigen::Vector3d { + return self->getAngularAcceleration(relativeTo, inCoordinatesOf); }, ::pybind11::arg("relativeTo"), ::pybind11::arg("inCoordinatesOf")) diff --git a/python/dartpy/dynamics/JacobianNode.cpp b/python/dartpy/dynamics/JacobianNode.cpp new file mode 100644 index 0000000000000..7cc9ce469c4a7 --- /dev/null +++ b/python/dartpy/dynamics/JacobianNode.cpp @@ -0,0 +1,272 @@ +/* + * 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 JacobianNode(pybind11::module& m) +{ + ::pybind11::class_< + dart::dynamics::JacobianNode, + dart::dynamics::Frame, + dart::dynamics::Node, + std::shared_ptr>(m, "JacobianNode") + .def( + "getIK", + +[](const dart::dynamics::JacobianNode* self) + -> std::shared_ptr { + return self->getIK(); + }) + .def( + "clearIK", + +[](dart::dynamics::JacobianNode* self) { self->clearIK(); }) + .def( + "dependsOn", + +[](const dart::dynamics::JacobianNode* self, + std::size_t _genCoordIndex) -> bool { + return self->dependsOn(_genCoordIndex); + }, + ::pybind11::arg("_genCoordIndex")) + .def( + "getNumDependentGenCoords", + +[](const dart::dynamics::JacobianNode* self) -> std::size_t { + return self->getNumDependentGenCoords(); + }) + .def( + "getDependentGenCoordIndex", + +[](const dart::dynamics::JacobianNode* self, + std::size_t _arrayIndex) -> std::size_t { + return self->getDependentGenCoordIndex(_arrayIndex); + }, + ::pybind11::arg("_arrayIndex")) + .def( + "getNumDependentDofs", + +[](const dart::dynamics::JacobianNode* self) -> std::size_t { + return self->getNumDependentDofs(); + }) + .def( + "getChainDofs", + +[](const dart::dynamics::JacobianNode* self) + -> const std::vector { + return self->getChainDofs(); + }) + .def( + "getJacobian", + +[](const dart::dynamics::JacobianNode* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobian(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobian", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset) -> dart::math::Jacobian { + return self->getJacobian(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getJacobian", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobian(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getWorldJacobian", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset) -> dart::math::Jacobian { + return self->getWorldJacobian(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getLinearJacobian", + +[](const dart::dynamics::JacobianNode* self) + -> dart::math::LinearJacobian { + return self->getLinearJacobian(); + }) + .def( + "getLinearJacobian", + +[](const dart::dynamics::JacobianNode* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::LinearJacobian { + return self->getLinearJacobian(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getLinearJacobian", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset) -> dart::math::LinearJacobian { + return self->getLinearJacobian(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getLinearJacobian", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::LinearJacobian { + return self->getLinearJacobian(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getAngularJacobian", + +[](const dart::dynamics::JacobianNode* self) + -> dart::math::AngularJacobian { + return self->getAngularJacobian(); + }) + .def( + "getAngularJacobian", + +[](const dart::dynamics::JacobianNode* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::AngularJacobian { + return self->getAngularJacobian(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobianSpatialDeriv", + +[](const dart::dynamics::JacobianNode* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobianSpatialDeriv(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobianSpatialDeriv", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset) -> dart::math::Jacobian { + return self->getJacobianSpatialDeriv(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getJacobianSpatialDeriv", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobianSpatialDeriv(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobianClassicDeriv", + +[](const dart::dynamics::JacobianNode* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobianClassicDeriv(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getJacobianClassicDeriv", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset) -> dart::math::Jacobian { + return self->getJacobianClassicDeriv(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getJacobianClassicDeriv", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::Jacobian { + return self->getJacobianClassicDeriv(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getLinearJacobianDeriv", + +[](const dart::dynamics::JacobianNode* self) + -> dart::math::LinearJacobian { + return self->getLinearJacobianDeriv(); + }) + .def( + "getLinearJacobianDeriv", + +[](const dart::dynamics::JacobianNode* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::LinearJacobian { + return self->getLinearJacobianDeriv(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getLinearJacobianDeriv", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset) -> dart::math::LinearJacobian { + return self->getLinearJacobianDeriv(_offset); + }, + ::pybind11::arg("_offset")) + .def( + "getLinearJacobianDeriv", + +[](const dart::dynamics::JacobianNode* self, + const Eigen::Vector3d& _offset, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::LinearJacobian { + return self->getLinearJacobianDeriv(_offset, _inCoordinatesOf); + }, + ::pybind11::arg("_offset"), + ::pybind11::arg("_inCoordinatesOf")) + .def( + "getAngularJacobianDeriv", + +[](const dart::dynamics::JacobianNode* self) + -> dart::math::AngularJacobian { + return self->getAngularJacobianDeriv(); + }) + .def( + "getAngularJacobianDeriv", + +[](const dart::dynamics::JacobianNode* self, + const dart::dynamics::Frame* _inCoordinatesOf) + -> dart::math::AngularJacobian { + return self->getAngularJacobianDeriv(_inCoordinatesOf); + }, + ::pybind11::arg("_inCoordinatesOf")) + // .def("notifyJacobianUpdate", +[](dart::dynamics::JacobianNode *self) { + // self->notifyJacobianUpdate(); }) + .def( + "dirtyJacobian", + +[](dart::dynamics::JacobianNode* self) { self->dirtyJacobian(); }) + // .def("notifyJacobianDerivUpdate", +[](dart::dynamics::JacobianNode + // *self) { self->notifyJacobianDerivUpdate(); }) + .def("dirtyJacobianDeriv", +[](dart::dynamics::JacobianNode* self) { + self->dirtyJacobianDeriv(); + }); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/MetaSkeleton.cpp b/python/dartpy/dynamics/MetaSkeleton.cpp index acbc65840bac5..9ed602e6dc220 100644 --- a/python/dartpy/dynamics/MetaSkeleton.cpp +++ b/python/dartpy/dynamics/MetaSkeleton.cpp @@ -84,13 +84,13 @@ void MetaSkeleton(pybind11::module& m) +[](dart::dynamics::MetaSkeleton* self, std::size_t index) -> dart::dynamics::BodyNode* { return self->getBodyNode(index); }, ::pybind11::arg("index"), - pybind11::return_value_policy::reference_internal) + pybind11::return_value_policy::reference) .def( "getBodyNode", +[](dart::dynamics::MetaSkeleton* self, const std::string& name) -> dart::dynamics::BodyNode* { return self->getBodyNode(name); }, ::pybind11::arg("treeIndex"), - pybind11::return_value_policy::reference_internal) + pybind11::return_value_policy::reference) .def( "getBodyNodes", +[](dart::dynamics::MetaSkeleton* self, const std::string& name) @@ -1157,6 +1157,43 @@ void MetaSkeleton(pybind11::module& m) "getMass", +[](const dart::dynamics::MetaSkeleton* self) -> double { return self->getMass(); }) + .def( + "getMassMatrix", + +[](const dart::dynamics::MetaSkeleton* self) + -> const Eigen::MatrixXd& { return self->getMassMatrix(); }) + .def( + "getAugMassMatrix", + +[](const dart::dynamics::MetaSkeleton* self) + -> const Eigen::MatrixXd& { return self->getAugMassMatrix(); }) + .def( + "getInvMassMatrix", + +[](const dart::dynamics::MetaSkeleton* self) + -> const Eigen::MatrixXd& { return self->getInvMassMatrix(); }) + .def( + "getCoriolisForces", + +[](dart::dynamics::MetaSkeleton* self) -> const Eigen::VectorXd& { + return self->getCoriolisForces(); + }) + .def( + "getGravityForces", + +[](dart::dynamics::MetaSkeleton* self) -> const Eigen::VectorXd& { + return self->getCoriolisForces(); + }) + .def( + "getCoriolisAndGravityForces", + +[](dart::dynamics::MetaSkeleton* self) -> const Eigen::VectorXd& { + return self->getCoriolisAndGravityForces(); + }) + .def( + "getExternalForces", + +[](dart::dynamics::MetaSkeleton* self) -> const Eigen::VectorXd& { + return self->getCoriolisAndGravityForces(); + }) + .def( + "getConstraintForces", + +[](dart::dynamics::MetaSkeleton* self) -> const Eigen::VectorXd& { + return self->getConstraintForces(); + }) .def( "clearExternalForces", +[](dart::dynamics::MetaSkeleton* diff --git a/python/dartpy/dynamics/Node.cpp b/python/dartpy/dynamics/Node.cpp new file mode 100644 index 0000000000000..08983df6da4d4 --- /dev/null +++ b/python/dartpy/dynamics/Node.cpp @@ -0,0 +1,116 @@ +/* + * 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 Node(pybind11::module& m) +{ + ::pybind11::class_< + dart::dynamics::Node, + /*dart::common::VersionCounter,*/ dart::common::Subject, + std::shared_ptr >(m, "Node") + .def( + "setName", + +[](dart::dynamics::Node* self, const std::string& newName) + -> const std::string& { return self->setName(newName); }, + ::pybind11::return_value_policy::reference_internal, + ::pybind11::arg("newName")) + .def( + "getName", + +[](const dart::dynamics::Node* self) -> const std::string& { + return self->getName(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setNodeState", + +[](dart::dynamics::Node* self, + const dart::dynamics::Node::State& otherState) { + self->setNodeState(otherState); + }, + ::pybind11::arg("otherState")) + .def( + "getNodeState", + +[](const dart::dynamics::Node* self) + -> std::unique_ptr { + return self->getNodeState(); + }) + .def( + "setNodeProperties", + +[](dart::dynamics::Node* self, + const dart::dynamics::Node::Properties& properties) { + self->setNodeProperties(properties); + }, + ::pybind11::arg("properties")) + .def( + "getNodeProperties", + +[](const dart::dynamics::Node* self) + -> std::unique_ptr { + return self->getNodeProperties(); + }) + .def( + "getBodyNodePtr", + +[](dart::dynamics::Node* self) -> dart::dynamics::BodyNodePtr { + return self->getBodyNodePtr(); + }) + .def( + "getBodyNodePtr", + +[](const dart::dynamics::Node* self) + -> dart::dynamics::ConstBodyNodePtr { + return self->getBodyNodePtr(); + }) + .def( + "isRemoved", + +[](const dart::dynamics::Node* self) -> bool { + return self->isRemoved(); + }) + .def( + "getSkeleton", + +[](dart::dynamics::Node* self) + -> std::shared_ptr { + return self->getSkeleton(); + }) + .def( + "getSkeleton", + +[](const dart::dynamics::Node* self) + -> std::shared_ptr { + return self->getSkeleton(); + }); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/ReferentialSkeleton.cpp b/python/dartpy/dynamics/ReferentialSkeleton.cpp index e5cb1bbcdbea6..9a4cc751497ee 100644 --- a/python/dartpy/dynamics/ReferentialSkeleton.cpp +++ b/python/dartpy/dynamics/ReferentialSkeleton.cpp @@ -58,7 +58,7 @@ void ReferentialSkeleton(pybind11::module& m) return self->setName(_name); }, ::pybind11::return_value_policy::reference_internal, - ::pybind11::arg("_name")) + ::pybind11::arg("name")) .def( "getName", +[](const dart::dynamics::ReferentialSkeleton* self) @@ -110,7 +110,7 @@ void ReferentialSkeleton(pybind11::module& m) const dart::dynamics::BodyNode* _bn) -> std::size_t { return self->getIndexOf(_bn); }, - ::pybind11::arg("_bn")) + ::pybind11::arg("bn")) .def( "getIndexOf", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -118,8 +118,8 @@ void ReferentialSkeleton(pybind11::module& m) bool _warning) -> std::size_t { return self->getIndexOf(_bn, _warning); }, - ::pybind11::arg("_bn"), - ::pybind11::arg("_warning")) + ::pybind11::arg("bn"), + ::pybind11::arg("warning")) .def( "getNumJoints", +[](const dart::dynamics::ReferentialSkeleton* self) -> std::size_t { @@ -165,7 +165,7 @@ void ReferentialSkeleton(pybind11::module& m) const dart::dynamics::Joint* _joint) -> std::size_t { return self->getIndexOf(_joint); }, - ::pybind11::arg("_joint")) + ::pybind11::arg("joint")) .def( "getIndexOf", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -173,8 +173,8 @@ void ReferentialSkeleton(pybind11::module& m) bool _warning) -> std::size_t { return self->getIndexOf(_joint, _warning); }, - ::pybind11::arg("_joint"), - ::pybind11::arg("_warning")) + ::pybind11::arg("joint"), + ::pybind11::arg("warning")) .def( "getNumDofs", +[](const dart::dynamics::ReferentialSkeleton* self) -> std::size_t { @@ -192,7 +192,7 @@ void ReferentialSkeleton(pybind11::module& m) const dart::dynamics::DegreeOfFreedom* _dof) -> std::size_t { return self->getIndexOf(_dof); }, - ::pybind11::arg("_dof")) + ::pybind11::arg("dof")) .def( "getIndexOf", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -200,14 +200,14 @@ void ReferentialSkeleton(pybind11::module& m) bool _warning) -> std::size_t { return self->getIndexOf(_dof, _warning); }, - ::pybind11::arg("_dof"), - ::pybind11::arg("_warning")) + ::pybind11::arg("dof"), + ::pybind11::arg("warning")) .def( "getJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, const dart::dynamics::JacobianNode* _node) -> dart::math::Jacobian { return self->getJacobian(_node); }, - ::pybind11::arg("_node")) + ::pybind11::arg("node")) .def( "getJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -216,8 +216,8 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::Jacobian { return self->getJacobian(_node, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("inCoordinatesOf")) .def( "getJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -225,8 +225,8 @@ void ReferentialSkeleton(pybind11::module& m) const Eigen::Vector3d& _localOffset) -> dart::math::Jacobian { return self->getJacobian(_node, _localOffset); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset")) .def( "getJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -236,15 +236,15 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::Jacobian { return self->getJacobian(_node, _localOffset, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset"), + ::pybind11::arg("inCoordinatesOf")) .def( "getWorldJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, const dart::dynamics::JacobianNode* _node) -> dart::math::Jacobian { return self->getWorldJacobian(_node); }, - ::pybind11::arg("_node")) + ::pybind11::arg("node")) .def( "getWorldJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -252,8 +252,8 @@ void ReferentialSkeleton(pybind11::module& m) const Eigen::Vector3d& _localOffset) -> dart::math::Jacobian { return self->getWorldJacobian(_node, _localOffset); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset")) .def( "getLinearJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -261,7 +261,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::LinearJacobian { return self->getLinearJacobian(_node); }, - ::pybind11::arg("_node")) + ::pybind11::arg("node")) .def( "getLinearJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -270,8 +270,8 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::LinearJacobian { return self->getLinearJacobian(_node, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("inCoordinatesOf")) .def( "getLinearJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -280,8 +280,8 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::LinearJacobian { return self->getLinearJacobian(_node, _localOffset); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset")) .def( "getLinearJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -292,9 +292,9 @@ void ReferentialSkeleton(pybind11::module& m) return self->getLinearJacobian( _node, _localOffset, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset"), + ::pybind11::arg("inCoordinatesOf")) .def( "getAngularJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -302,7 +302,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::AngularJacobian { return self->getAngularJacobian(_node); }, - ::pybind11::arg("_node")) + ::pybind11::arg("node")) .def( "getAngularJacobian", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -311,8 +311,8 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::AngularJacobian { return self->getAngularJacobian(_node, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("inCoordinatesOf")) .def( "getJacobianSpatialDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -320,7 +320,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::Jacobian { return self->getJacobianSpatialDeriv(_node); }, - ::pybind11::arg("_node")) + ::pybind11::arg("node")) .def( "getJacobianSpatialDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -329,8 +329,8 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::Jacobian { return self->getJacobianSpatialDeriv(_node, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("inCoordinatesOf")) .def( "getJacobianSpatialDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -338,8 +338,8 @@ void ReferentialSkeleton(pybind11::module& m) const Eigen::Vector3d& _localOffset) -> dart::math::Jacobian { return self->getJacobianSpatialDeriv(_node, _localOffset); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset")) .def( "getJacobianSpatialDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -350,9 +350,9 @@ void ReferentialSkeleton(pybind11::module& m) return self->getJacobianSpatialDeriv( _node, _localOffset, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset"), + ::pybind11::arg("inCoordinatesOf")) .def( "getJacobianClassicDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -360,7 +360,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::Jacobian { return self->getJacobianClassicDeriv(_node); }, - ::pybind11::arg("_node")) + ::pybind11::arg("node")) .def( "getJacobianClassicDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -369,8 +369,8 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::Jacobian { return self->getJacobianClassicDeriv(_node, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("inCoordinatesOf")) .def( "getJacobianClassicDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -378,8 +378,8 @@ void ReferentialSkeleton(pybind11::module& m) const Eigen::Vector3d& _localOffset) -> dart::math::Jacobian { return self->getJacobianClassicDeriv(_node, _localOffset); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset")) .def( "getJacobianClassicDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -390,9 +390,9 @@ void ReferentialSkeleton(pybind11::module& m) return self->getJacobianClassicDeriv( _node, _localOffset, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset"), + ::pybind11::arg("inCoordinatesOf")) .def( "getLinearJacobianDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -400,7 +400,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::LinearJacobian { return self->getLinearJacobianDeriv(_node); }, - ::pybind11::arg("_node")) + ::pybind11::arg("node")) .def( "getLinearJacobianDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -409,8 +409,8 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::LinearJacobian { return self->getLinearJacobianDeriv(_node, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("inCoordinatesOf")) .def( "getLinearJacobianDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -419,8 +419,8 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::LinearJacobian { return self->getLinearJacobianDeriv(_node, _localOffset); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset")) .def( "getLinearJacobianDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -431,9 +431,9 @@ void ReferentialSkeleton(pybind11::module& m) return self->getLinearJacobianDeriv( _node, _localOffset, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_localOffset"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("localOffset"), + ::pybind11::arg("inCoordinatesOf")) .def( "getAngularJacobianDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -441,7 +441,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::AngularJacobian { return self->getAngularJacobianDeriv(_node); }, - ::pybind11::arg("_node")) + ::pybind11::arg("node")) .def( "getAngularJacobianDeriv", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -450,8 +450,8 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::AngularJacobian { return self->getAngularJacobianDeriv(_node, _inCoordinatesOf); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("node"), + ::pybind11::arg("inCoordinatesOf")) .def( "getMass", +[](const dart::dynamics::ReferentialSkeleton* self) -> double { @@ -492,7 +492,7 @@ void ReferentialSkeleton(pybind11::module& m) const dart::dynamics::Frame* _withRespectTo) -> Eigen::Vector3d { return self->getCOM(_withRespectTo); }, - ::pybind11::arg("_withRespectTo")) + ::pybind11::arg("withRespectTo")) .def( "getCOMSpatialVelocity", +[](const dart::dynamics::ReferentialSkeleton* self) @@ -503,7 +503,7 @@ void ReferentialSkeleton(pybind11::module& m) const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector6d { return self->getCOMSpatialVelocity(_relativeTo); }, - ::pybind11::arg("_relativeTo")) + ::pybind11::arg("relativeTo")) .def( "getCOMSpatialVelocity", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -512,8 +512,8 @@ void ReferentialSkeleton(pybind11::module& m) -> Eigen::Vector6d { return self->getCOMSpatialVelocity(_relativeTo, _inCoordinatesOf); }, - ::pybind11::arg("_relativeTo"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("relativeTo"), + ::pybind11::arg("inCoordinatesOf")) .def( "getCOMLinearVelocity", +[](const dart::dynamics::ReferentialSkeleton* self) @@ -524,7 +524,7 @@ void ReferentialSkeleton(pybind11::module& m) const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d { return self->getCOMLinearVelocity(_relativeTo); }, - ::pybind11::arg("_relativeTo")) + ::pybind11::arg("relativeTo")) .def( "getCOMLinearVelocity", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -533,8 +533,8 @@ void ReferentialSkeleton(pybind11::module& m) -> Eigen::Vector3d { return self->getCOMLinearVelocity(_relativeTo, _inCoordinatesOf); }, - ::pybind11::arg("_relativeTo"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("relativeTo"), + ::pybind11::arg("inCoordinatesOf")) .def( "getCOMSpatialAcceleration", +[](const dart::dynamics::ReferentialSkeleton* self) @@ -545,7 +545,7 @@ void ReferentialSkeleton(pybind11::module& m) const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector6d { return self->getCOMSpatialAcceleration(_relativeTo); }, - ::pybind11::arg("_relativeTo")) + ::pybind11::arg("relativeTo")) .def( "getCOMSpatialAcceleration", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -555,8 +555,8 @@ void ReferentialSkeleton(pybind11::module& m) return self->getCOMSpatialAcceleration( _relativeTo, _inCoordinatesOf); }, - ::pybind11::arg("_relativeTo"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("relativeTo"), + ::pybind11::arg("inCoordinatesOf")) .def( "getCOMLinearAcceleration", +[](const dart::dynamics::ReferentialSkeleton* self) @@ -567,7 +567,7 @@ void ReferentialSkeleton(pybind11::module& m) const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d { return self->getCOMLinearAcceleration(_relativeTo); }, - ::pybind11::arg("_relativeTo")) + ::pybind11::arg("relativeTo")) .def( "getCOMLinearAcceleration", +[](const dart::dynamics::ReferentialSkeleton* self, @@ -577,8 +577,8 @@ void ReferentialSkeleton(pybind11::module& m) return self->getCOMLinearAcceleration( _relativeTo, _inCoordinatesOf); }, - ::pybind11::arg("_relativeTo"), - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("relativeTo"), + ::pybind11::arg("inCoordinatesOf")) .def( "getCOMJacobian", +[](const dart::dynamics::ReferentialSkeleton* self) @@ -590,7 +590,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::Jacobian { return self->getCOMJacobian(_inCoordinatesOf); }, - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("inCoordinatesOf")) .def( "getCOMLinearJacobian", +[](const dart::dynamics::ReferentialSkeleton* self) @@ -604,7 +604,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::LinearJacobian { return self->getCOMLinearJacobian(_inCoordinatesOf); }, - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("inCoordinatesOf")) .def( "getCOMJacobianSpatialDeriv", +[](const dart::dynamics::ReferentialSkeleton* self) @@ -618,7 +618,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::Jacobian { return self->getCOMJacobianSpatialDeriv(_inCoordinatesOf); }, - ::pybind11::arg("_inCoordinatesOf")) + ::pybind11::arg("inCoordinatesOf")) .def( "getCOMLinearJacobianDeriv", +[](const dart::dynamics::ReferentialSkeleton* self) @@ -632,7 +632,7 @@ void ReferentialSkeleton(pybind11::module& m) -> dart::math::LinearJacobian { return self->getCOMLinearJacobianDeriv(_inCoordinatesOf); }, - ::pybind11::arg("_inCoordinatesOf")); + ::pybind11::arg("inCoordinatesOf")); } } // namespace python diff --git a/python/dartpy/dynamics/Skeleton.cpp b/python/dartpy/dynamics/Skeleton.cpp index 503d5f8599360..849a714eebe39 100644 --- a/python/dartpy/dynamics/Skeleton.cpp +++ b/python/dartpy/dynamics/Skeleton.cpp @@ -284,7 +284,7 @@ void Skeleton(pybind11::module& m) +[](dart::dynamics::Skeleton* self) -> dart::dynamics::BodyNode* { return self->getRootBodyNode(); }, - pybind11::return_value_policy::reference_internal) + pybind11::return_value_policy::reference) .def( "getRootBodyNode", +[](dart::dynamics::Skeleton* self, @@ -292,7 +292,7 @@ void Skeleton(pybind11::module& m) return self->getRootBodyNode(index); }, ::pybind11::arg("treeIndex"), - pybind11::return_value_policy::reference_internal) + pybind11::return_value_policy::reference) .def( "getRootJoint", +[](dart::dynamics::Skeleton* self) -> dart::dynamics::Joint* { @@ -919,6 +919,98 @@ void Skeleton(pybind11::module& m) "getMass", +[](const dart::dynamics::Skeleton* self) -> double { return self->getMass(); }) + .def( + "getMassMatrix", + +[](const dart::dynamics::Skeleton* self, + std::size_t treeIndex) -> const Eigen::MatrixXd& { + return self->getMassMatrix(treeIndex); + }) + // TODO(JS): Redefining get[~]() that are already defined in MetaSkeleton. + // We need this because the methods with same name (but different + // arguments) are hidden. Update (or remove) once following issue is + // resolved: https://github.com/pybind/pybind11/issues/974 + .def( + "getMassMatrix", + +[](const dart::dynamics::Skeleton* self) -> const Eigen::MatrixXd& { + return self->getMassMatrix(); + }) + .def( + "getAugMassMatrix", + +[](const dart::dynamics::Skeleton* self, + std::size_t treeIndex) -> const Eigen::MatrixXd& { + return self->getAugMassMatrix(treeIndex); + }) + .def( + "getAugMassMatrix", + +[](const dart::dynamics::Skeleton* self) -> const Eigen::MatrixXd& { + return self->getAugMassMatrix(); + }) + .def( + "getInvMassMatrix", + +[](const dart::dynamics::Skeleton* self, + std::size_t treeIndex) -> const Eigen::MatrixXd& { + return self->getInvMassMatrix(treeIndex); + }) + .def( + "getInvMassMatrix", + +[](const dart::dynamics::Skeleton* self) -> const Eigen::MatrixXd& { + return self->getInvMassMatrix(); + }) + .def( + "getCoriolisForces", + +[](dart::dynamics::Skeleton* self, + std::size_t treeIndex) -> const Eigen::VectorXd& { + return self->getCoriolisForces(treeIndex); + }) + .def( + "getCoriolisForces", + +[](dart::dynamics::Skeleton* self) -> const Eigen::VectorXd& { + return self->getCoriolisForces(); + }) + .def( + "getGravityForces", + +[](dart::dynamics::Skeleton* self, + std::size_t treeIndex) -> const Eigen::VectorXd& { + return self->getCoriolisForces(treeIndex); + }) + .def( + "getGravityForces", + +[](dart::dynamics::Skeleton* self) -> const Eigen::VectorXd& { + return self->getCoriolisForces(); + }) + .def( + "getCoriolisAndGravityForces", + +[](dart::dynamics::Skeleton* self, + std::size_t treeIndex) -> const Eigen::VectorXd& { + return self->getCoriolisAndGravityForces(treeIndex); + }) + .def( + "getCoriolisAndGravityForces", + +[](dart::dynamics::Skeleton* self) -> const Eigen::VectorXd& { + return self->getCoriolisAndGravityForces(); + }) + .def( + "getExternalForces", + +[](dart::dynamics::Skeleton* self, + std::size_t treeIndex) -> const Eigen::VectorXd& { + return self->getCoriolisAndGravityForces(treeIndex); + }) + .def( + "getExternalForces", + +[](dart::dynamics::Skeleton* self) -> const Eigen::VectorXd& { + return self->getCoriolisAndGravityForces(); + }) + .def( + "getConstraintForces", + +[](dart::dynamics::Skeleton* self, + std::size_t treeIndex) -> const Eigen::VectorXd& { + return self->getConstraintForces(treeIndex); + }) + .def( + "getConstraintForces", + +[](dart::dynamics::Skeleton* self) -> const Eigen::VectorXd& { + return self->getConstraintForces(); + }) .def( "clearExternalForces", +[](dart::dynamics::Skeleton* self) diff --git a/python/dartpy/dynamics/module.cpp b/python/dartpy/dynamics/module.cpp index 3673e3dc1332f..8480a8aeed4ff 100644 --- a/python/dartpy/dynamics/module.cpp +++ b/python/dartpy/dynamics/module.cpp @@ -35,7 +35,12 @@ namespace dart { namespace python { +void Entity(pybind11::module& sm); void Frame(pybind11::module& sm); + +void Node(pybind11::module& sm); +void JacobianNode(pybind11::module& sm); + void BodyNode(pybind11::module& sm); void Joint(pybind11::module& sm); @@ -50,7 +55,12 @@ void dart_dynamics(pybind11::module& m) { auto sm = m.def_submodule("dynamics"); + Entity(sm); Frame(sm); + + Node(sm); + JacobianNode(sm); + BodyNode(sm); Joint(sm); diff --git a/python/dartpy/gui/osg/Viewer.cpp b/python/dartpy/gui/osg/Viewer.cpp index 443ca4446c1c3..93f7d5e40d6d6 100644 --- a/python/dartpy/gui/osg/Viewer.cpp +++ b/python/dartpy/gui/osg/Viewer.cpp @@ -103,13 +103,13 @@ void Viewer(pybind11::module& m) +[](dart::gui::osg::Viewer* self, bool _on) { self->switchDefaultEventHandler(_on); }, - ::pybind11::arg("_on")) + ::pybind11::arg("on")) .def( "switchHeadlights", +[](dart::gui::osg::Viewer* self, bool _on) { self->switchHeadlights(_on); }, - ::pybind11::arg("_on")) + ::pybind11::arg("on")) .def( "checkHeadlights", +[](const dart::gui::osg::Viewer* self) -> bool { @@ -121,42 +121,42 @@ void Viewer(pybind11::module& m) dart::gui::osg::WorldNode* _newWorldNode) { self->addWorldNode(_newWorldNode); }, - ::pybind11::arg("_newWorldNode")) + ::pybind11::arg("newWorldNode")) .def( "addWorldNode", +[](dart::gui::osg::Viewer* self, dart::gui::osg::WorldNode* _newWorldNode, bool _active) { self->addWorldNode(_newWorldNode, _active); }, - ::pybind11::arg("_newWorldNode"), - ::pybind11::arg("_active")) + ::pybind11::arg("newWorldNode"), + ::pybind11::arg("active")) .def( "removeWorldNode", +[](dart::gui::osg::Viewer* self, dart::gui::osg::WorldNode* _oldWorldNode) { self->removeWorldNode(_oldWorldNode); }, - ::pybind11::arg("_oldWorldNode")) + ::pybind11::arg("oldWorldNode")) .def( "removeWorldNode", +[](dart::gui::osg::Viewer* self, std::shared_ptr _oldWorld) { self->removeWorldNode(_oldWorld); }, - ::pybind11::arg("_oldWorld")) + ::pybind11::arg("oldWorld")) .def( "addAttachment", +[](dart::gui::osg::Viewer* self, dart::gui::osg::ViewerAttachment* _attachment) { self->addAttachment(_attachment); }, - ::pybind11::arg("_attachment")) + ::pybind11::arg("attachment")) .def( "removeAttachment", +[](dart::gui::osg::Viewer* self, dart::gui::osg::ViewerAttachment* _attachment) { self->removeAttachment(_attachment); }, - ::pybind11::arg("_attachment")) + ::pybind11::arg("attachment")) .def( "setupDefaultLights", +[](dart::gui::osg::Viewer* self) { self->setupDefaultLights(); }) @@ -165,44 +165,44 @@ void Viewer(pybind11::module& m) +[](dart::gui::osg::Viewer* self, const osg::Vec3& _up) { self->setUpwardsDirection(_up); }, - ::pybind11::arg("_up")) + ::pybind11::arg("up")) .def( "setUpwardsDirection", +[](dart::gui::osg::Viewer* self, const Eigen::Vector3d& _up) { self->setUpwardsDirection(_up); }, - ::pybind11::arg("_up")) + ::pybind11::arg("up")) .def( "setWorldNodeActive", +[](dart::gui::osg::Viewer* self, dart::gui::osg::WorldNode* _node) { self->setWorldNodeActive(_node); }, - ::pybind11::arg("_node")) + ::pybind11::arg("node")) .def( "setWorldNodeActive", +[](dart::gui::osg::Viewer* self, dart::gui::osg::WorldNode* _node, bool _active) { self->setWorldNodeActive(_node, _active); }, - ::pybind11::arg("_node"), - ::pybind11::arg("_active")) + ::pybind11::arg("node"), + ::pybind11::arg("active")) .def( "setWorldNodeActive", +[](dart::gui::osg::Viewer* self, std::shared_ptr _world) { self->setWorldNodeActive(_world); }, - ::pybind11::arg("_world")) + ::pybind11::arg("world")) .def( "setWorldNodeActive", +[](dart::gui::osg::Viewer* self, std::shared_ptr _world, bool _active) { self->setWorldNodeActive(_world, _active); }, - ::pybind11::arg("_world"), - ::pybind11::arg("_active")) + ::pybind11::arg("world"), + ::pybind11::arg("active")) .def( "simulate", +[](dart::gui::osg::Viewer* self, bool _on) { self->simulate(_on); }, - ::pybind11::arg("_on")) + ::pybind11::arg("on")) .def( "isSimulating", +[](const dart::gui::osg::Viewer* self) -> bool { @@ -213,7 +213,7 @@ void Viewer(pybind11::module& m) +[](dart::gui::osg::Viewer* self, bool _allow) { self->allowSimulation(_allow); }, - ::pybind11::arg("_allow")) + ::pybind11::arg("allow")) .def( "isAllowingSimulation", +[](const dart::gui::osg::Viewer* self) -> bool { @@ -223,33 +223,33 @@ void Viewer(pybind11::module& m) "disableDragAndDrop", +[](dart::gui::osg::Viewer* self, dart::gui::osg::DragAndDrop* _dnd) -> bool { return self->disableDragAndDrop(_dnd); }, - ::pybind11::arg("_dnd")) + ::pybind11::arg("dnd")) .def( "disableDragAndDrop", +[](dart::gui::osg::Viewer* self, dart::gui::osg::SimpleFrameDnD* _dnd) -> bool { return self->disableDragAndDrop(_dnd); }, - ::pybind11::arg("_dnd")) + ::pybind11::arg("dnd")) .def( "disableDragAndDrop", +[](dart::gui::osg::Viewer* self, dart::gui::osg::SimpleFrameShapeDnD* _dnd) -> bool { return self->disableDragAndDrop(_dnd); }, - ::pybind11::arg("_dnd")) + ::pybind11::arg("dnd")) .def( "disableDragAndDrop", +[](dart::gui::osg::Viewer* self, dart::gui::osg::InteractiveFrameDnD* _dnd) -> bool { return self->disableDragAndDrop(_dnd); }, - ::pybind11::arg("_dnd")) + ::pybind11::arg("dnd")) .def( "disableDragAndDrop", +[](dart::gui::osg::Viewer* self, dart::gui::osg::BodyNodeDnD* _dnd) -> bool { return self->disableDragAndDrop(_dnd); }, - ::pybind11::arg("_dnd")) + ::pybind11::arg("dnd")) .def( "getInstructions", +[](const dart::gui::osg::Viewer* self) -> const std::string& { @@ -261,7 +261,7 @@ void Viewer(pybind11::module& m) +[](dart::gui::osg::Viewer* self, const std::string& _instruction) { self->addInstructionText(_instruction); }, - ::pybind11::arg("_instruction")) + ::pybind11::arg("instruction")) .def( "updateViewer", +[](dart::gui::osg::Viewer* self) { self->updateViewer(); }) diff --git a/python/examples/CMakeLists.txt b/python/examples/CMakeLists.txt index 699167f1ded1f..34d61ec0d006c 100644 --- a/python/examples/CMakeLists.txt +++ b/python/examples/CMakeLists.txt @@ -19,3 +19,4 @@ endfunction() add_subdirectory(hello_world) add_subdirectory(hello_world_gui) +add_subdirectory(operational_space_control) diff --git a/python/examples/operational_space_control/CMakeLists.txt b/python/examples/operational_space_control/CMakeLists.txt new file mode 100644 index 0000000000000..067d40b755b37 --- /dev/null +++ b/python/examples/operational_space_control/CMakeLists.txt @@ -0,0 +1 @@ +dartpy_add_example(py_operational_space_control main.py) diff --git a/python/examples/operational_space_control/main.py b/python/examples/operational_space_control/main.py new file mode 100644 index 0000000000000..86275f7f982b9 --- /dev/null +++ b/python/examples/operational_space_control/main.py @@ -0,0 +1,76 @@ +import numpy as np +import dartpy as dart + + +class HelloWorldNode(dart.gui.osg.RealTimeWorldNode): + def __init__(self, world, kr5, target_pose = [0.3, 0.3, 0.3]): + super(HelloWorldNode, self).__init__(world) + self.kr5 = kr5 + self.dofs = self.kr5.getNumDofs() + self.ee = kr5.getBodyNode('palm') + self.target_pose = target_pose + self.Kp = np.eye(3) * 50.0 + self.Kd = np.eye(self.dofs) * 5.0 + + def customPreStep(self): + M = self.kr5.getMassMatrix() + + J = self.ee.getLinearJacobian() + Jt = J.transpose(); + JJt = np.matmul(J, Jt) + kI = 0.0025 * np.eye(3) + invJ = np.matmul(Jt, np.linalg.inv(JJt + kI)) + + dJ = self.ee.getLinearJacobianDeriv() + dJt = dJ.transpose() + dJdJt = np.matmul(dJ, dJt) + invdJ = np.matmul(dJt, np.linalg.inv(dJdJt + kI)) + + e = self.target_pose - self.ee.getTransform().translation() + de = -self.ee.getLinearVelocity() + + cg = self.kr5.getCoriolisAndGravityForces() + + tmp1 = np.matmul(np.matmul(invJ, self.Kp), de) + tmp2 = np.matmul(np.matmul(invdJ, self.Kp), e) + + forces1 = np.matmul(M, tmp1 + tmp2) + forces2 = cg + forces3 = np.matmul(np.matmul(np.matmul(self.Kd, invJ), self.Kp), e) + + forces = forces1 + forces2 + forces3 + + self.kr5.setForces(forces) + + +def main(): + world = dart.simulation.World.create() + + urdfParser = dart.utils.DartLoader() + kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf") + ground = urdfParser.parseSkeleton("dart://sample/urdf/KR5/ground.urdf") + world.addSkeleton(kr5) + world.addSkeleton(ground) + world.setGravity([0, -9.81, 0]) + + node = HelloWorldNode(world, kr5, target_pose=[0.3, 0.3, 0.3]) + + # Create world node and add it to viewer + viewer = dart.gui.osg.Viewer() + 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() diff --git a/python/tests/CMakeLists.txt b/python/tests/CMakeLists.txt index b4711aea58686..44dc5c27a0368 100644 --- a/python/tests/CMakeLists.txt +++ b/python/tests/CMakeLists.txt @@ -23,13 +23,7 @@ set(dartpy_test_utils util.py ) -set(dartpy_test_files - unit/common/test_uri.py - unit/math/test_random.py - unit/dynamics/test_meta_skeleton.py - unit/simulation/test_world.py - unit/utils/test_dart_loader.py -) +file(GLOB_RECURSE dartpy_test_files "test_*.py") # Add custom target to run the tests add_custom_target(pytest diff --git a/python/tests/unit/dynamics/test_body_node.py b/python/tests/unit/dynamics/test_body_node.py new file mode 100644 index 0000000000000..942b0a803d348 --- /dev/null +++ b/python/tests/unit/dynamics/test_body_node.py @@ -0,0 +1,18 @@ +import platform +import pytest +import numpy as np +import dartpy as dart + + +def test_basic(): + urdfParser = dart.utils.DartLoader() + kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf") + assert kr5 is not None + + for i in range(kr5.getNumBodyNodes()): + body = kr5.getBodyNode(i) + assert np.array_equal(np.array(body.getSpatialVelocity()), np.zeros(6)) is True + + +if __name__ == "__main__": + pytest.main()