Skeleton.hpp

/*
 * 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_