BodyNode.hpp

Go to the documentation of this file.

/*
 * 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.
 */
 
#ifndef DART_DYNAMICS_BODYNODE_HPP_
#define DART_DYNAMICS_BODYNODE_HPP_
 
#include <string>
#include <vector>
 
#include <Eigen/Dense>
#include <Eigen/StdVector>
 
#include "dart/config.hpp"
#include "dart/common/Deprecated.hpp"
#include "dart/common/Signal.hpp"
#include "dart/common/EmbeddedAspect.hpp"
#include "dart/math/Geometry.hpp"
#include "dart/dynamics/Node.hpp"
#include "dart/dynamics/Frame.hpp"
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/dynamics/TemplatedJacobianNode.hpp"
#include "dart/dynamics/SpecializedNodeManager.hpp"
#include "dart/dynamics/detail/BodyNodeAspect.hpp"
#include "dart/dynamics/Skeleton.hpp"
 
namespace dart {
namespace dynamics {
 
class GenCoord;
class Skeleton;
class Joint;
class DegreeOfFreedom;
class Shape;
class EndEffector;
class Marker;
 
class BodyNode :
    public detail::BodyNodeCompositeBase,
    public virtual BodyNodeSpecializedFor<ShapeNode, EndEffector, Marker>,
    public SkeletonRefCountingBase,
    public TemplatedJacobianNode<BodyNode>
{
public:
 
  using ColShapeAddedSignal
      = common::Signal<void(const BodyNode*, ConstShapePtr _newColShape)>;
 
  using ColShapeRemovedSignal = ColShapeAddedSignal;
 
  using StructuralChangeSignal
      = common::Signal<void(const BodyNode*)>;
  using CompositeProperties = common::Composite::Properties;
 
  using AllNodeStates = detail::AllNodeStates;
  using NodeStateMap = detail::NodeStateMap;
 
  using AllNodeProperties = detail::AllNodeProperties;
  using NodePropertiesMap = detail::NodePropertiesMap;
 
  using AspectProperties = detail::BodyNodeAspectProperties;
  using Properties = common::Composite::MakeProperties<BodyNode>;
 
  BodyNode(const BodyNode&) = delete;
 
  virtual ~BodyNode();
 
  virtual SoftBodyNode* asSoftBodyNode();
 
  virtual const SoftBodyNode* asSoftBodyNode() const;
 
  void setAllNodeStates(const AllNodeStates& states);
 
  AllNodeStates getAllNodeStates() const;
 
  void setAllNodeProperties(const AllNodeProperties& properties);
 
  AllNodeProperties getAllNodeProperties() const;
 
  void setProperties(const CompositeProperties& _properties);
 
  void setProperties(const AspectProperties& _properties);
 
  void setAspectState(const AspectState& state);
 
  void setAspectProperties(const AspectProperties& properties);
 
  Properties getBodyNodeProperties() const;
 
  void copy(const BodyNode& otherBodyNode);
 
  void copy(const BodyNode* otherBodyNode);
 
  BodyNode& operator=(const BodyNode& _otherBodyNode);
 
  void duplicateNodes(const BodyNode* otherBodyNode);
 
  void matchNodes(const BodyNode* otherBodyNode);
 
  const std::string& setName(const std::string& _name) override;
 
  // Documentation inherited
  const std::string& getName() const override;
 
  void setGravityMode(bool _gravityMode);
 
  bool getGravityMode() const;
 
  bool isCollidable() const;
 
  void setCollidable(bool _isCollidable);
 
  void setMass(double mass);
 
  double getMass() const;
 
  void setMomentOfInertia(
      double _Ixx, double _Iyy, double _Izz,
      double _Ixy = 0.0, double _Ixz = 0.0, double _Iyz = 0.0);
 
  void getMomentOfInertia(
      double& _Ixx, double& _Iyy, double& _Izz,
      double& _Ixy, double& _Ixz, double& _Iyz) const;
 
  const Eigen::Matrix6d& getSpatialInertia() const;
 
  void setInertia(const Inertia& inertia);
 
  const Inertia& getInertia() const;
 
  const math::Inertia& getArticulatedInertia() const;
 
  const math::Inertia& getArticulatedInertiaImplicit() const;
 
  void setLocalCOM(const Eigen::Vector3d& _com);
 
  const Eigen::Vector3d& getLocalCOM() const;
 
  Eigen::Vector3d getCOM(const Frame* _withRespectTo = Frame::World()) const;
 
  Eigen::Vector3d getCOMLinearVelocity(
                          const Frame* _relativeTo = Frame::World(),
                          const Frame* _inCoordinatesOf = Frame::World()) const;
 
  Eigen::Vector6d getCOMSpatialVelocity() const;
 
  Eigen::Vector6d getCOMSpatialVelocity(const Frame* _relativeTo,
                                        const Frame* _inCoordinatesOf) const;
 
  Eigen::Vector3d getCOMLinearAcceleration(
                          const Frame* _relativeTo = Frame::World(),
                          const Frame* _inCoordinatesOf = Frame::World()) const;
 
  Eigen::Vector6d getCOMSpatialAcceleration() const;
 
  Eigen::Vector6d getCOMSpatialAcceleration(const Frame* _relativeTo,
                                            const Frame* _inCoordinatesOf) const;
 
  void setFrictionCoeff(double _coeff);
 
  double getFrictionCoeff() const;
 
  void setRestitutionCoeff(double _coeff);
 
  double getRestitutionCoeff() const;
 
  //--------------------------------------------------------------------------
  // Structural Properties
  //--------------------------------------------------------------------------
 
  std::size_t getIndexInSkeleton() const;
 
  std::size_t getIndexInTree() const;
 
  std::size_t getTreeIndex() const;
 
  SkeletonPtr remove(const std::string& _name = "temporary");
 
  bool moveTo(BodyNode* _newParent);
 
  bool moveTo(const SkeletonPtr& _newSkeleton, BodyNode* _newParent);
 
#ifdef _WIN32
  template <typename JointType>
  static typename JointType::Properties createJointProperties()
  {
    return typename JointType::Properties();
  }
 
  template <typename NodeType>
  static typename NodeType::Properties createBodyNodeProperties()
  {
    return typename NodeType::Properties();
  }
#endif
  // TODO: Workaround for MSVC bug on template function specialization with
  // default argument. Please see #487 for detail
 
  template <class JointType>
  JointType* moveTo(BodyNode* _newParent,
#ifdef _WIN32
      const typename JointType::Properties& _joint
          = BodyNode::createJointProperties<JointType>());
#else
      const typename JointType::Properties& _joint
          = typename JointType::Properties());
#endif
  // TODO: Workaround for MSVC bug on template function specialization with
  // default argument. Please see #487 for detail
 
