BodyNode.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011-2022, 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/common/Deprecated.hpp"
#include "dart/common/EmbeddedAspect.hpp"
#include "dart/common/Signal.hpp"
#include "dart/config.hpp"
#include "dart/dynamics/EndEffector.hpp"
#include "dart/dynamics/Frame.hpp"
#include "dart/dynamics/Marker.hpp"
#include "dart/dynamics/Node.hpp"
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/dynamics/SpecializedNodeManager.hpp"
#include "dart/dynamics/TemplatedJacobianNode.hpp"
#include "dart/dynamics/detail/BodyNodeAspect.hpp"
#include "dart/math/Geometry.hpp"
 
namespace dart {
namespace dynamics {
 
class GenCoord;
class Skeleton;
class Joint;
class DegreeOfFreedom;
class Shape;
class EndEffector;
class Marker;
 
DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
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;
 
  DART_DEPRECATED(6.10)
  void setFrictionCoeff(double coeff);
 
  DART_DEPRECATED(6.10)
  double getFrictionCoeff() const;
 
  DART_DEPRECATED(6.10)
  void setRestitutionCoeff(double coeff);
 
  DART_DEPRECATED(6.10)
  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);
 
  template <class JointType>
  JointType* moveTo(
      BodyNode* _newParent,
      const typename JointType::Properties& _joint
      = typename JointType::Properties());
 
  template <class JointType>
  JointType* moveTo(
      const SkeletonPtr& _newSkeleton,
      BodyNode* _newParent,
      const typename JointType::Properties& _joint
      = typename JointType::Properties());
 
  SkeletonPtr split(const std::string& _skeletonName);
 
  template <class JointType>
  SkeletonPtr split(
      const std::string& _skeletonName,
      const typename JointType::Properties& _joint
      = typename JointType::Properties());
 
  template <class JointType>
  JointType* changeParentJointType(
      const typename JointType::Properties& _joint
      = typename JointType::Properties());
 
  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,
      const typename JointType::Properties& _joint
      = typename JointType::Properties(),
      bool _recursive = true);
 
  template <class JointType>
  std::pair<JointType*, BodyNode*> copyTo(
      const SkeletonPtr& _newSkeleton,
      BodyNode* _newParent,
      const typename JointType::Properties& _joint
      = typename JointType::Properties(),
      bool _recursive = true) const;
 
  SkeletonPtr copyAs(
      const std::string& _skeletonName, bool _recursive = true) const;
 
  template <class JointType>
  SkeletonPtr copyAs(
      const std::string& _skeletonName,
      const typename JointType::Properties& _joint
      = typename JointType::Properties(),
      bool _recursive = true) const;
 
  // 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(
      const typename JointType::Properties& _jointProperties
      = typename JointType::Properties(),
      const typename NodeType::Properties& _bodyProperties
      = typename NodeType::Properties());
 
  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);
 
  DART_DEPRECATED(6.13)
  const std::vector<ShapeNode*> getShapeNodes();
 
  DART_DEPRECATED(6.13)
  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>
  DART_DEPRECATED(6.13)
  const std::vector<ShapeNode*> getShapeNodesWith();
 
  template <class Aspect>
  DART_DEPRECATED(6.13)
  const std::vector<const ShapeNode*> getShapeNodesWith() const;
 
  template <class Aspect>
  ShapeNode* getShapeNodeWith(std::size_t index);
 
  template <class Aspect>
  const ShapeNode* getShapeNodeWith(std::size_t index) const;
 
  template <class Aspect>
  void removeAllShapeNodesWith();
 
  template <typename Aspect, typename Func>
  void eachShapeNodeWith(Func func) const;
 
  template <typename Aspect, typename Func>
  void eachShapeNodeWith(Func func);
 
  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();
 
  void setColor(const Eigen::Vector3d& color);
 
  void setColor(const Eigen::Vector4d& color);
 
  void setAlpha(double alpha);
 
  //----------------------------------------------------------------------------
  // 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;
};
DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END
 
} // namespace dynamics
} // namespace dart
 
#include "dart/dynamics/detail/BodyNode.hpp"
 
#endif // DART_DYNAMICS_BODYNODE_HPP_