diff --git a/CHANGELOG.md b/CHANGELOG.md index cb8671ff7bf24..3d5e37e592309 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,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/config.hpp.in b/dart/config.hpp.in index f442b7cabf40e..a1f52a1259861 100644 --- a/dart/config.hpp.in +++ b/dart/config.hpp.in @@ -1,4 +1,4 @@ -/* config.h. Generated by CMake for @PROJECT_NAME@. */ +/* config.hpp. Generated by CMake for @PROJECT_NAME@. */ #ifndef DART_CONFIG_HPP_ #define DART_CONFIG_HPP_ diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 5aa5bb0952e94..7600dd998fea8 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 c7ce02b5e614b..d900d38a78d0d 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;