Skeleton.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011-2021, 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.
 */
 
#ifndef DART_DYNAMICS_SKELETON_HPP_
#define DART_DYNAMICS_SKELETON_HPP_
 
#include <mutex>
#include "dart/common/NameManager.hpp"
#include "dart/common/VersionCounter.hpp"
#include "dart/dynamics/EndEffector.hpp"
#include "dart/dynamics/HierarchicalIK.hpp"
#include "dart/dynamics/Joint.hpp"
#include "dart/dynamics/Marker.hpp"
#include "dart/dynamics/MetaSkeleton.hpp"
#include "dart/dynamics/ShapeNode.hpp"
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/dynamics/SpecializedNodeManager.hpp"
#include "dart/dynamics/detail/BodyNodeAspect.hpp"
#include "dart/dynamics/detail/SkeletonAspect.hpp"
 
namespace dart {
namespace dynamics {
 
DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
class Skeleton : public virtual common::VersionCounter,
                 public MetaSkeleton,
                 public SkeletonSpecializedFor<ShapeNode, EndEffector, Marker>,
                 public detail::SkeletonAspectBase
{
public:
  // Some of non-virtual functions of MetaSkeleton are hidden because of the
  // functions of the same name in this class. We expose those functions as
  // follows.
  using MetaSkeleton::getAngularJacobian;
  using MetaSkeleton::getAngularJacobianDeriv;
  using MetaSkeleton::getJacobian;
  using MetaSkeleton::getJacobianClassicDeriv;
  using MetaSkeleton::getJacobianSpatialDeriv;
  using MetaSkeleton::getLinearJacobian;
  using MetaSkeleton::getLinearJacobianDeriv;
 
  using AspectPropertiesData = detail::SkeletonAspectProperties;
  using AspectProperties = common::Aspect::MakeProperties<AspectPropertiesData>;
 
  using State = common::Composite::State;
  using Properties = common::Composite::Properties;
 
  enum ConfigFlags
  {
    CONFIG_NOTHING = 0,
    CONFIG_POSITIONS = 1 << 1,
    CONFIG_VELOCITIES = 1 << 2,
    CONFIG_ACCELERATIONS = 1 << 3,
    CONFIG_FORCES = 1 << 4,
    CONFIG_COMMANDS = 1 << 5,
    CONFIG_ALL = 0xFF
  };
 
  struct Configuration
  {
    Configuration(
        const Eigen::VectorXd& positions = Eigen::VectorXd(),
        const Eigen::VectorXd& velocities = Eigen::VectorXd(),
        const Eigen::VectorXd& accelerations = Eigen::VectorXd(),
        const Eigen::VectorXd& forces = Eigen::VectorXd(),
        const Eigen::VectorXd& commands = Eigen::VectorXd());
 
    Configuration(
        const std::vector<std::size_t>& indices,
        const Eigen::VectorXd& positions = Eigen::VectorXd(),
        const Eigen::VectorXd& velocities = Eigen::VectorXd(),
        const Eigen::VectorXd& accelerations = Eigen::VectorXd(),
        const Eigen::VectorXd& forces = Eigen::VectorXd(),
        const Eigen::VectorXd& commands = Eigen::VectorXd());
 
    std::vector<std::size_t> mIndices;
 
    Eigen::VectorXd mPositions;
 
    Eigen::VectorXd mVelocities;
 
    Eigen::VectorXd mAccelerations;
 
    Eigen::VectorXd mForces;
 
    Eigen::VectorXd mCommands;
 
    bool operator==(const Configuration& other) const;
 
    bool operator!=(const Configuration& other) const;
  };
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  static SkeletonPtr create(const std::string& _name = "Skeleton");
 
  static SkeletonPtr create(const AspectPropertiesData& properties);
 
  SkeletonPtr getPtr();
 
  ConstSkeletonPtr getPtr() const;
 
  SkeletonPtr getSkeleton();
 
  ConstSkeletonPtr getSkeleton() const;
 
  std::mutex& getMutex() const;
 
  std::unique_ptr<common::LockableReference> getLockableReference()
      const override;
 
  Skeleton(const Skeleton&) = delete;
 
  virtual ~Skeleton();
 
  Skeleton& operator=(const Skeleton& _other) = delete;
 
  DART_DEPRECATED(6.7)
  SkeletonPtr clone() const;
  // TODO: In DART 7, change this function to override MetaSkeleton::clone()
  // that returns MetaSkeletonPtr
 
  DART_DEPRECATED(6.7)
  SkeletonPtr clone(const std::string& cloneName) const;
  // TODO: In DART 7, change this function to override MetaSkeleton::clone()
  // that returns MetaSkeletonPtr
 
  SkeletonPtr cloneSkeleton() const;
 
  SkeletonPtr cloneSkeleton(const std::string& cloneName) const;
 
  // Documentation inherited
  MetaSkeletonPtr cloneMetaSkeleton(
      const std::string& cloneName) const override;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  void setConfiguration(const Configuration& configuration);
 
  Configuration getConfiguration(int flags = CONFIG_ALL) const;
 
  Configuration getConfiguration(
      const std::vector<std::size_t>& indices, int flags = CONFIG_ALL) const;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  void setState(const State& state);
 
  State getState() const;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  void setProperties(const Properties& properties);
 
  Properties getProperties() const;
 
  void setProperties(const AspectProperties& properties);
 
  DART_DEPRECATED(6.0)
  const AspectProperties& getSkeletonProperties() const;
 
  void setAspectProperties(const AspectProperties& properties);
 
  const std::string& setName(const std::string& _name) override;
 
  const std::string& getName() const override;
 
  DART_DEPRECATED(6.0)
  void enableSelfCollision(bool enableAdjacentBodyCheck = false);
 
  DART_DEPRECATED(6.0)
  void disableSelfCollision();
 
  void setSelfCollisionCheck(bool enable);
 
  bool getSelfCollisionCheck() const;
 
  void enableSelfCollisionCheck();
 
  void disableSelfCollisionCheck();
 
  bool isEnabledSelfCollisionCheck() const;
 
  void setAdjacentBodyCheck(bool enable);
 
  bool getAdjacentBodyCheck() const;
 
  void enableAdjacentBodyCheck();
 
  void disableAdjacentBodyCheck();
 
  bool isEnabledAdjacentBodyCheck() const;
 
  void setMobile(bool _isMobile);
 
  bool isMobile() const;
 
  void setTimeStep(double _timeStep);
 
  double getTimeStep() const;
 
  void setGravity(const Eigen::Vector3d& _gravity);
 
  const Eigen::Vector3d& getGravity() const;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  template <class JointType, class NodeType = BodyNode>
  std::pair<JointType*, NodeType*> createJointAndBodyNodePair(
      BodyNode* _parent = nullptr,
      const typename JointType::Properties& _jointProperties
      = typename JointType::Properties(),
      const typename NodeType::Properties& _bodyProperties
      = typename NodeType::Properties());
 
  // Documentation inherited
  std::size_t getNumBodyNodes() const override;
 
  std::size_t getNumRigidBodyNodes() const;
 
  std::size_t getNumSoftBodyNodes() const;
 
  std::size_t getNumTrees() const;
 
  BodyNode* getRootBodyNode(std::size_t _treeIdx = 0);
 
  const BodyNode* getRootBodyNode(std::size_t _treeIdx = 0) const;
 
  Joint* getRootJoint(std::size_t treeIdx = 0u);
 
  const Joint* getRootJoint(std::size_t treeIdx = 0u) const;
 
  // Documentation inherited
  BodyNode* getBodyNode(std::size_t _idx) override;
 
  // Documentation inherited
  const BodyNode* getBodyNode(std::size_t _idx) const override;
 
  SoftBodyNode* getSoftBodyNode(std::size_t _idx);
 
  const SoftBodyNode* getSoftBodyNode(std::size_t _idx) const;
 
  // Documentation inherited
  BodyNode* getBodyNode(const std::string& name) override;
 
  // Documentation inherited
  const BodyNode* getBodyNode(const std::string& name) const override;
 
  SoftBodyNode* getSoftBodyNode(const std::string& _name);
 
  const SoftBodyNode* getSoftBodyNode(const std::string& _name) const;
 
  // Documentation inherited
  const std::vector<BodyNode*>& getBodyNodes() override;
 
  // Documentation inherited
  const std::vector<const BodyNode*>& getBodyNodes() const override;
 
  std::vector<BodyNode*> getBodyNodes(const std::string& name) override;
 
  std::vector<const BodyNode*> getBodyNodes(
      const std::string& name) const override;
 
  // Documentation inherited
  bool hasBodyNode(const BodyNode* bodyNode) const override;
 
  // Documentation inherited
  std::size_t getIndexOf(
      const BodyNode* _bn, bool _warning = true) const override;
 
  const std::vector<BodyNode*>& getTreeBodyNodes(std::size_t _treeIdx);
 
  std::vector<const BodyNode*> getTreeBodyNodes(std::size_t _treeIdx) const;
 
  // Documentation inherited
  std::size_t getNumJoints() const override;
 
  // Documentation inherited
  Joint* getJoint(std::size_t _idx) override;
 
  // Documentation inherited
  const Joint* getJoint(std::size_t _idx) const override;
 
  // Documentation inherited
  Joint* getJoint(const std::string& name) override;
 
  // Documentation inherited
  const Joint* getJoint(const std::string& name) const override;
 
  // Documentation inherited
  std::vector<Joint*> getJoints() override;
 
  // Documentation inherited
  std::vector<const Joint*> getJoints() const override;
 
  std::vector<Joint*> getJoints(const std::string& name) override;
 
  std::vector<const Joint*> getJoints(const std::string& name) const override;
 
  // Documentation inherited
  bool hasJoint(const Joint* joint) const override;
 
  // Documentation inherited
  std::size_t getIndexOf(
      const Joint* _joint, bool _warning = true) const override;
 
  // Documentation inherited
  std::size_t getNumDofs() const override;
 
  // Documentation inherited
  DegreeOfFreedom* getDof(std::size_t _idx) override;
 
  // Documentation inherited
  const DegreeOfFreedom* getDof(std::size_t _idx) const override;
 
  DegreeOfFreedom* getDof(const std::string& _name);
 
  const DegreeOfFreedom* getDof(const std::string& _name) const;
 
  // Documentation inherited
  const std::vector<DegreeOfFreedom*>& getDofs() override;
 
  // Documentation inherited
  std::vector<const DegreeOfFreedom*> getDofs() const override;
 
  // Documentation inherited
  std::size_t getIndexOf(
      const DegreeOfFreedom* _dof, bool _warning = true) const override;
 
  const std::vector<DegreeOfFreedom*>& getTreeDofs(std::size_t _treeIdx);
 
  const std::vector<const DegreeOfFreedom*>& getTreeDofs(
      std::size_t _treeIdx) const;
 
  bool checkIndexingConsistency() const;
 
  const std::shared_ptr<WholeBodyIK>& getIK(bool _createIfNull = false);
 
  const std::shared_ptr<WholeBodyIK>& getOrCreateIK();
 
  std::shared_ptr<const WholeBodyIK> getIK() const;
 
  const std::shared_ptr<WholeBodyIK>& createIK();
 
  void clearIK();
 
  DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(Marker)
 
  DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(ShapeNode)
 
  DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(EndEffector)
 
 
  //----------------------------------------------------------------------------
  // Integration and finite difference
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  void integratePositions(double _dt);
 
  // Documentation inherited
  void integrateVelocities(double _dt);
 
  Eigen::VectorXd getPositionDifferences(
      const Eigen::VectorXd& _q2, const Eigen::VectorXd& _q1) const;
 
  Eigen::VectorXd getVelocityDifferences(
      const Eigen::VectorXd& _dq2, const Eigen::VectorXd& _dq1) const;
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  const math::SupportPolygon& getSupportPolygon() const;
 
  const math::SupportPolygon& getSupportPolygon(std::size_t _treeIdx) const;
 
  const std::vector<std::size_t>& getSupportIndices() const;
 
  const std::vector<std::size_t>& getSupportIndices(std::size_t _treeIdx) const;
 
  const std::pair<Eigen::Vector3d, Eigen::Vector3d>& getSupportAxes() const;
 
  const std::pair<Eigen::Vector3d, Eigen::Vector3d>& getSupportAxes(
      std::size_t _treeIdx) const;
 
  const Eigen::Vector2d& getSupportCentroid() const;
 
  const Eigen::Vector2d& getSupportCentroid(std::size_t _treeIdx) const;
 
  std::size_t getSupportVersion() const;
 
  std::size_t getSupportVersion(std::size_t _treeIdx) const;
 
 
  //----------------------------------------------------------------------------
  // Kinematics algorithms
  //----------------------------------------------------------------------------
 
  void computeForwardKinematics(
      bool _updateTransforms = true,
      bool _updateVels = true,
      bool _updateAccs = true);
 
  //----------------------------------------------------------------------------
  // Dynamics algorithms
  //----------------------------------------------------------------------------
 
  void computeForwardDynamics();
 
  void computeInverseDynamics(
      bool _withExternalForces = false,
      bool _withDampingForces = false,
      bool _withSpringForces = false);
 
  //----------------------------------------------------------------------------
  // Impulse-based dynamics algorithms
  //----------------------------------------------------------------------------
 
  void clearConstraintImpulses();
 
  void updateBiasImpulse(BodyNode* _bodyNode);
 
  void updateBiasImpulse(BodyNode* _bodyNode, const Eigen::Vector6d& _imp);
 
  void updateBiasImpulse(
      BodyNode* _bodyNode1,
      const Eigen::Vector6d& _imp1,
      BodyNode* _bodyNode2,
      const Eigen::Vector6d& _imp2);
 
  void updateBiasImpulse(
      SoftBodyNode* _softBodyNode,
      PointMass* _pointMass,
      const Eigen::Vector3d& _imp);
 
  void updateVelocityChange();
 
  // TODO(JS): Better naming
  void setImpulseApplied(bool _val);
 
  bool isImpulseApplied() const;
 
  void computeImpulseForwardDynamics();
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  math::Jacobian getJacobian(const JacobianNode* _node) const override;
 
  // Documentation inherited
  math::Jacobian getJacobian(
      const JacobianNode* _node, const Frame* _inCoordinatesOf) const override;
 
  // Documentation inherited
  math::Jacobian getJacobian(
      const JacobianNode* _node,
      const Eigen::Vector3d& _localOffset) const override;
 
  // Documentation inherited
  math::Jacobian getJacobian(
      const JacobianNode* _node,
      const Eigen::Vector3d& _localOffset,
      const Frame* _inCoordinatesOf) const override;
 
  // Documentation inherited
  math::Jacobian getWorldJacobian(const JacobianNode* _node) const override;
 
  // Documentation inherited
  math::Jacobian getWorldJacobian(
      const JacobianNode* _node,
      const Eigen::Vector3d& _localOffset) const override;
 
  // Documentation inherited
  math::LinearJacobian getLinearJacobian(
      const JacobianNode* _node,
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  // Documentation inherited
  math::LinearJacobian getLinearJacobian(
      const JacobianNode* _node,
      const Eigen::Vector3d& _localOffset,
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  // Documentation inherited
  math::AngularJacobian getAngularJacobian(
      const JacobianNode* _node,
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  // Documentation inherited
  math::Jacobian getJacobianSpatialDeriv(
      const JacobianNode* _node) const override;
 
  // Documentation inherited
  math::Jacobian getJacobianSpatialDeriv(
      const JacobianNode* _node, const Frame* _inCoordinatesOf) const override;
 
  // Documentation inherited
  math::Jacobian getJacobianSpatialDeriv(
      const JacobianNode* _node,
      const Eigen::Vector3d& _localOffset) const override;
 
  // Documentation inherited
  math::Jacobian getJacobianSpatialDeriv(
      const JacobianNode* _node,
      const Eigen::Vector3d& _localOffset,
      const Frame* _inCoordinatesOf) const override;
 
  // Documentation inherited
  math::Jacobian getJacobianClassicDeriv(
      const JacobianNode* _node) const override;
 
  // Documentation inherited
  math::Jacobian getJacobianClassicDeriv(
      const JacobianNode* _node, const Frame* _inCoordinatesOf) const override;
 
  // Documentation inherited
  math::Jacobian getJacobianClassicDeriv(
      const JacobianNode* _node,
      const Eigen::Vector3d& _localOffset,
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  // Documentation inherited
  math::LinearJacobian getLinearJacobianDeriv(
      const JacobianNode* _node,
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  // Documentation inherited
  math::LinearJacobian getLinearJacobianDeriv(
      const JacobianNode* _node,
      const Eigen::Vector3d& _localOffset,
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  // Documentation inherited
  math::AngularJacobian getAngularJacobianDeriv(
      const JacobianNode* _node,
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  double getMass() const override;
 
  const Eigen::MatrixXd& getMassMatrix(std::size_t _treeIdx) const;
 
  // Documentation inherited
  const Eigen::MatrixXd& getMassMatrix() const override;
 
  const Eigen::MatrixXd& getAugMassMatrix(std::size_t _treeIdx) const;
 
  // Documentation inherited
  const Eigen::MatrixXd& getAugMassMatrix() const override;
 
  const Eigen::MatrixXd& getInvMassMatrix(std::size_t _treeIdx) const;
 
  // Documentation inherited
  const Eigen::MatrixXd& getInvMassMatrix() const override;
 
  const Eigen::MatrixXd& getInvAugMassMatrix(std::size_t _treeIdx) const;
 
  // Documentation inherited
  const Eigen::MatrixXd& getInvAugMassMatrix() const override;
 
  const Eigen::VectorXd& getCoriolisForces(std::size_t _treeIdx) const;
 
  // Documentation inherited
  const Eigen::VectorXd& getCoriolisForces() const override;
 
  const Eigen::VectorXd& getGravityForces(std::size_t _treeIdx) const;
 
  // Documentation inherited
  const Eigen::VectorXd& getGravityForces() const override;
 
  const Eigen::VectorXd& getCoriolisAndGravityForces(
      std::size_t _treeIdx) const;
 
  // Documentation inherited
  const Eigen::VectorXd& getCoriolisAndGravityForces() const override;
 
  const Eigen::VectorXd& getExternalForces(std::size_t _treeIdx) const;
 
  // Documentation inherited
  const Eigen::VectorXd& getExternalForces() const override;
 
  //  const Eigen::VectorXd& getDampingForceVector();
 
  const Eigen::VectorXd& getConstraintForces(std::size_t _treeIdx) const;
 
  const Eigen::VectorXd& getConstraintForces() const override;
 
  // Documentation inherited
  void clearExternalForces() override;
 
  // Documentation inherited
  void clearInternalForces() override;
 
  DART_DEPRECATED(6.2)
  void notifyArticulatedInertiaUpdate(std::size_t _treeIdx);
 
  void dirtyArticulatedInertia(std::size_t _treeIdx);
 
  DART_DEPRECATED(6.2)
  void notifySupportUpdate(std::size_t _treeIdx);
 
  void dirtySupportPolygon(std::size_t _treeIdx);
 
  // Documentation inherited
  double computeKineticEnergy() const override;
 
  // Documentation inherited
  double computePotentialEnergy() const override;
 
  // Documentation inherited
  DART_DEPRECATED(6.0)
  void clearCollidingBodies() override;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  Eigen::Vector3d getCOM(
      const Frame* _withRespectTo = Frame::World()) const override;
 
  Eigen::Vector6d getCOMSpatialVelocity(
      const Frame* _relativeTo = Frame::World(),
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  Eigen::Vector3d getCOMLinearVelocity(
      const Frame* _relativeTo = Frame::World(),
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  Eigen::Vector6d getCOMSpatialAcceleration(
      const Frame* _relativeTo = Frame::World(),
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  Eigen::Vector3d getCOMLinearAcceleration(
      const Frame* _relativeTo = Frame::World(),
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  math::Jacobian getCOMJacobian(
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  math::LinearJacobian getCOMLinearJacobian(
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  math::Jacobian getCOMJacobianSpatialDeriv(
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
  math::LinearJacobian getCOMLinearJacobianDeriv(
      const Frame* _inCoordinatesOf = Frame::World()) const override;
 
 
  //----------------------------------------------------------------------------
  // Friendship
  //----------------------------------------------------------------------------
  friend class BodyNode;
  friend class SoftBodyNode;
  friend class Joint;
  template <class>
  friend class GenericJoint;
  friend class DegreeOfFreedom;
  friend class Node;
  friend class ShapeNode;
  friend class EndEffector;
 
protected:
  struct DataCache;
 
  Skeleton(const AspectPropertiesData& _properties);
 
  void setPtr(const SkeletonPtr& _ptr);
 
  void constructNewTree();
 
  void registerBodyNode(BodyNode* _newBodyNode);
 
  void registerJoint(Joint* _newJoint);
 
  void registerNode(NodeMap& nodeMap, Node* _newNode, std::size_t& _index);
 
  void registerNode(Node* _newNode);
 
  void destructOldTree(std::size_t tree);
 
  void unregisterBodyNode(BodyNode* _oldBodyNode);
 
  void unregisterJoint(Joint* _oldJoint);
 
  void unregisterNode(NodeMap& nodeMap, Node* _oldNode, std::size_t& _index);
 
  void unregisterNode(Node* _oldNode);
 
  bool moveBodyNodeTree(
      Joint* _parentJoint,
      BodyNode* _bodyNode,
      SkeletonPtr _newSkeleton,
      BodyNode* _parentNode);
 
  template <class JointType>
  JointType* moveBodyNodeTree(
      BodyNode* _bodyNode,
      const SkeletonPtr& _newSkeleton,
      BodyNode* _parentNode,
      const typename JointType::Properties& _joint);
 
  std::pair<Joint*, BodyNode*> cloneBodyNodeTree(
      Joint* _parentJoint,
      const BodyNode* _bodyNode,
      const SkeletonPtr& _newSkeleton,
      BodyNode* _parentNode,
      bool _recursive) const;
 
  template <class JointType>
  std::pair<JointType*, BodyNode*> cloneBodyNodeTree(
      const BodyNode* _bodyNode,
      const SkeletonPtr& _newSkeleton,
      BodyNode* _parentNode,
      const typename JointType::Properties& _joint,
      bool _recursive) const;
 
  std::vector<const BodyNode*> constructBodyNodeTree(
      const BodyNode* _bodyNode) const;
 
  std::vector<BodyNode*> constructBodyNodeTree(BodyNode* _bodyNode);
 
  std::vector<BodyNode*> extractBodyNodeTree(BodyNode* _bodyNode);
 
  void receiveBodyNodeTree(const std::vector<BodyNode*>& _tree);
 
  void updateTotalMass();
 
  void updateCacheDimensions(DataCache& _cache);
 
  void updateCacheDimensions(std::size_t _treeIdx);
 
  void updateArticulatedInertia(std::size_t _tree) const;
 
  void updateArticulatedInertia() const;
 
  void updateMassMatrix(std::size_t _treeIdx) const;
 
  void updateMassMatrix() const;
 
  void updateAugMassMatrix(std::size_t _treeIdx) const;
 
  void updateAugMassMatrix() const;
 
  void updateInvMassMatrix(std::size_t _treeIdx) const;
 
  void updateInvMassMatrix() const;
 
  void updateInvAugMassMatrix(std::size_t _treeIdx) const;
 
  void updateInvAugMassMatrix() const;
 
  void updateCoriolisForces(std::size_t _treeIdx) const;
 
  void updateCoriolisForces() const;
 
  void updateGravityForces(std::size_t _treeIdx) const;
 
  void updateGravityForces() const;
 
  void updateCoriolisAndGravityForces(std::size_t _treeIdx) const;
 
  void updateCoriolisAndGravityForces() const;
 
  void updateExternalForces(std::size_t _treeIdx) const;
 
  // TODO(JS): Not implemented yet
  void updateExternalForces() const;
 
  const Eigen::VectorXd& computeConstraintForces(DataCache& cache) const;
 
  //  /// Update damping force vector.
  //  virtual void updateDampingForceVector();
 
  const std::string& addEntryToBodyNodeNameMgr(BodyNode* _newNode);
 
  const std::string& addEntryToJointNameMgr(
      Joint* _newJoint, bool _updateDofNames = true);
 
  void addEntryToSoftBodyNodeNameMgr(SoftBodyNode* _newNode);
 
  void updateNameManagerNames();
 
protected:
  std::weak_ptr<Skeleton> mPtr;
 
  std::vector<SoftBodyNode*> mSoftBodyNodes;
 
  dart::common::NameManager<BodyNode*> mNameMgrForBodyNodes;
 
  dart::common::NameManager<Joint*> mNameMgrForJoints;
 
  dart::common::NameManager<DegreeOfFreedom*> mNameMgrForDofs;
 
  dart::common::NameManager<SoftBodyNode*> mNameMgrForSoftBodyNodes;
 
  std::shared_ptr<WholeBodyIK> mWholeBodyIK;
 
  struct DirtyFlags
  {
    DirtyFlags();
 
    bool mArticulatedInertia;
 
    bool mMassMatrix;
 
    bool mAugMassMatrix;
 
    bool mInvMassMatrix;
 
    bool mInvAugMassMatrix;
 
    bool mGravityForces;
 
    bool mCoriolisForces;
 
    bool mCoriolisAndGravityForces;
 
    bool mExternalForces;
 
    bool mDampingForces;
 
    bool mSupport;
 
    std::size_t mSupportVersion;
  };
 
  struct DataCache
  {
    DirtyFlags mDirty;
 
    std::vector<BodyNode*> mBodyNodes;
 
    std::vector<const BodyNode*> mConstBodyNodes;
 
    std::vector<DegreeOfFreedom*> mDofs;
 
    std::vector<const DegreeOfFreedom*> mConstDofs;
 
    Eigen::MatrixXd mM;
 
    Eigen::MatrixXd mAugM;
 
    Eigen::MatrixXd mInvM;
 
    Eigen::MatrixXd mInvAugM;
 
    Eigen::VectorXd mCvec;
 
    Eigen::VectorXd mG;
 
    Eigen::VectorXd mCg;
 
    Eigen::VectorXd mFext;
 
    Eigen::VectorXd mFc;
 
    math::SupportPolygon mSupportPolygon;
 
    std::vector<std::size_t> mSupportIndices;
 
    std::pair<Eigen::Vector3d, Eigen::Vector3d> mSupportAxes;
 
    math::SupportGeometry mSupportGeometry;
 
    Eigen::Vector2d mSupportCentroid;
 
    // To get byte-aligned Eigen vectors
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
 
  mutable common::aligned_vector<DataCache> mTreeCache;
 
  mutable DataCache mSkelCache;
 
  using SpecializedTreeNodes
      = std::map<std::type_index, std::vector<NodeMap::iterator>*>;
 
  SpecializedTreeNodes mSpecializedTreeNodes;
 
  double mTotalMass;
 
  // TODO(JS): Better naming
  bool mIsImpulseApplied;
 
  mutable std::mutex mMutex;
 
public:
  //--------------------------------------------------------------------------
  // Union finding
  //--------------------------------------------------------------------------
  void resetUnion()
  {
    mUnionRootSkeleton = mPtr;
    mUnionSize = 1;
  }
 
  std::weak_ptr<Skeleton> mUnionRootSkeleton;
 
  std::size_t mUnionSize;
 
  std::size_t mUnionIndex;
};
DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END
 
} // namespace dynamics
} // namespace dart
 
#include "dart/dynamics/detail/Skeleton.hpp"
 
#endif // DART_DYNAMICS_SKELETON_HPP_