  template <class JointType>
  JointType* moveTo(const SkeletonPtr& _newSkeleton, BodyNode* _newParent,
#ifdef _WIN32
      const typename JointType::Properties& _joint
          = BodyNode::createJointProperties<JointType>());
#else
      const typename JointType::Properties& _joint
          = typename JointType::Properties());
#endif
  // TODO: Workaround for MSVC bug on template function specialization with
  // default argument. Please see #487 for detail
 
  SkeletonPtr split(const std::string& _skeletonName);
 
  template <class JointType>
  SkeletonPtr split(const std::string& _skeletonName,
#ifdef _WIN32
      const typename JointType::Properties& _joint
          = BodyNode::createJointProperties<JointType>());
#else
      const typename JointType::Properties& _joint
          = typename JointType::Properties());
#endif
  // TODO: Workaround for MSVC bug on template function specialization with
  // default argument. Please see #487 for detail
 
  template <class JointType>
  JointType* changeParentJointType(
#ifdef _WIN32
      const typename JointType::Properties& _joint
          = BodyNode::createJointProperties<JointType>());
#else
      const typename JointType::Properties& _joint
          = typename JointType::Properties());
#endif
  // TODO: Workaround for MSVC bug on template function specialization with
  // default argument. Please see #487 for detail
 
  std::pair<Joint*, BodyNode*> copyTo(BodyNode* _newParent,
                                      bool _recursive=true);
 
  std::pair<Joint*, BodyNode*> copyTo(const SkeletonPtr& _newSkeleton,
                                      BodyNode* _newParent,
                                      bool _recursive=true) const;
 
  template <class JointType>
  std::pair<JointType*, BodyNode*> copyTo(
      BodyNode* _newParent,
#ifdef _WIN32
      const typename JointType::Properties& _joint
          = BodyNode::createJointProperties<JointType>(),
#else
      const typename JointType::Properties& _joint
          = typename JointType::Properties(),
#endif
      bool _recursive = true);
  // TODO: Workaround for MSVC bug on template function specialization with
  // default argument. Please see #487 for detail
 
  template <class JointType>
  std::pair<JointType*, BodyNode*> copyTo(
      const SkeletonPtr& _newSkeleton, BodyNode* _newParent,
#ifdef _WIN32
      const typename JointType::Properties& _joint
          = BodyNode::createJointProperties<JointType>(),
#else
      const typename JointType::Properties& _joint
          = typename JointType::Properties(),
#endif
      bool _recursive = true) const;
  // TODO: Workaround for MSVC bug on template function specialization with
  // default argument. Please see #487 for detail
 
  SkeletonPtr copyAs(const std::string& _skeletonName,
                     bool _recursive=true) const;
 
  template <class JointType>
  SkeletonPtr copyAs(
      const std::string& _skeletonName,
#ifdef _WIN32
      const typename JointType::Properties& _joint
          = BodyNode::createJointProperties<JointType>(),
#else
      const typename JointType::Properties& _joint
          = typename JointType::Properties(),
#endif
      bool _recursive=true) const;
  // TODO: Workaround for MSVC bug on template function specialization with
  // default argument. Please see #487 for detail
 
  // Documentation inherited
  SkeletonPtr getSkeleton() override;
 
  // Documentation inherited
  ConstSkeletonPtr getSkeleton() const override;
 
  Joint* getParentJoint();
 
  const Joint* getParentJoint() const;
 
  BodyNode* getParentBodyNode();
 
  const BodyNode* getParentBodyNode() const;
 
  template <class JointType, class NodeType = BodyNode>
  std::pair<JointType*, NodeType*> createChildJointAndBodyNodePair(
#ifdef _WIN32
      const typename JointType::Properties& _jointProperties
          = BodyNode::createJointProperties<JointType>(),
      const typename NodeType::Properties& _bodyProperties
          = BodyNode::createBodyNodeProperties<NodeType>());
