BodyNode.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_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/Frame.hpp"
#include "dart/dynamics/Node.hpp"
#include "dart/dynamics/Skeleton.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);

  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;
};
DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END

} // namespace dynamics
} // namespace dart

#include "dart/dynamics/detail/BodyNode.hpp"

#endif // DART_DYNAMICS_BODYNODE_HPP_