diff --git a/CHANGELOG.md b/CHANGELOG.md index 3008efee35e5e..bcd1109d1671a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -50,6 +50,7 @@ * Added DegreeOfFreedom and ShapeNode: [#1332](https://github.com/dartsim/dart/pull/1332) * Added constraint APIs: [#1333](https://github.com/dartsim/dart/pull/1333) * Added BallJoint, RevoluteJoint, joint properties, and chain tutorial (incomplete): [#1335](https://github.com/dartsim/dart/pull/1335) + * Added all the joints: [#1337](https://github.com/dartsim/dart/pull/1337) ### [DART 6.8.4 (2019-05-03)](https://github.com/dartsim/dart/milestone/56?closed=1) diff --git a/python/dartpy/dynamics/BodyNode.cpp b/python/dartpy/dynamics/BodyNode.cpp index 71b2cdea2e8ba..701c63489d344 100644 --- a/python/dartpy/dynamics/BodyNode.cpp +++ b/python/dartpy/dynamics/BodyNode.cpp @@ -38,6 +38,46 @@ PYBIND11_DECLARE_HOLDER_TYPE(T, dart::dynamics::TemplateBodyNodePtr, true); +#define DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(joint_type) \ + .def( \ + "create" #joint_type "AndBodyNodePair", \ + +[](dart::dynamics::BodyNode* self) \ + -> std:: \ + pair { \ + return self->createChildJointAndBodyNodePair< \ + dart::dynamics::joint_type, \ + dart::dynamics::BodyNode>(); \ + }, \ + ::pybind11::return_value_policy::reference_internal) \ + .def( \ + "create" #joint_type "AndBodyNodePair", \ + +[](dart::dynamics::BodyNode* self, \ + const dart::dynamics::joint_type::Properties& jointProperties) \ + -> std::pair< \ + dart::dynamics::joint_type*, \ + dart::dynamics::BodyNode*> { \ + return self->createChildJointAndBodyNodePair< \ + dart::dynamics::joint_type, \ + dart::dynamics::BodyNode>(jointProperties); \ + }, \ + ::pybind11::return_value_policy::reference_internal, \ + ::pybind11::arg("jointProperties")) \ + .def( \ + "create" #joint_type "AndBodyNodePair", \ + +[](dart::dynamics::BodyNode* self, \ + const dart::dynamics::joint_type::Properties& jointProperties, \ + const dart::dynamics::BodyNode::Properties& bodyProperties) \ + -> std::pair< \ + dart::dynamics::joint_type*, \ + dart::dynamics::BodyNode*> { \ + return self->createChildJointAndBodyNodePair< \ + dart::dynamics::joint_type, \ + dart::dynamics::BodyNode>(jointProperties, bodyProperties); \ + }, \ + ::pybind11::return_value_policy::reference_internal, \ + ::pybind11::arg("jointProperties"), \ + ::pybind11::arg("bodyProperties")) + namespace dart { namespace python { @@ -863,6 +903,19 @@ void BodyNode(pybind11::module& m) return self->getParentBodyNode(); }, ::pybind11::return_value_policy::reference_internal) + // clang-format off + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(WeldJoint) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(RevoluteJoint) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(PrismaticJoint) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(ScrewJoint) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(UniversalJoint) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(TranslationalJoint2D) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(PlanarJoint) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(EulerJoint) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(BallJoint) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(TranslationalJoint) + DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(FreeJoint) + // clang-format on .def( "getNumChildBodyNodes", +[](const dart::dynamics::BodyNode* self) -> std::size_t { diff --git a/python/dartpy/dynamics/EulerJoint.cpp b/python/dartpy/dynamics/EulerJoint.cpp new file mode 100644 index 0000000000000..ba33ca24d086d --- /dev/null +++ b/python/dartpy/dynamics/EulerJoint.cpp @@ -0,0 +1,231 @@ +/* + * 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 +#include "Joint.hpp" + +namespace dart { +namespace python { + +void EulerJoint(pybind11::module& m) +{ + ::pybind11::class_( + m, "EulerJointUniqueProperties") + .def(::pybind11::init<>()) + .def( + ::pybind11::init(), + ::pybind11::arg("axisOrder")); + + ::pybind11::class_< + dart::dynamics::EulerJoint::Properties, + dart::dynamics::GenericJoint::Properties, + dart::dynamics::EulerJoint::UniqueProperties>(m, "EulerJointProperties") + .def(::pybind11::init<>()) + .def( + ::pybind11::init::Properties&>(), + ::pybind11::arg("genericJointProperties")) + .def( + ::pybind11::init< + const dart::dynamics::GenericJoint< + dart::math::R3Space>::Properties&, + const dart::dynamics::EulerJoint::UniqueProperties&>(), + ::pybind11::arg("genericJointProperties"), + ::pybind11::arg("uniqueProperties")) + .def_readwrite( + "mAxisOrder", + &dart::dynamics::detail::EulerJointUniqueProperties::mAxisOrder); + + DARTPY_DEFINE_JOINT_COMMON_BASE(EulerJoint, R3Space) + + ::pybind11::class_< + dart::dynamics::EulerJoint, + dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::EulerJoint, + dart::dynamics::detail::EulerJointUniqueProperties, + dart::dynamics::GenericJoint>>, + std::shared_ptr>(m, "EulerJoint") + .def( + "hasEulerJointAspect", + +[](const dart::dynamics::EulerJoint* self) -> bool { + return self->hasEulerJointAspect(); + }) + .def( + "setEulerJointAspect", + +[](dart::dynamics::EulerJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::EulerJoint, + dart::dynamics::detail::EulerJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<3>>>::Aspect* aspect) { + self->setEulerJointAspect(aspect); + }, + ::pybind11::arg("aspect")) + .def( + "removeEulerJointAspect", + +[](dart::dynamics::EulerJoint* self) { + self->removeEulerJointAspect(); + }) + .def( + "releaseEulerJointAspect", + +[](dart::dynamics::EulerJoint* self) + -> std::unique_ptr>>::Aspect> { + return self->releaseEulerJointAspect(); + }) + .def( + "setProperties", + +[](dart::dynamics::EulerJoint* self, + const dart::dynamics::EulerJoint::Properties& _properties) { + self->setProperties(_properties); + }, + ::pybind11::arg("properties")) + .def( + "setProperties", + +[](dart::dynamics::EulerJoint* self, + const dart::dynamics::EulerJoint::UniqueProperties& _properties) { + self->setProperties(_properties); + }, + ::pybind11::arg("properties")) + .def( + "setAspectProperties", + +[](dart::dynamics::EulerJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::EulerJoint, + dart::dynamics::detail::EulerJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<3>>>::AspectProperties& + properties) { self->setAspectProperties(properties); }, + ::pybind11::arg("properties")) + .def( + "getEulerJointProperties", + +[](const dart::dynamics::EulerJoint* self) + -> dart::dynamics::EulerJoint::Properties { + return self->getEulerJointProperties(); + }) + .def( + "copy", + +[](dart::dynamics::EulerJoint* self, + const dart::dynamics::EulerJoint* _otherJoint) { + self->copy(_otherJoint); + }, + ::pybind11::arg("otherJoint")) + .def( + "getType", + +[](const dart::dynamics::EulerJoint* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "isCyclic", + +[](const dart::dynamics::EulerJoint* self, + std::size_t _index) -> bool { return self->isCyclic(_index); }, + ::pybind11::arg("index")) + .def( + "setAxisOrder", + +[](dart::dynamics::EulerJoint* self, + dart::dynamics::EulerJoint::AxisOrder _order) { + self->setAxisOrder(_order); + }, + ::pybind11::arg("order")) + .def( + "setAxisOrder", + +[](dart::dynamics::EulerJoint* self, + dart::dynamics::EulerJoint::AxisOrder _order, + bool _renameDofs) { self->setAxisOrder(_order, _renameDofs); }, + ::pybind11::arg("order"), + ::pybind11::arg("renameDofs")) + .def( + "getAxisOrder", + +[](const dart::dynamics::EulerJoint* self) + -> dart::dynamics::EulerJoint::AxisOrder { + return self->getAxisOrder(); + }) + .def( + "convertToTransform", + +[](const dart::dynamics::EulerJoint* self, + const Eigen::Vector3d& _positions) -> Eigen::Isometry3d { + return self->convertToTransform(_positions); + }, + ::pybind11::arg("positions")) + .def( + "convertToRotation", + +[](const dart::dynamics::EulerJoint* self, + const Eigen::Vector3d& _positions) -> Eigen::Matrix3d { + return self->convertToRotation(_positions); + }, + ::pybind11::arg("positions")) + .def( + "getRelativeJacobianStatic", + +[](const dart::dynamics::EulerJoint* self, + const Eigen::Vector3d& _positions) + -> Eigen::Matrix { + return self->getRelativeJacobianStatic(_positions); + }, + ::pybind11::arg("positions")) + .def_static( + "getStaticType", + +[]() -> const std:: + string& { + return dart::dynamics::EulerJoint::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def_static( + "convertToTransformOf", + +[](const Eigen::Vector3d& _positions, + dart::dynamics::EulerJoint::AxisOrder _ordering) + -> Eigen::Isometry3d { + return dart::dynamics::EulerJoint::convertToTransform( + _positions, _ordering); + }, + ::pybind11::arg("positions"), + ::pybind11::arg("ordering")) + .def_static( + "convertToRotationOf", + +[](const Eigen::Vector3d& _positions, + dart::dynamics::EulerJoint::AxisOrder _ordering) + -> Eigen::Matrix3d { + return dart::dynamics::EulerJoint::convertToRotation( + _positions, _ordering); + }, + ::pybind11::arg("positions"), + ::pybind11::arg("ordering")); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/Joint.cpp b/python/dartpy/dynamics/Joint.cpp index d8a3204fba64b..9d8acbecb8db8 100644 --- a/python/dartpy/dynamics/Joint.cpp +++ b/python/dartpy/dynamics/Joint.cpp @@ -840,6 +840,26 @@ void Joint(pybind11::module& m) +[](const dart::dynamics::Joint* self) -> double { return self->computePotentialEnergy(); }) + .def( + "getRelativeTransform", + +[](const dart::dynamics::Joint* self) -> const Eigen::Isometry3d& { + return self->getRelativeTransform(); + }) + .def( + "getRelativeSpatialVelocity", + +[](const dart::dynamics::Joint* self) -> const Eigen::Vector6d& { + return self->getRelativeSpatialVelocity(); + }) + .def( + "getRelativeSpatialAcceleration", + +[](const dart::dynamics::Joint* self) -> const Eigen::Vector6d& { + return self->getRelativeSpatialAcceleration(); + }) + .def( + "getRelativePrimaryAcceleration", + +[](const dart::dynamics::Joint* self) -> const Eigen::Vector6d& { + return self->getRelativePrimaryAcceleration(); + }) .def( "getRelativeJacobian", +[](const dart::dynamics::Joint* self) -> const dart::math::Jacobian { diff --git a/python/dartpy/dynamics/Joint.hpp b/python/dartpy/dynamics/Joint.hpp new file mode 100644 index 0000000000000..8545c4da0bb89 --- /dev/null +++ b/python/dartpy/dynamics/Joint.hpp @@ -0,0 +1,124 @@ +/* + * 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 + +namespace dart { +namespace python { + +#define DARTPY_DEFINE_JOINT_COMMON_BASE(type, space) \ + ::pybind11::class_< \ + dart::common::SpecializedForAspect< \ + dart::common::EmbeddedPropertiesAspect< \ + dart::dynamics::type, \ + dart::dynamics::detail::type##UniqueProperties>>, \ + dart::common::Composite, \ + std::shared_ptr>>>( \ + m, \ + "SpecializedForAspect_EmbeddedPropertiesAspect_" #type "_" #type \ + "UniqueProperties") \ + .def(::pybind11::init<>()); \ + \ + ::pybind11::class_< \ + dart::common::RequiresAspect>, \ + dart::common::SpecializedForAspect< \ + dart::common::EmbeddedPropertiesAspect< \ + dart::dynamics::type, \ + dart::dynamics::detail::type##UniqueProperties>>, \ + std::shared_ptr< \ + dart::common::RequiresAspect>>>( \ + m, \ + "RequiresAspect_EmbeddedPropertiesAspect_" #type "_" #type \ + "UniqueProperties") \ + .def(::pybind11::init<>()); \ + \ + ::pybind11::class_< \ + dart::common::EmbedProperties< \ + dart::dynamics::type, \ + dart::dynamics::detail::type##UniqueProperties>, \ + dart::common::RequiresAspect>, \ + std::shared_ptr>>( \ + m, "EmbedProperties_" #type "_" #type "UniqueProperties"); \ + \ + ::pybind11::class_< \ + dart::common::CompositeJoiner< \ + dart::common::EmbedProperties< \ + dart::dynamics::type, \ + dart::dynamics::detail::type##UniqueProperties>, \ + dart::dynamics::GenericJoint>, \ + dart::common::EmbedProperties< \ + dart::dynamics::type, \ + dart::dynamics::detail::type##UniqueProperties>, \ + dart::dynamics::GenericJoint, \ + std::shared_ptr, \ + dart::dynamics::GenericJoint>>>( \ + m, \ + "CompositeJoiner_EmbedProperties_" #type "_" #type \ + "UniqueProperties_GenericJoint_" #space); \ + \ + ::pybind11::class_< \ + dart::common::EmbedPropertiesOnTopOf< \ + dart::dynamics::type, \ + dart::dynamics::detail::type##UniqueProperties, \ + dart::dynamics::GenericJoint>, \ + dart::common::CompositeJoiner< \ + dart::common::EmbedProperties< \ + dart::dynamics::type, \ + dart::dynamics::detail::type##UniqueProperties>, \ + dart::dynamics::GenericJoint>, \ + std::shared_ptr>>>( \ + m, \ + "EmbedPropertiesOnTopOf_" #type "_" #type \ + "UniqueProperties_GenericJoint_" #space); + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/PlanarJoint.cpp b/python/dartpy/dynamics/PlanarJoint.cpp new file mode 100644 index 0000000000000..71ec8c31e33ec --- /dev/null +++ b/python/dartpy/dynamics/PlanarJoint.cpp @@ -0,0 +1,254 @@ +/* + * 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 +#include "Joint.hpp" + +namespace dart { +namespace python { + +void PlanarJoint(pybind11::module& m) +{ + ::pybind11::class_( + m, "PlanarJointUniqueProperties") + .def(::pybind11::init<>()); + + ::pybind11::class_< + dart::dynamics::PlanarJoint::Properties, + dart::dynamics::GenericJoint::Properties, + dart::dynamics::PlanarJoint::UniqueProperties>(m, "PlanarJointProperties") + .def(::pybind11::init<>()) + .def( + ::pybind11::init::Properties&>(), + ::pybind11::arg("genericJointProperties")) + .def( + ::pybind11::init< + const dart::dynamics::GenericJoint< + dart::math::R3Space>::Properties&, + const dart::dynamics::PlanarJoint::UniqueProperties&>(), + ::pybind11::arg("genericJointProperties"), + ::pybind11::arg("uniqueProperties")) + .def_readwrite( + "mPlaneType", + &dart::dynamics::detail::PlanarJointUniqueProperties::mPlaneType) + .def_readwrite( + "mTransAxis1", + &dart::dynamics::detail::PlanarJointUniqueProperties::mTransAxis1) + .def_readwrite( + "mTransAxis2", + &dart::dynamics::detail::PlanarJointUniqueProperties::mTransAxis2) + .def_readwrite( + "mRotAxis", + &dart::dynamics::detail::PlanarJointUniqueProperties::mRotAxis); + + DARTPY_DEFINE_JOINT_COMMON_BASE(PlanarJoint, R3Space) + + ::pybind11::class_< + dart::dynamics::PlanarJoint, + dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::PlanarJoint, + dart::dynamics::detail::PlanarJointUniqueProperties, + dart::dynamics::GenericJoint>>, + std::shared_ptr>(m, "PlanarJoint") + .def( + "hasPlanarJointAspect", + +[](const dart::dynamics::PlanarJoint* self) -> bool { + return self->hasPlanarJointAspect(); + }) + .def( + "setPlanarJointAspect", + +[](dart::dynamics::PlanarJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::PlanarJoint, + dart::dynamics::detail::PlanarJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<3>>>::Aspect* aspect) { + self->setPlanarJointAspect(aspect); + }, + ::pybind11::arg("aspect")) + .def( + "removePlanarJointAspect", + +[](dart::dynamics::PlanarJoint* self) { + self->removePlanarJointAspect(); + }) + .def( + "releasePlanarJointAspect", + +[](dart::dynamics::PlanarJoint* self) + -> std::unique_ptr>>::Aspect> { + return self->releasePlanarJointAspect(); + }) + .def( + "setProperties", + +[](dart::dynamics::PlanarJoint* self, + const dart::dynamics::PlanarJoint::Properties& _properties) { + self->setProperties(_properties); + }, + ::pybind11::arg("properties")) + .def( + "setProperties", + +[](dart::dynamics::PlanarJoint* self, + const dart::dynamics::PlanarJoint::UniqueProperties& + _properties) { self->setProperties(_properties); }, + ::pybind11::arg("properties")) + .def( + "setAspectProperties", + +[](dart::dynamics::PlanarJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::PlanarJoint, + dart::dynamics::detail::PlanarJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<3>>>::AspectProperties& + properties) { self->setAspectProperties(properties); }, + ::pybind11::arg("properties")) + .def( + "getPlanarJointProperties", + +[](const dart::dynamics::PlanarJoint* self) + -> dart::dynamics::PlanarJoint::Properties { + return self->getPlanarJointProperties(); + }) + .def( + "copy", + +[](dart::dynamics::PlanarJoint* self, + const dart::dynamics::PlanarJoint* _otherJoint) { + self->copy(_otherJoint); + }, + ::pybind11::arg("otherJoint")) + .def( + "getType", + +[](const dart::dynamics::PlanarJoint* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "isCyclic", + +[](const dart::dynamics::PlanarJoint* self, + std::size_t _index) -> bool { return self->isCyclic(_index); }, + ::pybind11::arg("index")) + .def( + "setXYPlane", + +[](dart::dynamics::PlanarJoint* self) { self->setXYPlane(); }) + .def( + "setXYPlane", + +[](dart::dynamics::PlanarJoint* self, bool _renameDofs) { + self->setXYPlane(_renameDofs); + }, + ::pybind11::arg("renameDofs")) + .def( + "setYZPlane", + +[](dart::dynamics::PlanarJoint* self) { self->setYZPlane(); }) + .def( + "setYZPlane", + +[](dart::dynamics::PlanarJoint* self, bool _renameDofs) { + self->setYZPlane(_renameDofs); + }, + ::pybind11::arg("renameDofs")) + .def( + "setZXPlane", + +[](dart::dynamics::PlanarJoint* self) { self->setZXPlane(); }) + .def( + "setZXPlane", + +[](dart::dynamics::PlanarJoint* self, bool _renameDofs) { + self->setZXPlane(_renameDofs); + }, + ::pybind11::arg("renameDofs")) + .def( + "setArbitraryPlane", + +[](dart::dynamics::PlanarJoint* self, + const Eigen::Vector3d& _transAxis1, + const Eigen::Vector3d& _transAxis2) { + self->setArbitraryPlane(_transAxis1, _transAxis2); + }, + ::pybind11::arg("transAxis1"), + ::pybind11::arg("transAxis2")) + .def( + "setArbitraryPlane", + +[](dart::dynamics::PlanarJoint* self, + const Eigen::Vector3d& _transAxis1, + const Eigen::Vector3d& _transAxis2, + bool _renameDofs) { + self->setArbitraryPlane(_transAxis1, _transAxis2, _renameDofs); + }, + ::pybind11::arg("transAxis1"), + ::pybind11::arg("transAxis2"), + ::pybind11::arg("renameDofs")) + .def( + "getPlaneType", + +[](const dart::dynamics::PlanarJoint* self) + -> dart::dynamics::PlanarJoint::PlaneType { + return self->getPlaneType(); + }) + .def( + "getRotationalAxis", + +[](const dart::dynamics::PlanarJoint* self) + -> const Eigen::Vector3d& { return self->getRotationalAxis(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getTranslationalAxis1", + +[](const dart::dynamics::PlanarJoint* self) + -> const Eigen::Vector3d& { + return self->getTranslationalAxis1(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getTranslationalAxis2", + +[](const dart::dynamics::PlanarJoint* self) + -> const Eigen::Vector3d& { + return self->getTranslationalAxis2(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getRelativeJacobianStatic", + +[](const dart::dynamics::PlanarJoint* self, + const Eigen::Vector3d& _positions) + -> Eigen::Matrix { + return self->getRelativeJacobianStatic(_positions); + }, + ::pybind11::arg("positions")) + .def_static( + "getStaticType", + +[]() -> const std:: + string& { + return dart::dynamics::PlanarJoint::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/PrismaticJoint.cpp b/python/dartpy/dynamics/PrismaticJoint.cpp new file mode 100644 index 0000000000000..da4cedecea783 --- /dev/null +++ b/python/dartpy/dynamics/PrismaticJoint.cpp @@ -0,0 +1,185 @@ +/* + * 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 +#include "Joint.hpp" + +namespace dart { +namespace python { + +void PrismaticJoint(pybind11::module& m) +{ + ::pybind11::class_( + m, "PrismaticJointUniqueProperties") + .def(::pybind11::init<>()) + .def(::pybind11::init(), ::pybind11::arg("axis")); + + ::pybind11::class_< + dart::dynamics::PrismaticJoint::Properties, + dart::dynamics::GenericJoint::Properties, + dart::dynamics::PrismaticJoint::UniqueProperties>( + m, "PrismaticJointProperties") + .def(::pybind11::init<>()) + .def( + ::pybind11::init::Properties&>(), + ::pybind11::arg("genericJointProperties")) + .def( + ::pybind11::init< + const dart::dynamics::GenericJoint< + dart::math::R1Space>::Properties&, + const dart::dynamics::PrismaticJoint::UniqueProperties&>(), + ::pybind11::arg("genericJointProperties"), + ::pybind11::arg("revoluteProperties")) + .def_readwrite( + "mAxis", + &dart::dynamics::detail::PrismaticJointUniqueProperties::mAxis); + + DARTPY_DEFINE_JOINT_COMMON_BASE(PrismaticJoint, R1Space) + + ::pybind11::class_< + dart::dynamics::PrismaticJoint, + dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::PrismaticJoint, + dart::dynamics::detail::PrismaticJointUniqueProperties, + dart::dynamics::GenericJoint>>, + std::shared_ptr>(m, "PrismaticJoint") + .def( + "hasPrismaticJointAspect", + +[](const dart::dynamics::PrismaticJoint* self) -> bool { + return self->hasPrismaticJointAspect(); + }) + .def( + "setPrismaticJointAspect", + +[](dart::dynamics::PrismaticJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::PrismaticJoint, + dart::dynamics::detail::PrismaticJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<1>>>::Aspect* aspect) { + self->setPrismaticJointAspect(aspect); + }, + ::pybind11::arg("aspect")) + .def( + "removePrismaticJointAspect", + +[](dart::dynamics::PrismaticJoint* self) { + self->removePrismaticJointAspect(); + }) + .def( + "releasePrismaticJointAspect", + +[](dart::dynamics::PrismaticJoint* self) + -> std::unique_ptr>>::Aspect> { + return self->releasePrismaticJointAspect(); + }) + .def( + "setProperties", + +[](dart::dynamics::PrismaticJoint* self, + const dart::dynamics::PrismaticJoint::Properties& _properties) { + self->setProperties(_properties); + }, + ::pybind11::arg("properties")) + .def( + "setProperties", + +[](dart::dynamics::PrismaticJoint* self, + const dart::dynamics::PrismaticJoint::UniqueProperties& + _properties) { self->setProperties(_properties); }, + ::pybind11::arg("properties")) + .def( + "setAspectProperties", + +[](dart::dynamics::PrismaticJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::PrismaticJoint, + dart::dynamics::detail::PrismaticJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<1>>>::AspectProperties& + properties) { self->setAspectProperties(properties); }, + ::pybind11::arg("properties")) + .def( + "getPrismaticJointProperties", + +[](const dart::dynamics::PrismaticJoint* self) + -> dart::dynamics::PrismaticJoint::Properties { + return self->getPrismaticJointProperties(); + }) + .def( + "copy", + +[](dart::dynamics::PrismaticJoint* self, + const dart::dynamics::PrismaticJoint* _otherJoint) { + self->copy(_otherJoint); + }, + ::pybind11::arg("otherJoint")) + .def( + "getType", + +[](const dart::dynamics::PrismaticJoint* self) + -> const std::string& { return self->getType(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "isCyclic", + +[](const dart::dynamics::PrismaticJoint* self, + std::size_t _index) -> bool { return self->isCyclic(_index); }, + ::pybind11::arg("index")) + .def( + "setAxis", + +[](dart::dynamics::PrismaticJoint* self, + const Eigen::Vector3d& _axis) { self->setAxis(_axis); }, + ::pybind11::arg("axis")) + .def( + "getAxis", + +[](const dart::dynamics::PrismaticJoint* self) + -> const Eigen::Vector3d& { return self->getAxis(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getRelativeJacobianStatic", + +[](const dart::dynamics::PrismaticJoint* self, + const dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<1>>::Vector& positions) + -> dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<1>>::JacobianMatrix { + return self->getRelativeJacobianStatic(positions); + }, + ::pybind11::arg("positions")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::PrismaticJoint::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/RevoluteJoint.cpp b/python/dartpy/dynamics/RevoluteJoint.cpp index 3949431d52937..ef6885b1e525f 100644 --- a/python/dartpy/dynamics/RevoluteJoint.cpp +++ b/python/dartpy/dynamics/RevoluteJoint.cpp @@ -34,6 +34,7 @@ #include #include #include +#include "Joint.hpp" namespace dart { namespace python { @@ -61,89 +62,12 @@ void RevoluteJoint(pybind11::module& m) dart::math::R1Space>::Properties&, const dart::dynamics::RevoluteJoint::UniqueProperties&>(), ::pybind11::arg("genericJointProperties"), - ::pybind11::arg("revoluteProperties")) + ::pybind11::arg("uniqueProperties")) .def_readwrite( "mAxis", &dart::dynamics::detail::RevoluteJointUniqueProperties::mAxis); - ::pybind11::class_< - dart::common::SpecializedForAspect>, - dart::common::Composite, - std::shared_ptr>>>( - m, - "SpecializedForAspect_EmbeddedPropertiesAspect_RevoluteJoint_" - "RevoluteJointUniqueProperties") - .def(::pybind11::init<>()); - - ::pybind11::class_< - dart::common::RequiresAspect>, - dart::common::SpecializedForAspect>, - std::shared_ptr< - dart::common::RequiresAspect>>>( - m, - "RequiresAspect_EmbeddedPropertiesAspect_RevoluteJoint_" - "RevoluteJointUniqueProperties") - .def(::pybind11::init<>()); - - ::pybind11::class_< - dart::common::EmbedProperties< - dart::dynamics::RevoluteJoint, - dart::dynamics::detail::RevoluteJointUniqueProperties>, - dart::common::RequiresAspect>, - std::shared_ptr>>( - m, "EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties"); - - ::pybind11::class_< - dart::common::CompositeJoiner< - dart::common::EmbedProperties< - dart::dynamics::RevoluteJoint, - dart::dynamics::detail::RevoluteJointUniqueProperties>, - dart::dynamics::GenericJoint>>, - dart::common::EmbedProperties< - dart::dynamics::RevoluteJoint, - dart::dynamics::detail::RevoluteJointUniqueProperties>, - dart::dynamics::GenericJoint>, - std::shared_ptr, - dart::dynamics::GenericJoint>>>>( - m, - "CompositeJoiner_EmbedProperties_RevoluteJoint_" - "RevoluteJointUniqueProperties_GenericJoint_R1"); - - ::pybind11::class_< - dart::common::EmbedPropertiesOnTopOf< - dart::dynamics::RevoluteJoint, - dart::dynamics::detail::RevoluteJointUniqueProperties, - dart::dynamics::GenericJoint>>, - dart::common::CompositeJoiner< - dart::common::EmbedProperties< - dart::dynamics::RevoluteJoint, - dart::dynamics::detail::RevoluteJointUniqueProperties>, - dart::dynamics::GenericJoint>>, - std::shared_ptr>>>>( - m, - "EmbedPropertiesOnTopOf_RevoluteJoint_RevoluteJointUniqueProperties_" - "GenericJoint_R1"); + DARTPY_DEFINE_JOINT_COMMON_BASE(RevoluteJoint, R1Space) ::pybind11::class_< dart::dynamics::RevoluteJoint, diff --git a/python/dartpy/dynamics/ScrewJoint.cpp b/python/dartpy/dynamics/ScrewJoint.cpp new file mode 100644 index 0000000000000..8627f23b0e1bb --- /dev/null +++ b/python/dartpy/dynamics/ScrewJoint.cpp @@ -0,0 +1,205 @@ +/* + * 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 +#include "Joint.hpp" + +namespace dart { +namespace python { + +void ScrewJoint(pybind11::module& m) +{ + ::pybind11::class_( + m, "ScrewJointUniqueProperties") + .def(::pybind11::init<>()) + .def(::pybind11::init(), ::pybind11::arg("axis")) + .def( + ::pybind11::init(), + ::pybind11::arg("axis"), + ::pybind11::arg("pitch")); + + ::pybind11::class_< + dart::dynamics::ScrewJoint::Properties, + dart::dynamics::GenericJoint::Properties, + dart::dynamics::ScrewJoint::UniqueProperties>(m, "ScrewJointProperties") + .def(::pybind11::init<>()) + .def( + ::pybind11::init::Properties&>(), + ::pybind11::arg("genericJointProperties")) + .def( + ::pybind11::init< + const dart::dynamics::GenericJoint< + dart::math::R1Space>::Properties&, + const dart::dynamics::ScrewJoint::UniqueProperties&>(), + ::pybind11::arg("genericJointProperties"), + ::pybind11::arg("revoluteProperties")) + .def_readwrite( + "mAxis", &dart::dynamics::detail::ScrewJointUniqueProperties::mAxis) + .def_readwrite( + "mPitch", + &dart::dynamics::detail::ScrewJointUniqueProperties::mPitch); + + DARTPY_DEFINE_JOINT_COMMON_BASE(ScrewJoint, R1Space) + + ::pybind11::class_< + dart::dynamics::ScrewJoint, + dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::ScrewJoint, + dart::dynamics::detail::ScrewJointUniqueProperties, + dart::dynamics::GenericJoint>>, + std::shared_ptr>(m, "ScrewJoint") + .def( + "hasScrewJointAspect", + +[](const dart::dynamics::ScrewJoint* self) -> bool { + return self->hasScrewJointAspect(); + }) + .def( + "setScrewJointAspect", + +[](dart::dynamics::ScrewJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::ScrewJoint, + dart::dynamics::detail::ScrewJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<1>>>::Aspect* aspect) { + self->setScrewJointAspect(aspect); + }, + ::pybind11::arg("aspect")) + .def( + "removeScrewJointAspect", + +[](dart::dynamics::ScrewJoint* self) { + self->removeScrewJointAspect(); + }) + .def( + "releaseScrewJointAspect", + +[](dart::dynamics::ScrewJoint* self) + -> std::unique_ptr>>::Aspect> { + return self->releaseScrewJointAspect(); + }) + .def( + "setProperties", + +[](dart::dynamics::ScrewJoint* self, + const dart::dynamics::ScrewJoint::Properties& _properties) { + self->setProperties(_properties); + }, + ::pybind11::arg("properties")) + .def( + "setProperties", + +[](dart::dynamics::ScrewJoint* self, + const dart::dynamics::ScrewJoint::UniqueProperties& _properties) { + self->setProperties(_properties); + }, + ::pybind11::arg("properties")) + .def( + "setAspectProperties", + +[](dart::dynamics::ScrewJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::ScrewJoint, + dart::dynamics::detail::ScrewJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<1>>>::AspectProperties& + properties) { self->setAspectProperties(properties); }, + ::pybind11::arg("properties")) + .def( + "getScrewJointProperties", + +[](const dart::dynamics::ScrewJoint* self) + -> dart::dynamics::ScrewJoint::Properties { + return self->getScrewJointProperties(); + }) + .def( + "copy", + +[](dart::dynamics::ScrewJoint* self, + const dart::dynamics::ScrewJoint* _otherJoint) { + self->copy(_otherJoint); + }, + ::pybind11::arg("otherJoint")) + .def( + "getType", + +[](const dart::dynamics::ScrewJoint* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "isCyclic", + +[](const dart::dynamics::ScrewJoint* self, + std::size_t _index) -> bool { return self->isCyclic(_index); }, + ::pybind11::arg("index")) + .def( + "setAxis", + +[](dart::dynamics::ScrewJoint* self, const Eigen::Vector3d& _axis) { + self->setAxis(_axis); + }, + ::pybind11::arg("axis")) + .def( + "getAxis", + +[](const dart::dynamics::ScrewJoint* self) + -> const Eigen::Vector3d& { return self->getAxis(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "setPitch", + +[](dart::dynamics::ScrewJoint* self, double _pitch) { + self->setPitch(_pitch); + }, + ::pybind11::arg("pitch")) + .def( + "getPitch", + +[](const dart::dynamics::ScrewJoint* self) -> double { + return self->getPitch(); + }) + .def( + "getRelativeJacobianStatic", + +[](const dart::dynamics::ScrewJoint* self, + const dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<1>>::Vector& positions) + -> dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<1>>::JacobianMatrix { + return self->getRelativeJacobianStatic(positions); + }, + ::pybind11::arg("positions")) + .def_static( + "getStaticType", + +[]() -> const std:: + string& { + return dart::dynamics::ScrewJoint::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/Skeleton.cpp b/python/dartpy/dynamics/Skeleton.cpp index 96118ca6895e5..cde2e21bef7f4 100644 --- a/python/dartpy/dynamics/Skeleton.cpp +++ b/python/dartpy/dynamics/Skeleton.cpp @@ -36,7 +36,7 @@ #include "eigen_geometry_pybind.h" #include "eigen_pybind.h" -#define DARTPY_DEFINE_CREATEJOINTANDBODYNODEPAIR(joint_type) \ +#define DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(joint_type) \ .def( \ "create" #joint_type "AndBodyNodePair", \ +[](dart::dynamics::Skeleton* self) \ @@ -326,9 +326,17 @@ void Skeleton(pybind11::module& m) }, ::pybind11::return_value_policy::reference_internal) // clang-format off - DARTPY_DEFINE_CREATEJOINTANDBODYNODEPAIR(RevoluteJoint) - DARTPY_DEFINE_CREATEJOINTANDBODYNODEPAIR(BallJoint) - DARTPY_DEFINE_CREATEJOINTANDBODYNODEPAIR(FreeJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(WeldJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(RevoluteJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(PrismaticJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(ScrewJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(UniversalJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(TranslationalJoint2D) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(PlanarJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(EulerJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(BallJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(TranslationalJoint) + DARTPY_DEFINE_CREATE_JOINT_AND_BODY_NODE_PAIR(FreeJoint) // clang-format on .def( "getNumBodyNodes", diff --git a/python/dartpy/dynamics/TranslationalJoint.cpp b/python/dartpy/dynamics/TranslationalJoint.cpp new file mode 100644 index 0000000000000..fca7aa87533f3 --- /dev/null +++ b/python/dartpy/dynamics/TranslationalJoint.cpp @@ -0,0 +1,90 @@ +/* + * 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 +#include "Joint.hpp" + +namespace dart { +namespace python { + +void TranslationalJoint(pybind11::module& m) +{ + ::pybind11::class_( + m, "TranslationalJointProperties") + .def(::pybind11::init<>()) + .def( + ::pybind11::init>::Properties&>(), + ::pybind11::arg("properties")); + + ::pybind11::class_< + dart::dynamics::TranslationalJoint, + dart::dynamics::GenericJoint>, + std::shared_ptr>( + m, "TranslationalJoint") + .def( + "getTranslationalJointProperties", + +[](const dart::dynamics::TranslationalJoint* self) + -> dart::dynamics::TranslationalJoint::Properties { + return self->getTranslationalJointProperties(); + }) + .def( + "getType", + +[](const dart::dynamics::TranslationalJoint* self) + -> const std::string& { return self->getType(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "isCyclic", + +[](const dart::dynamics::TranslationalJoint* self, + std::size_t _index) -> bool { return self->isCyclic(_index); }, + ::pybind11::arg("index")) + .def( + "getRelativeJacobianStatic", + +[](const dart::dynamics::TranslationalJoint* self, + const Eigen::Vector3d& _positions) + -> Eigen::Matrix { + return self->getRelativeJacobianStatic(_positions); + }, + ::pybind11::arg("positions")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::TranslationalJoint::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/TranslationalJoint2D.cpp b/python/dartpy/dynamics/TranslationalJoint2D.cpp new file mode 100644 index 0000000000000..6a68f9a794cee --- /dev/null +++ b/python/dartpy/dynamics/TranslationalJoint2D.cpp @@ -0,0 +1,235 @@ +/* + * 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 +#include "Joint.hpp" + +namespace dart { +namespace python { + +void TranslationalJoint2D(pybind11::module& m) +{ + ::pybind11::class_( + m, "TranslationalJoint2DUniqueProperties") + .def(::pybind11::init<>()); + + ::pybind11::class_< + dart::dynamics::TranslationalJoint2D::Properties, + dart::dynamics::GenericJoint::Properties, + dart::dynamics::TranslationalJoint2D::UniqueProperties>( + m, "TranslationalJoint2DProperties") + .def(::pybind11::init<>()) + .def( + ::pybind11::init::Properties&>(), + ::pybind11::arg("genericJointProperties")) + .def( + ::pybind11::init< + const dart::dynamics::GenericJoint< + dart::math::R2Space>::Properties&, + const dart::dynamics::TranslationalJoint2D::UniqueProperties&>(), + ::pybind11::arg("genericJointProperties"), + ::pybind11::arg("uniqueProperties")); + + DARTPY_DEFINE_JOINT_COMMON_BASE(TranslationalJoint2D, R2Space) + + ::pybind11::class_< + dart::dynamics::TranslationalJoint2D, + dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::TranslationalJoint2D, + dart::dynamics::detail::TranslationalJoint2DUniqueProperties, + dart::dynamics::GenericJoint > >, + std::shared_ptr >( + m, "TranslationalJoint2D") + .def( + "hasTranslationalJoint2DAspect", + +[](const dart::dynamics::TranslationalJoint2D* self) -> bool { + return self->hasTranslationalJoint2DAspect(); + }) + .def( + "setTranslationalJoint2DAspect", + +[](dart::dynamics::TranslationalJoint2D* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::TranslationalJoint2D, + dart::dynamics::detail::TranslationalJoint2DUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<2> > >::Aspect* aspect) { + self->setTranslationalJoint2DAspect(aspect); + }, + ::pybind11::arg("aspect")) + .def( + "removeTranslationalJoint2DAspect", + +[](dart::dynamics::TranslationalJoint2D* self) { + self->removeTranslationalJoint2DAspect(); + }) + .def( + "releaseTranslationalJoint2DAspect", + +[](dart::dynamics::TranslationalJoint2D* self) + -> std::unique_ptr > >::Aspect> { + return self->releaseTranslationalJoint2DAspect(); + }) + .def( + "setProperties", + +[](dart::dynamics::TranslationalJoint2D* self, + const dart::dynamics::TranslationalJoint2D::Properties& + properties) { self->setProperties(properties); }, + ::pybind11::arg("properties")) + .def( + "setProperties", + +[](dart::dynamics::TranslationalJoint2D* self, + const dart::dynamics::TranslationalJoint2D::UniqueProperties& + properties) { self->setProperties(properties); }, + ::pybind11::arg("properties")) + .def( + "setAspectProperties", + +[](dart::dynamics::TranslationalJoint2D* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::TranslationalJoint2D, + dart::dynamics::detail::TranslationalJoint2DUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<2> > >::AspectProperties& + properties) { self->setAspectProperties(properties); }, + ::pybind11::arg("properties")) + .def( + "getTranslationalJoint2DProperties", + +[](const dart::dynamics::TranslationalJoint2D* self) + -> dart::dynamics::TranslationalJoint2D::Properties { + return self->getTranslationalJoint2DProperties(); + }) + .def( + "copy", + +[](dart::dynamics::TranslationalJoint2D* self, + const dart::dynamics::TranslationalJoint2D* otherJoint) { + self->copy(otherJoint); + }, + ::pybind11::arg("otherJoint")) + .def( + "getType", + +[](const dart::dynamics::TranslationalJoint2D* self) + -> const std::string& { return self->getType(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "isCyclic", + +[](const dart::dynamics::TranslationalJoint2D* self, + std::size_t index) -> bool { return self->isCyclic(index); }, + ::pybind11::arg("index")) + .def( + "setXYPlane", + +[](dart::dynamics::TranslationalJoint2D* self) { + self->setXYPlane(); + }) + .def( + "setXYPlane", + +[](dart::dynamics::TranslationalJoint2D* self, bool renameDofs) { + self->setXYPlane(renameDofs); + }, + ::pybind11::arg("renameDofs")) + .def( + "setYZPlane", + +[](dart::dynamics::TranslationalJoint2D* self) { + self->setYZPlane(); + }) + .def( + "setYZPlane", + +[](dart::dynamics::TranslationalJoint2D* self, bool renameDofs) { + self->setYZPlane(renameDofs); + }, + ::pybind11::arg("renameDofs")) + .def( + "setZXPlane", + +[](dart::dynamics::TranslationalJoint2D* self) { + self->setZXPlane(); + }) + .def( + "setZXPlane", + +[](dart::dynamics::TranslationalJoint2D* self, bool renameDofs) { + self->setZXPlane(renameDofs); + }, + ::pybind11::arg("renameDofs")) + .def( + "setArbitraryPlane", + +[](dart::dynamics::TranslationalJoint2D* self, + const Eigen::Vector3d& transAxis1, + const Eigen::Vector3d& transAxis2) { + self->setArbitraryPlane(transAxis1, transAxis2); + }, + ::pybind11::arg("transAxis1"), + ::pybind11::arg("transAxis2")) + .def( + "setArbitraryPlane", + +[](dart::dynamics::TranslationalJoint2D* self, + const Eigen::Vector3d& transAxis1, + const Eigen::Vector3d& transAxis2, + bool renameDofs) { + self->setArbitraryPlane(transAxis1, transAxis2, renameDofs); + }, + ::pybind11::arg("transAxis1"), + ::pybind11::arg("transAxis2"), + ::pybind11::arg("renameDofs")) + .def( + "getPlaneType", + +[](const dart::dynamics::TranslationalJoint2D* self) + -> dart::dynamics::TranslationalJoint2D::PlaneType { + return self->getPlaneType(); + }) + .def( + "getTranslationalAxis1", + +[](const dart::dynamics::TranslationalJoint2D* self) + -> Eigen::Vector3d { return self->getTranslationalAxis1(); }) + .def( + "getTranslationalAxis2", + +[](const dart::dynamics::TranslationalJoint2D* self) + -> Eigen::Vector3d { return self->getTranslationalAxis2(); }) + .def( + "getRelativeJacobianStatic", + +[](const dart::dynamics::TranslationalJoint2D* self, + const Eigen::Vector2d& positions) -> Eigen::Matrix { + return self->getRelativeJacobianStatic(positions); + }, + ::pybind11::arg("positions")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::TranslationalJoint2D::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/UniversalJoint.cpp b/python/dartpy/dynamics/UniversalJoint.cpp new file mode 100644 index 0000000000000..d20e637f8670c --- /dev/null +++ b/python/dartpy/dynamics/UniversalJoint.cpp @@ -0,0 +1,192 @@ +/* + * 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 +#include "Joint.hpp" + +namespace dart { +namespace python { + +void UniversalJoint(pybind11::module& m) +{ + ::pybind11::class_( + m, "UniversalJointUniqueProperties") + .def(::pybind11::init<>()); + + ::pybind11::class_< + dart::dynamics::UniversalJoint::Properties, + dart::dynamics::GenericJoint::Properties, + dart::dynamics::UniversalJoint::UniqueProperties>( + m, "UniversalJointProperties") + .def(::pybind11::init<>()) + .def( + ::pybind11::init::Properties&>(), + ::pybind11::arg("genericJointProperties")) + .def( + ::pybind11::init< + const dart::dynamics::GenericJoint< + dart::math::R2Space>::Properties&, + const dart::dynamics::UniversalJoint::UniqueProperties&>(), + ::pybind11::arg("genericJointProperties"), + ::pybind11::arg("uniqueProperties")) + .def_readwrite( + "mAxis", + &dart::dynamics::detail::UniversalJointUniqueProperties::mAxis); + + DARTPY_DEFINE_JOINT_COMMON_BASE(UniversalJoint, R2Space) + + ::pybind11::class_< + dart::dynamics::UniversalJoint, + dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::UniversalJoint, + dart::dynamics::detail::UniversalJointUniqueProperties, + dart::dynamics::GenericJoint > >, + std::shared_ptr >(m, "UniversalJoint") + .def( + "hasUniversalJointAspect", + +[](const dart::dynamics::UniversalJoint* self) -> bool { + return self->hasUniversalJointAspect(); + }) + .def( + "setUniversalJointAspect", + +[](dart::dynamics::UniversalJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::UniversalJoint, + dart::dynamics::detail::UniversalJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<2> > >::Aspect* aspect) { + self->setUniversalJointAspect(aspect); + }, + ::pybind11::arg("aspect")) + .def( + "removeUniversalJointAspect", + +[](dart::dynamics::UniversalJoint* self) { + self->removeUniversalJointAspect(); + }) + .def( + "releaseUniversalJointAspect", + +[](dart::dynamics::UniversalJoint* self) + -> std::unique_ptr > >::Aspect> { + return self->releaseUniversalJointAspect(); + }) + .def( + "setProperties", + +[](dart::dynamics::UniversalJoint* self, + const dart::dynamics::UniversalJoint::Properties& _properties) { + self->setProperties(_properties); + }, + ::pybind11::arg("properties")) + .def( + "setProperties", + +[](dart::dynamics::UniversalJoint* self, + const dart::dynamics::UniversalJoint::UniqueProperties& + _properties) { self->setProperties(_properties); }, + ::pybind11::arg("properties")) + .def( + "setAspectProperties", + +[](dart::dynamics::UniversalJoint* self, + const dart::common::EmbedPropertiesOnTopOf< + dart::dynamics::UniversalJoint, + dart::dynamics::detail::UniversalJointUniqueProperties, + dart::dynamics::GenericJoint< + dart::math::RealVectorSpace<2> > >::AspectProperties& + properties) { self->setAspectProperties(properties); }, + ::pybind11::arg("properties")) + .def( + "getUniversalJointProperties", + +[](const dart::dynamics::UniversalJoint* self) + -> dart::dynamics::UniversalJoint::Properties { + return self->getUniversalJointProperties(); + }) + .def( + "copy", + +[](dart::dynamics::UniversalJoint* self, + const dart::dynamics::UniversalJoint* _otherJoint) { + self->copy(_otherJoint); + }, + ::pybind11::arg("otherJoint")) + .def( + "getType", + +[](const dart::dynamics::UniversalJoint* self) + -> const std::string& { return self->getType(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "isCyclic", + +[](const dart::dynamics::UniversalJoint* self, + std::size_t _index) -> bool { return self->isCyclic(_index); }, + ::pybind11::arg("index")) + .def( + "setAxis1", + +[](dart::dynamics::UniversalJoint* self, + const Eigen::Vector3d& _axis) { self->setAxis1(_axis); }, + ::pybind11::arg("axis")) + .def( + "setAxis2", + +[](dart::dynamics::UniversalJoint* self, + const Eigen::Vector3d& _axis) { self->setAxis2(_axis); }, + ::pybind11::arg("axis")) + .def( + "getAxis1", + +[](const dart::dynamics::UniversalJoint* self) + -> const Eigen::Vector3d& { return self->getAxis1(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getAxis2", + +[](const dart::dynamics::UniversalJoint* self) + -> const Eigen::Vector3d& { return self->getAxis2(); }, + ::pybind11::return_value_policy::reference_internal) + .def( + "getRelativeJacobianStatic", + +[](const dart::dynamics::UniversalJoint* self, + const Eigen::Vector2d& _positions) + -> Eigen::Matrix { + return self->getRelativeJacobianStatic(_positions); + }, + ::pybind11::arg("positions")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::UniversalJoint::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/WeldJoint.cpp b/python/dartpy/dynamics/WeldJoint.cpp new file mode 100644 index 0000000000000..365928891303e --- /dev/null +++ b/python/dartpy/dynamics/WeldJoint.cpp @@ -0,0 +1,86 @@ +/* + * 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 +#include "Joint.hpp" + +namespace dart { +namespace python { + +void WeldJoint(pybind11::module& m) +{ + ::pybind11::class_< + dart::dynamics::WeldJoint, + dart::dynamics::ZeroDofJoint, + std::shared_ptr>(m, "WeldJoint") + .def( + "getWeldJointProperties", + +[](const dart::dynamics::WeldJoint* self) + -> dart::dynamics::WeldJoint::Properties { + return self->getWeldJointProperties(); + }) + .def( + "getType", + +[](const dart::dynamics::WeldJoint* self) -> const std::string& { + return self->getType(); + }, + ::pybind11::return_value_policy::reference_internal) + .def( + "isCyclic", + +[](const dart::dynamics::WeldJoint* self, + std::size_t _index) -> bool { return self->isCyclic(_index); }, + ::pybind11::arg("index")) + .def( + "setTransformFromParentBodyNode", + +[](dart::dynamics::WeldJoint* self, const Eigen::Isometry3d& _T) { + self->setTransformFromParentBodyNode(_T); + }, + ::pybind11::arg("T")) + .def( + "setTransformFromChildBodyNode", + +[](dart::dynamics::WeldJoint* self, const Eigen::Isometry3d& _T) { + self->setTransformFromChildBodyNode(_T); + }, + ::pybind11::arg("T")) + .def_static( + "getStaticType", + +[]() -> const std::string& { + return dart::dynamics::WeldJoint::getStaticType(); + }, + ::pybind11::return_value_policy::reference_internal); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/ZeroDofJoint.cpp b/python/dartpy/dynamics/ZeroDofJoint.cpp new file mode 100644 index 0000000000000..aaf2fbc1ab44e --- /dev/null +++ b/python/dartpy/dynamics/ZeroDofJoint.cpp @@ -0,0 +1,632 @@ +/* + * 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 +#include "Joint.hpp" + +namespace dart { +namespace python { + +void ZeroDofJoint(pybind11::module& m) +{ + ::pybind11::class_( + m, "ZeroDofJointProperties") + .def(::pybind11::init<>()) + .def( + ::pybind11::init(), + ::pybind11::arg("properties")); + + ::pybind11::class_< + dart::dynamics::ZeroDofJoint, + dart::dynamics::Joint, + std::shared_ptr >(m, "ZeroDofJoint") + .def( + "getZeroDofJointProperties", + +[](const dart::dynamics::ZeroDofJoint* self) + -> dart::dynamics::ZeroDofJoint::Properties { + return self->getZeroDofJointProperties(); + }) + .def( + "setDofName", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _arg0_, + const std::string& _arg1_, + bool _arg2_) -> const std::string& { + return self->setDofName(_arg0_, _arg1_, _arg2_); + }, + ::pybind11::return_value_policy::reference_internal, + ::pybind11::arg("arg0_"), + ::pybind11::arg("arg1_"), + ::pybind11::arg("arg2_")) + .def( + "preserveDofName", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _arg0_, + bool _arg1_) { self->preserveDofName(_arg0_, _arg1_); }, + ::pybind11::arg("arg0_"), + ::pybind11::arg("arg1_")) + .def( + "isDofNamePreserved", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _arg0_) + -> bool { return self->isDofNamePreserved(_arg0_); }, + ::pybind11::arg("arg0_")) + .def( + "getDofName", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _arg0_) + -> const std::string& { return self->getDofName(_arg0_); }, + ::pybind11::return_value_policy::reference_internal, + ::pybind11::arg("arg0_")) + .def( + "getNumDofs", + +[](const dart::dynamics::ZeroDofJoint* self) -> std::size_t { + return self->getNumDofs(); + }) + .def( + "getIndexInSkeleton", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> std::size_t { return self->getIndexInSkeleton(_index); }, + ::pybind11::arg("index")) + .def( + "getIndexInTree", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> std::size_t { return self->getIndexInTree(_index); }, + ::pybind11::arg("index")) + .def( + "setCommand", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _command) { self->setCommand(_index, _command); }, + ::pybind11::arg("index"), + ::pybind11::arg("command")) + .def( + "getCommand", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getCommand(_index); }, + ::pybind11::arg("index")) + .def( + "setCommands", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& _commands) { + self->setCommands(_commands); + }, + ::pybind11::arg("commands")) + .def( + "getCommands", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getCommands(); + }) + .def( + "resetCommands", + +[](dart::dynamics::ZeroDofJoint* self) { self->resetCommands(); }) + .def( + "setPosition", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _arg0_, + double _arg1_) { self->setPosition(_arg0_, _arg1_); }, + ::pybind11::arg("arg0_"), + ::pybind11::arg("arg1_")) + .def( + "getPosition", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getPosition(_index); }, + ::pybind11::arg("index")) + .def( + "setPositions", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& _positions) { + self->setPositions(_positions); + }, + ::pybind11::arg("positions")) + .def( + "getPositions", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getPositions(); + }) + .def( + "setPositionLowerLimit", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _position) { + self->setPositionLowerLimit(_index, _position); + }, + ::pybind11::arg("index"), + ::pybind11::arg("position")) + .def( + "getPositionLowerLimit", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getPositionLowerLimit(_index); }, + ::pybind11::arg("index")) + .def( + "setPositionLowerLimits", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& lowerLimits) { + self->setPositionLowerLimits(lowerLimits); + }, + ::pybind11::arg("lowerLimits")) + .def( + "getPositionLowerLimits", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getPositionLowerLimits(); + }) + .def( + "setPositionUpperLimit", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t index, + double position) { + self->setPositionUpperLimit(index, position); + }, + ::pybind11::arg("index"), + ::pybind11::arg("position")) + .def( + "getPositionUpperLimit", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t index) + -> double { return self->getPositionUpperLimit(index); }, + ::pybind11::arg("index")) + .def( + "setPositionUpperLimits", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& upperLimits) { + self->setPositionUpperLimits(upperLimits); + }, + ::pybind11::arg("upperLimits")) + .def( + "getPositionUpperLimits", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getPositionUpperLimits(); + }) + .def( + "hasPositionLimit", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> bool { return self->hasPositionLimit(_index); }, + ::pybind11::arg("index")) + .def( + "resetPosition", + +[](dart::dynamics::ZeroDofJoint* self, std::size_t _index) { + self->resetPosition(_index); + }, + ::pybind11::arg("index")) + .def( + "resetPositions", + +[](dart::dynamics::ZeroDofJoint* self) { self->resetPositions(); }) + .def( + "setInitialPosition", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _initial) { self->setInitialPosition(_index, _initial); }, + ::pybind11::arg("index"), + ::pybind11::arg("initial")) + .def( + "getInitialPosition", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getInitialPosition(_index); }, + ::pybind11::arg("index")) + .def( + "setInitialPositions", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& _initial) { + self->setInitialPositions(_initial); + }, + ::pybind11::arg("initial")) + .def( + "getInitialPositions", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getInitialPositions(); + }) + .def( + "setVelocity", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _velocity) { self->setVelocity(_index, _velocity); }, + ::pybind11::arg("index"), + ::pybind11::arg("velocity")) + .def( + "getVelocity", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getVelocity(_index); }, + ::pybind11::arg("index")) + .def( + "setVelocities", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& _velocities) { + self->setVelocities(_velocities); + }, + ::pybind11::arg("velocities")) + .def( + "getVelocities", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getVelocities(); + }) + .def( + "setVelocityLowerLimit", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _velocity) { + self->setVelocityLowerLimit(_index, _velocity); + }, + ::pybind11::arg("index"), + ::pybind11::arg("velocity")) + .def( + "getVelocityLowerLimit", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getVelocityLowerLimit(_index); }, + ::pybind11::arg("index")) + .def( + "setVelocityLowerLimits", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& lowerLimits) { + self->setVelocityLowerLimits(lowerLimits); + }, + ::pybind11::arg("lowerLimits")) + .def( + "getVelocityLowerLimits", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getVelocityLowerLimits(); + }) + .def( + "setVelocityUpperLimit", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _velocity) { + self->setVelocityUpperLimit(_index, _velocity); + }, + ::pybind11::arg("index"), + ::pybind11::arg("velocity")) + .def( + "getVelocityUpperLimit", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getVelocityUpperLimit(_index); }, + ::pybind11::arg("index")) + .def( + "setVelocityUpperLimits", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& upperLimits) { + self->setVelocityUpperLimits(upperLimits); + }, + ::pybind11::arg("upperLimits")) + .def( + "getVelocityUpperLimits", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getVelocityUpperLimits(); + }) + .def( + "resetVelocity", + +[](dart::dynamics::ZeroDofJoint* self, std::size_t _index) { + self->resetVelocity(_index); + }, + ::pybind11::arg("index")) + .def( + "resetVelocities", + +[](dart::dynamics::ZeroDofJoint* self) { self->resetVelocities(); }) + .def( + "setInitialVelocity", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _initial) { self->setInitialVelocity(_index, _initial); }, + ::pybind11::arg("index"), + ::pybind11::arg("initial")) + .def( + "getInitialVelocity", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getInitialVelocity(_index); }, + ::pybind11::arg("index")) + .def( + "setInitialVelocities", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& _initial) { + self->setInitialVelocities(_initial); + }, + ::pybind11::arg("initial")) + .def( + "getInitialVelocities", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getInitialVelocities(); + }) + .def( + "setAcceleration", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _acceleration) { + self->setAcceleration(_index, _acceleration); + }, + ::pybind11::arg("index"), + ::pybind11::arg("acceleration")) + .def( + "getAcceleration", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getAcceleration(_index); }, + ::pybind11::arg("index")) + .def( + "setAccelerations", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& _accelerations) { + self->setAccelerations(_accelerations); + }, + ::pybind11::arg("accelerations")) + .def( + "getAccelerations", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getAccelerations(); + }) + .def( + "resetAccelerations", + +[](dart::dynamics::ZeroDofJoint* self) { + self->resetAccelerations(); + }) + .def( + "setAccelerationLowerLimit", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _acceleration) { + self->setAccelerationLowerLimit(_index, _acceleration); + }, + ::pybind11::arg("index"), + ::pybind11::arg("acceleration")) + .def( + "getAccelerationLowerLimit", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getAccelerationLowerLimit(_index); }, + ::pybind11::arg("index")) + .def( + "setAccelerationLowerLimits", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& lowerLimits) { + self->setAccelerationLowerLimits(lowerLimits); + }, + ::pybind11::arg("lowerLimits")) + .def( + "getAccelerationLowerLimits", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getAccelerationLowerLimits(); + }) + .def( + "setAccelerationUpperLimit", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _acceleration) { + self->setAccelerationUpperLimit(_index, _acceleration); + }, + ::pybind11::arg("index"), + ::pybind11::arg("acceleration")) + .def( + "getAccelerationUpperLimit", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getAccelerationUpperLimit(_index); }, + ::pybind11::arg("index")) + .def( + "setAccelerationUpperLimits", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& upperLimits) { + self->setAccelerationUpperLimits(upperLimits); + }, + ::pybind11::arg("upperLimits")) + .def( + "getAccelerationUpperLimits", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getAccelerationUpperLimits(); + }) + .def( + "setForce", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _force) { self->setForce(_index, _force); }, + ::pybind11::arg("index"), + ::pybind11::arg("force")) + .def( + "getForce", + +[](const dart::dynamics::ZeroDofJoint* self, + std::size_t _index) -> double { return self->getForce(_index); }, + ::pybind11::arg("index")) + .def( + "setForces", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& _forces) { self->setForces(_forces); }, + ::pybind11::arg("forces")) + .def( + "getForces", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getForces(); + }) + .def( + "resetForces", + +[](dart::dynamics::ZeroDofJoint* self) { self->resetForces(); }) + .def( + "setForceLowerLimit", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _force) { self->setForceLowerLimit(_index, _force); }, + ::pybind11::arg("index"), + ::pybind11::arg("force")) + .def( + "getForceLowerLimit", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getForceLowerLimit(_index); }, + ::pybind11::arg("index")) + .def( + "setForceLowerLimits", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& lowerLimits) { + self->setForceLowerLimits(lowerLimits); + }, + ::pybind11::arg("lowerLimits")) + .def( + "getForceLowerLimits", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getForceLowerLimits(); + }) + .def( + "setForceUpperLimit", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _force) { self->setForceUpperLimit(_index, _force); }, + ::pybind11::arg("index"), + ::pybind11::arg("force")) + .def( + "getForceUpperLimit", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getForceUpperLimit(_index); }, + ::pybind11::arg("index")) + .def( + "setForceUpperLimits", + +[](dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& upperLimits) { + self->setForceUpperLimits(upperLimits); + }, + ::pybind11::arg("upperLimits")) + .def( + "getForceUpperLimits", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::VectorXd { + return self->getForceUpperLimits(); + }) + .def( + "setVelocityChange", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _velocityChange) { + self->setVelocityChange(_index, _velocityChange); + }, + ::pybind11::arg("index"), + ::pybind11::arg("velocityChange")) + .def( + "getVelocityChange", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getVelocityChange(_index); }, + ::pybind11::arg("index")) + .def( + "resetVelocityChanges", + +[](dart::dynamics::ZeroDofJoint* + self) { self->resetVelocityChanges(); }) + .def( + "setConstraintImpulse", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _impulse) { + self->setConstraintImpulse(_index, _impulse); + }, + ::pybind11::arg("index"), + ::pybind11::arg("impulse")) + .def( + "getConstraintImpulse", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getConstraintImpulse(_index); }, + ::pybind11::arg("index")) + .def( + "resetConstraintImpulses", + +[](dart::dynamics::ZeroDofJoint* + self) { self->resetConstraintImpulses(); }) + .def( + "integratePositions", + +[](dart::dynamics::ZeroDofJoint* self, double _dt) { + self->integratePositions(_dt); + }, + ::pybind11::arg("dt")) + .def( + "integrateVelocities", + +[](dart::dynamics::ZeroDofJoint* self, double _dt) { + self->integrateVelocities(_dt); + }, + ::pybind11::arg("dt")) + .def( + "getPositionDifferences", + +[](const dart::dynamics::ZeroDofJoint* self, + const Eigen::VectorXd& _q2, + const Eigen::VectorXd& _q1) -> Eigen::VectorXd { + return self->getPositionDifferences(_q2, _q1); + }, + ::pybind11::arg("q2"), + ::pybind11::arg("q1")) + .def( + "setSpringStiffness", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _k) { self->setSpringStiffness(_index, _k); }, + ::pybind11::arg("index"), + ::pybind11::arg("k")) + .def( + "getSpringStiffness", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getSpringStiffness(_index); }, + ::pybind11::arg("index")) + .def( + "setRestPosition", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _q0) { self->setRestPosition(_index, _q0); }, + ::pybind11::arg("index"), + ::pybind11::arg("q0")) + .def( + "getRestPosition", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getRestPosition(_index); }, + ::pybind11::arg("index")) + .def( + "setDampingCoefficient", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _d) { self->setDampingCoefficient(_index, _d); }, + ::pybind11::arg("index"), + ::pybind11::arg("d")) + .def( + "getDampingCoefficient", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getDampingCoefficient(_index); }, + ::pybind11::arg("index")) + .def( + "setCoulombFriction", + +[](dart::dynamics::ZeroDofJoint* self, + std::size_t _index, + double _friction) { + self->setCoulombFriction(_index, _friction); + }, + ::pybind11::arg("index"), + ::pybind11::arg("friction")) + .def( + "getCoulombFriction", + +[](const dart::dynamics::ZeroDofJoint* self, std::size_t _index) + -> double { return self->getCoulombFriction(_index); }, + ::pybind11::arg("index")) + .def( + "computePotentialEnergy", + +[](const dart::dynamics::ZeroDofJoint* self) -> double { + return self->computePotentialEnergy(); + }) + .def( + "getBodyConstraintWrench", + +[](const dart::dynamics::ZeroDofJoint* self) -> Eigen::Vector6d { + return self->getBodyConstraintWrench(); + }); +} + +} // namespace python +} // namespace dart diff --git a/python/dartpy/dynamics/module.cpp b/python/dartpy/dynamics/module.cpp index 6adf67636cfb5..202ceabbcbb61 100644 --- a/python/dartpy/dynamics/module.cpp +++ b/python/dartpy/dynamics/module.cpp @@ -51,9 +51,18 @@ void DegreeOfFreedom(pybind11::module& sm); void BodyNode(pybind11::module& sm); void Joint(pybind11::module& sm); +void ZeroDofJoint(pybind11::module& sm); +void WeldJoint(pybind11::module& sm); void GenericJoint(pybind11::module& sm); void RevoluteJoint(pybind11::module& sm); +void PrismaticJoint(pybind11::module& sm); +void ScrewJoint(pybind11::module& sm); +void UniversalJoint(pybind11::module& sm); +void TranslationalJoint2D(pybind11::module& sm); +void PlanarJoint(pybind11::module& sm); +void EulerJoint(pybind11::module& sm); void BallJoint(pybind11::module& sm); +void TranslationalJoint(pybind11::module& sm); void FreeJoint(pybind11::module& sm); void MetaSkeleton(pybind11::module& sm); @@ -84,9 +93,18 @@ void dart_dynamics(pybind11::module& m) BodyNode(sm); Joint(sm); + ZeroDofJoint(sm); + WeldJoint(sm); GenericJoint(sm); RevoluteJoint(sm); + PrismaticJoint(sm); + ScrewJoint(sm); + UniversalJoint(sm); + TranslationalJoint2D(sm); + PlanarJoint(sm); + EulerJoint(sm); BallJoint(sm); + TranslationalJoint(sm); FreeJoint(sm); MetaSkeleton(sm); diff --git a/python/dartpy/math/EulerAngles.cpp b/python/dartpy/math/Geometry.cpp similarity index 73% rename from python/dartpy/math/EulerAngles.cpp rename to python/dartpy/math/Geometry.cpp index cadf01781dc2e..4162fe48d50df 100644 --- a/python/dartpy/math/EulerAngles.cpp +++ b/python/dartpy/math/Geometry.cpp @@ -56,7 +56,7 @@ namespace dart { namespace python { -void EulerAngles(pybind11::module& m) +void Geometry(pybind11::module& m) { DARTPY_DEFINE_EULAERTOMATRIX(XYX); DARTPY_DEFINE_EULAERTOMATRIX(XYZ); @@ -83,6 +83,62 @@ void EulerAngles(pybind11::module& m) DARTPY_DEFINE_MATRIXTOEULAER(ZYX); // DARTPY_DEFINE_MATRIXTOEULAER(ZXZ); // DARTPY_DEFINE_MATRIXTOEULAER(ZYZ); + + m.def( + "expMap", + +[](const Eigen::Vector6d& _S) -> Eigen::Isometry3d { + return dart::math::expMap(_S); + }, + ::pybind11::arg("S")); + + m.def( + "expMapJac", + +[](const Eigen::Vector3d& _expmap) -> Eigen::Matrix3d { + return dart::math::expMapJac(_expmap); + }, + ::pybind11::arg("expmap")); + + m.def( + "expMapRot", + +[](const Eigen::Vector3d& _expmap) -> Eigen::Matrix3d { + return dart::math::expMapRot(_expmap); + }, + ::pybind11::arg("expmap")); + + m.def( + "expToQuat", + +[](const Eigen::Vector3d& _v) -> Eigen::Quaterniond { + return dart::math::expToQuat(_v); + }, + ::pybind11::arg("v")); + + m.def( + "quatToExp", + +[](const Eigen::Quaterniond& _q) -> Eigen::Vector3d { + return dart::math::quatToExp(_q); + }, + ::pybind11::arg("q")); + + m.def( + "expAngular", + +[](const Eigen::Vector3d& _s) -> Eigen::Isometry3d { + return dart::math::expAngular(_s); + }, + ::pybind11::arg("s")); + + m.def( + "verifyRotation", + +[](const Eigen::Matrix3d& _R) -> bool { + return dart::math::verifyRotation(_R); + }, + ::pybind11::arg("R")); + + m.def( + "verifyTransform", + +[](const Eigen::Isometry3d& _T) -> bool { + return dart::math::verifyTransform(_T); + }, + ::pybind11::arg("T")); } } // namespace python diff --git a/python/dartpy/math/module.cpp b/python/dartpy/math/module.cpp index c81bf549eb25f..094fee9f5562f 100644 --- a/python/dartpy/math/module.cpp +++ b/python/dartpy/math/module.cpp @@ -36,14 +36,14 @@ namespace dart { namespace python { void Random(pybind11::module& sm); -void EulerAngles(pybind11::module& sm); +void Geometry(pybind11::module& sm); void dart_math(pybind11::module& m) { auto sm = m.def_submodule("math"); Random(sm); - EulerAngles(sm); + Geometry(sm); } } // namespace python diff --git a/python/tests/CMakeLists.txt b/python/tests/CMakeLists.txt index 44dc5c27a0368..4381cb8b49c5e 100644 --- a/python/tests/CMakeLists.txt +++ b/python/tests/CMakeLists.txt @@ -32,3 +32,27 @@ add_custom_target(pytest WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} SOURCES ${dartpy_test_files} ${dartpy_test_utils} ) + +#=============================================================================== +# Usage: +# dartpy_add_test(test_name) # assumed source is test_name.py +# dartpy_add_test(test_name main.py) +#=============================================================================== +function(dartpy_add_test test_name) # ARGN for source file + if(${ARGC} GREATER 1) + set(source ${ARGN}) + else() + set(source "${test_name}.py") + endif() + add_custom_target(${test_name} + COMMAND ${CMAKE_COMMAND} -E echo "Running pytest by: PYTHONPATH=${DART_DARTPY_BUILD_DIR} ${PYTHON_EXECUTABLE} ${source}" + COMMAND PYTHONPATH=${DART_DARTPY_BUILD_DIR} ${PYTHON_EXECUTABLE} ${source} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + SOURCES ${source} + ) +endfunction() + +foreach(dartpy_test_file ${dartpy_test_files}) + get_filename_component(dartpy_test_name ${dartpy_test_file} NAME_WE) + dartpy_add_test(py_${dartpy_test_name} ${dartpy_test_file}) +endforeach() diff --git a/python/tests/unit/dynamics/test_joint.py b/python/tests/unit/dynamics/test_joint.py index 57a204493f2b0..f5f743f6d22fa 100644 --- a/python/tests/unit/dynamics/test_joint.py +++ b/python/tests/unit/dynamics/test_joint.py @@ -1,11 +1,105 @@ -import platform +import math import pytest import numpy as np import dartpy as dart -def test_basic(): - pass +def kinematics_tester(joint): + num_tests = 2 + + joint.setTransformFromChildBodyNode(dart.math.expMap(np.random.rand(6))) + joint.setTransformFromParentBodyNode(dart.math.expMap(np.random.rand(6))) + + dof = joint.getNumDofs() + + q = np.zeros(dof) + dq = np.zeros(dof) + + for _ in range(num_tests): + q_delta = 1e-5 + + for i in range(dof): + q[i] = dart.math.Random.uniform(-math.pi, math.pi) + dq[i] = dart.math.Random.uniform(-math.pi, math.pi) + + joint.setPositions(q) + joint.setVelocities(dq) + + if dof == 0: + return + + T = joint.getRelativeTransform() + J = joint.getRelativeJacobian(q) + dJ = joint.getRelativeJacobianTimeDeriv() + + # Verify transform + assert dart.math.verifyTransform(T) + + # Test analytic Jacobian and numerical Jacobian + numericJ = np.zeros((6, dof)) + for i in range(dof): + q_a = q.copy() + joint.setPositions(q_a) + T_a = joint.getRelativeTransform() + + q_b = q.copy() + q_b[i] += q_delta + joint.setPositions(q_b) + T_b = joint.getRelativeTransform() + + Tinv_a = T_a.inverse() + + dTdq = (T_b.matrix() - T_a.matrix()) / q_delta + + Ji_4x4matrix = np.matmul(Tinv_a.matrix(), dTdq) + Ji = np.zeros(6) + Ji[0] = Ji_4x4matrix[2, 1] + Ji[1] = Ji_4x4matrix[0, 2] + Ji[2] = Ji_4x4matrix[1, 0] + Ji[3] = Ji_4x4matrix[0, 3] + Ji[4] = Ji_4x4matrix[1, 3] + Ji[5] = Ji_4x4matrix[2, 3] + numericJ[:,i] = Ji + + assert np.allclose(J, numericJ, atol=1e-5) + + +def test_kinematics(): + skel = dart.dynamics.Skeleton() + joint, _ = skel.createWeldJointAndBodyNodePair() + kinematics_tester(joint) + + skel = dart.dynamics.Skeleton() + joint, _ = skel.createRevoluteJointAndBodyNodePair() + kinematics_tester(joint) + + skel = dart.dynamics.Skeleton() + joint, _ = skel.createPrismaticJointAndBodyNodePair() + kinematics_tester(joint) + + skel = dart.dynamics.Skeleton() + joint, _ = skel.createScrewJointAndBodyNodePair() + kinematics_tester(joint) + + skel = dart.dynamics.Skeleton() + joint, _ = skel.createUniversalJointAndBodyNodePair() + kinematics_tester(joint) + + skel = dart.dynamics.Skeleton() + joint, _ = skel.createTranslationalJoint2DAndBodyNodePair() + kinematics_tester(joint) + + skel = dart.dynamics.Skeleton() + joint, _ = skel.createEulerJointAndBodyNodePair() + kinematics_tester(joint) + + skel = dart.dynamics.Skeleton() + joint, _ = skel.createTranslationalJointAndBodyNodePair() + kinematics_tester(joint) + + skel = dart.dynamics.Skeleton() + joint, _ = skel.createPlanarJointAndBodyNodePair() + kinematics_tester(joint) if __name__ == "__main__": diff --git a/unittests/comprehensive/test_Joints.cpp b/unittests/comprehensive/test_Joints.cpp index 75cba6e616d7e..6f174f27bf3d7 100644 --- a/unittests/comprehensive/test_Joints.cpp +++ b/unittests/comprehensive/test_Joints.cpp @@ -163,8 +163,8 @@ void JOINTS::kinematicsTest(const typename JointType::Properties& _properties) dq(i) = Random::uniform(-constantsd::pi()*1.0, constantsd::pi()*1.0); } - skeleton->setPositions(q); - skeleton->setVelocities(dq); + joint->setPositions(q); + joint->setVelocities(dq); if (dof == 0) return;