#else
      const typename JointType::Properties& _jointProperties
          = typename JointType::Properties(),
      const typename NodeType::Properties& _bodyProperties
          = typename NodeType::Properties());
#endif
  // TODO: Workaround for MSVC bug on template function specialization with
  // default argument. Please see #487 for detail
 
  std::size_t getNumChildBodyNodes() const;
 
  BodyNode* getChildBodyNode(std::size_t _index);
 
  const BodyNode* getChildBodyNode(std::size_t _index) const;
 
  std::size_t getNumChildJoints() const;
 
  Joint* getChildJoint(std::size_t _index);
 
  const Joint* getChildJoint(std::size_t _index) const;
 
  template <class NodeType, typename ...Args>
  NodeType* createNode(Args&&... args);
 
  DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( ShapeNode )
 
  
  template <class ShapeNodeProperties>
  ShapeNode* createShapeNode(ShapeNodeProperties properties,
                             bool automaticName = true);
 
  template <class ShapeType>
  ShapeNode* createShapeNode(const std::shared_ptr<ShapeType>& shape);
 
  template <class ShapeType, class StringType>
  ShapeNode* createShapeNode(
      const std::shared_ptr<ShapeType>& shape,
      StringType&& name);
 
  const std::vector<ShapeNode*> getShapeNodes();
 
  const std::vector<const ShapeNode*> getShapeNodes() const;
 
  void removeAllShapeNodes();
 
  template <class... Aspects>
  ShapeNode* createShapeNodeWith(const ShapePtr& shape);
 
  template <class... Aspects>
  ShapeNode* createShapeNodeWith(const ShapePtr& shape,
                                 const std::string& name);
 
  template <class Aspect>
  std::size_t getNumShapeNodesWith() const;
 
  template <class Aspect>
  const std::vector<ShapeNode*> getShapeNodesWith();
 
  template <class Aspect>
  const std::vector<const ShapeNode*> getShapeNodesWith() const;
 
  template <class Aspect>
  void removeAllShapeNodesWith();
 
  DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( EndEffector )
 
  
  EndEffector* createEndEffector(
      const EndEffector::BasicProperties& _properties);
 
  EndEffector* createEndEffector(const std::string& _name = "EndEffector");
 
  EndEffector* createEndEffector(const char* _name);
 
  DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( Marker )
 
  
  Marker* createMarker(
      const std::string& name = "marker",
      const Eigen::Vector3d& position = Eigen::Vector3d::Zero(),
      const Eigen::Vector4d& color = Eigen::Vector4d::Constant(1.0));
 
  Marker* createMarker(const Marker::BasicProperties& properties);
 
  // Documentation inherited
  bool dependsOn(std::size_t _genCoordIndex) const override;
 
  // Documentation inherited
  std::size_t getNumDependentGenCoords() const override;
 
  // Documentation inherited
  std::size_t getDependentGenCoordIndex(std::size_t _arrayIndex) const override;
 
  // Documentation inherited
  const std::vector<std::size_t>& getDependentGenCoordIndices() const override;
 
  // Documentation inherited
  std::size_t getNumDependentDofs() const override;
 
  // Documentation inherited
  DegreeOfFreedom* getDependentDof(std::size_t _index) override;
 
  // Documentation inherited
  const DegreeOfFreedom* getDependentDof(std::size_t _index) const override;
 
  // Documentation inherited
  const std::vector<DegreeOfFreedom*>& getDependentDofs() override;
 
  // Documentation inherited
  const std::vector<const DegreeOfFreedom*>& getDependentDofs() const override;
 
  // Documentation inherited
  const std::vector<const DegreeOfFreedom*> getChainDofs() const override;
 
  //--------------------------------------------------------------------------
  // Properties updated by dynamics (kinematics)
  //--------------------------------------------------------------------------
 
  const Eigen::Isometry3d& getRelativeTransform() const override;
 
  // Documentation inherited
  const Eigen::Vector6d& getRelativeSpatialVelocity() const override;
 
  // Documentation inherited
  const Eigen::Vector6d& getRelativeSpatialAcceleration() const override;
 
  // Documentation inherited
  const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override;
 
  const Eigen::Vector6d& getPartialAcceleration() const override;
 
  const math::Jacobian& getJacobian() const override final;
 
  // Prevent the inherited getJacobian functions from being shadowed
  using TemplatedJacobianNode<BodyNode>::getJacobian;
 
  const math::Jacobian& getWorldJacobian() const override final;
 
  // Prevent the inherited getWorldJacobian functions from being shadowed
  using TemplatedJacobianNode<BodyNode>::getWorldJacobian;
 
  const math::Jacobian& getJacobianSpatialDeriv() const override final;
 
  // Prevent the inherited getJacobianSpatialDeriv functions from being shadowed
  using TemplatedJacobianNode<BodyNode>::getJacobianSpatialDeriv;
 
  const math::Jacobian& getJacobianClassicDeriv() const override final;
 
  // Prevent the inherited getJacobianClassicDeriv functions from being shadowed
  using TemplatedJacobianNode<BodyNode>::getJacobianClassicDeriv;
 
  const Eigen::Vector6d& getBodyVelocityChange() const;
 
  DART_DEPRECATED(6.0)
  void setColliding(bool _isColliding);
 
  DART_DEPRECATED(6.0)
  bool isColliding();
 
  void addExtForce(const Eigen::Vector3d& _force,
                   const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero(),
                   bool _isForceLocal = false,
                   bool _isOffsetLocal = true);
 
  void setExtForce(const Eigen::Vector3d& _force,
                   const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero(),
                   bool _isForceLocal = false,
                   bool _isOffsetLocal = true);
 
  void addExtTorque(const Eigen::Vector3d& _torque, bool _isLocal = false);
 
  void setExtTorque(const Eigen::Vector3d& _torque, bool _isLocal = false);
 
  virtual void clearExternalForces();
 
  virtual void clearInternalForces();
 
  const Eigen::Vector6d& getExternalForceLocal() const;
 
  Eigen::Vector6d getExternalForceGlobal() const;
 
  const Eigen::Vector6d& getBodyForce() const;
 
  //----------------------------------------------------------------------------
  // Constraints
  //   - Following functions are managed by constraint solver.
  //----------------------------------------------------------------------------
 
  bool isReactive() const;
 
  void setConstraintImpulse(const Eigen::Vector6d& _constImp);
 
  void addConstraintImpulse(const Eigen::Vector6d& _constImp);
 
  void addConstraintImpulse(const Eigen::Vector3d& _constImp,
                            const Eigen::Vector3d& _offset,
                            bool _isImpulseLocal = false,
                            bool _isOffsetLocal = true);
 
  virtual void clearConstraintImpulse();
 
  const Eigen::Vector6d& getConstraintImpulse() const;
 
  //----------------------------------------------------------------------------
  // Energies
  //----------------------------------------------------------------------------
 
  double computeLagrangian(const Eigen::Vector3d& gravity) const;
 
  DART_DEPRECATED(6.1)
  virtual double getKineticEnergy() const;
 
  double computeKineticEnergy() const;
 
  DART_DEPRECATED(6.1)
  virtual double getPotentialEnergy(const Eigen::Vector3d& _gravity) const;
 
  double computePotentialEnergy(const Eigen::Vector3d& gravity) const;
 
  Eigen::Vector3d getLinearMomentum() const;
 
  Eigen::Vector3d getAngularMomentum(
      const Eigen::Vector3d& _pivot = Eigen::Vector3d::Zero());
 
  //----------------------------------------------------------------------------
  // Notifications
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  void dirtyTransform() override;
 
  // Documentation inherited
  void dirtyVelocity() override;
 
  // Documentation inherited
  void dirtyAcceleration() override;
 
  DART_DEPRECATED(6.2)
  void notifyArticulatedInertiaUpdate();
 
  void dirtyArticulatedInertia();
 
  DART_DEPRECATED(6.2)
  void notifyExternalForcesUpdate();
 
  void dirtyExternalForces();
 
  DART_DEPRECATED(6.2)
  void notifyCoriolisUpdate();
 
  void dirtyCoriolisForces();
 
  //----------------------------------------------------------------------------
  // Friendship
  //----------------------------------------------------------------------------
 
  friend class Skeleton;
  friend class Joint;
  friend class EndEffector;
  friend class SoftBodyNode;
  friend class PointMass;
  friend class Node;
 
