diff --git a/CHANGELOG.md b/CHANGELOG.md index 10b679a9a50d..4dc6d453c9ba 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ * Misc + * Added Skeleton::getRootJoint(): [#832](https://github.com/dartsim/dart/pull/832) * Added CMake targets for code formatting using clang-format: [#811](https://github.com/dartsim/dart/pull/811), [#817](https://github.com/dartsim/dart/pull/817) ### DART 6.1.2 (2016-XX-XX) diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 5aa5bb0952e9..7600dd998fea 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -796,6 +796,23 @@ const BodyNode* Skeleton::getRootBodyNode(std::size_t _treeIdx) const return const_cast(this)->getRootBodyNode(_treeIdx); } +//============================================================================== +Joint* Skeleton::getRootJoint(std::size_t treeIdx) +{ + auto rootBodyNode = getRootBodyNode(treeIdx); + + if (rootBodyNode) + return rootBodyNode->getParentJoint(); + + return nullptr; +} + +//============================================================================== +const Joint* Skeleton::getRootJoint(std::size_t treeIdx) const +{ + return const_cast(this)->getRootJoint(treeIdx); +} + //============================================================================== BodyNode* Skeleton::getBodyNode(std::size_t _idx) { diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index c7ce02b5e614..d900d38a78d0 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -344,6 +344,13 @@ class Skeleton : /// _treeIdx const BodyNode* getRootBodyNode(std::size_t _treeIdx = 0) const; + /// Get the root Joint of the tree whose index in this Skeleton is treeIdx + Joint* getRootJoint(std::size_t treeIdx = 0u); + + /// Get the const root Joint of the tree whose index in this Skeleton is + /// treeIdx + const Joint* getRootJoint(std::size_t treeIdx = 0u) const; + // Documentation inherited BodyNode* getBodyNode(std::size_t _idx) override;