protected:
 
  BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint,
           const Properties& _properties);
 
  BodyNode(const std::tuple<BodyNode*, Joint*, Properties>& args);
 
  virtual BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint,
                          bool cloneNodes) const;
 
  Node* cloneNode(BodyNode* bn) const override final;
 
  virtual void init(const SkeletonPtr& _skeleton);
 
  void addChildBodyNode(BodyNode* _body);
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  void processNewEntity(Entity* _newChildEntity) override;
 
  void processRemovedEntity(Entity* _oldChildEntity) override;
 
  virtual void updateTransform();
 
  virtual void updateVelocity();
 
  virtual void updatePartialAcceleration() const;
 
  virtual void updateArtInertia(double _timeStep) const;
 
  virtual void updateBiasForce(const Eigen::Vector3d& _gravity,
                               double _timeStep);
 
  virtual void updateBiasImpulse();
 
  virtual void updateAccelerationID();
 
  virtual void updateAccelerationFD();
 
  virtual void updateVelocityChangeFD();
 
  virtual void updateTransmittedForceID(const Eigen::Vector3d& _gravity,
                                        bool _withExternalForces = false);
 
  virtual void updateTransmittedForceFD();
 
  virtual void updateTransmittedImpulse();
  // TODO: Rename to updateTransmittedImpulseFD if impulse-based inverse
  // dynamics is implemented.
 
  virtual void updateJointForceID(double _timeStep,
                                  bool _withDampingForces,
                                  bool _withSpringForces);
 
  virtual void updateJointForceFD(double _timeStep,
                                  bool _withDampingForces,
                                  bool _withSpringForces);
 
  virtual void updateJointImpulseFD();
 
  virtual void updateConstrainedTerms(double _timeStep);
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void updateMassMatrix();
  virtual void aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col);
  virtual void aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col,
                                      double _timeStep);
 
  virtual void updateInvMassMatrix();
  virtual void updateInvAugMassMatrix();
  virtual void aggregateInvMassMatrix(Eigen::MatrixXd& _InvMCol, std::size_t _col);
  virtual void aggregateInvAugMassMatrix(Eigen::MatrixXd& _InvMCol, std::size_t _col,
                                         double _timeStep);
 
  virtual void aggregateCoriolisForceVector(Eigen::VectorXd& _C);
 
  virtual void aggregateGravityForceVector(Eigen::VectorXd& _g,
                                           const Eigen::Vector3d& _gravity);
 
  virtual void updateCombinedVector();
  virtual void aggregateCombinedVector(Eigen::VectorXd& _Cg,
                                       const Eigen::Vector3d& _gravity);
 
  virtual void aggregateExternalForces(Eigen::VectorXd& _Fext);
 
  virtual void aggregateSpatialToGeneralized(Eigen::VectorXd& _generalized,
                                             const Eigen::Vector6d& _spatial);
 
  void updateBodyJacobian() const;
 
  void updateWorldJacobian() const;
 
  void updateBodyJacobianSpatialDeriv() const;
 
  void updateWorldJacobianClassicDeriv() const;
 
 
protected:
 
  //--------------------------------------------------------------------------
  // General properties
  //--------------------------------------------------------------------------
 
  int mID;
 
  static std::size_t msBodyNodeCount;
 
  bool mIsColliding;
 
  //--------------------------------------------------------------------------
  // Structural Properties
  //--------------------------------------------------------------------------
 
  std::size_t mIndexInSkeleton;
 
  std::size_t mIndexInTree;
 
  std::size_t mTreeIndex;
 
  Joint* mParentJoint;
 
  BodyNode* mParentBodyNode;
 
  std::vector<BodyNode*> mChildBodyNodes;
 
  std::set<Entity*> mNonBodyNodeEntities;
 
  std::vector<std::size_t> mDependentGenCoordIndices;
 
  std::vector<DegreeOfFreedom*> mDependentDofs;
 
  std::vector<const DegreeOfFreedom*> mConstDependentDofs;
 
  //--------------------------------------------------------------------------
  // Dynamical Properties
  //--------------------------------------------------------------------------
 
  mutable math::Jacobian mBodyJacobian;
 
  mutable math::Jacobian mWorldJacobian;
 
  mutable math::Jacobian mBodyJacobianSpatialDeriv;
 
  mutable math::Jacobian mWorldJacobianClassicDeriv;
 
  mutable Eigen::Vector6d mPartialAcceleration;
  // TODO(JS): Rename with more informative name
 
  mutable bool mIsPartialAccelerationDirty;
 
  Eigen::Vector6d mF;
 
  Eigen::Vector6d mFgravity;
 
  mutable math::Inertia mArtInertia;
 
  mutable math::Inertia mArtInertiaImplicit;
 
  Eigen::Vector6d mBiasForce;
 
  Eigen::Vector6d mCg_dV;
  Eigen::Vector6d mCg_F;
 
  Eigen::Vector6d mG_F;
 
  Eigen::Vector6d mFext_F;
 
  Eigen::Vector6d mM_dV;
  Eigen::Vector6d mM_F;
 
  Eigen::Vector6d mInvM_c;
  Eigen::Vector6d mInvM_U;
 
  Eigen::Vector6d mArbitrarySpatial;
 
  //------------------------- Impulse-based Dyanmics ---------------------------
  Eigen::Vector6d mDelV;
 
  Eigen::Vector6d mBiasImpulse;
 
  Eigen::Vector6d mConstraintImpulse;
 
  // TODO(JS): rename with more informative one
  Eigen::Vector6d mImpF;
 
  ColShapeAddedSignal mColShapeAddedSignal;
 
  ColShapeRemovedSignal mColShapeRemovedSignal;
 
  StructuralChangeSignal mStructuralChangeSignal;
 
public:
  // To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  common::SlotRegister<ColShapeAddedSignal> onColShapeAdded;
 
  common::SlotRegister<ColShapeRemovedSignal> onColShapeRemoved;
 
  mutable common::SlotRegister<StructuralChangeSignal> onStructuralChange;
 
 
private:
 
  std::shared_ptr<NodeDestructor> mSelfDestructor;
 
};
 
}  // namespace dynamics
}  // namespace dart
 
#include "dart/dynamics/detail/BodyNode.hpp"
 
#endif  // DART_DYNAMICS_BODYNODE_HPP_