SoftBodyNode.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_SOFTBODYNODE_HPP_
#define DART_DYNAMICS_SOFTBODYNODE_HPP_
 
#include "dart/dynamics/detail/SoftBodyNodeAspect.hpp"
 
namespace dart {
namespace dynamics {
 
class SoftBodyNode : public detail::SoftBodyNodeBase
{
public:
 
  using UniqueProperties = detail::SoftBodyNodeUniqueProperties;
  using Properties = detail::SoftBodyNodeProperties;
  using Base = detail::SoftBodyNodeBase;
 
  friend class Skeleton;
  friend class PointMass;
  friend class PointMassNotifier;
 
  DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, SoftBodyAspect)
 
  
  virtual ~SoftBodyNode();
 
  // Documentation inherited
  SoftBodyNode* asSoftBodyNode() override;
 
  // Documentation inherited
  const SoftBodyNode* asSoftBodyNode() const override;
 
  void setProperties(const Properties& _properties);
 
  void setProperties(const UniqueProperties& _properties);
 
  void setAspectState(const AspectState& state);
 
  void setAspectProperties(const AspectProperties& properties);
 
  using SkeletonRefCountingBase::getSkeleton;
 
  Properties getSoftBodyNodeProperties() const;
 
  void copy(const SoftBodyNode& _otherSoftBodyNode);
 
  void copy(const SoftBodyNode* _otherSoftBodyNode);
 
  SoftBodyNode& operator=(const SoftBodyNode& _otherSoftBodyNode);
 
  PointMassNotifier* getNotifier();
 
  const PointMassNotifier* getNotifier() const;
 
  double getMass() const;
 
  void setVertexSpringStiffness(double _kv);
 
  double getVertexSpringStiffness() const;
 
  void setEdgeSpringStiffness(double _ke);
 
  double getEdgeSpringStiffness() const;
 
  void setDampingCoefficient(double _damp);
 
  double getDampingCoefficient() const;
 
  void removeAllPointMasses();
 
  PointMass* addPointMass(const PointMass::Properties& _properties);
 
  std::size_t getNumPointMasses() const;
 
  PointMass* getPointMass(std::size_t _idx);
 
  const PointMass* getPointMass(std::size_t _idx) const;
 
  const std::vector<PointMass*>& getPointMasses() const;
 
  void connectPointMasses(std::size_t _idx1, std::size_t _idx2);
 
  void addFace(const Eigen::Vector3i& _face);
 
  const Eigen::Vector3i& getFace(std::size_t _idx) const;
 
  std::size_t getNumFaces() const;
 
  // Documentation inherited.
  void clearConstraintImpulse() override;
 
protected:
 
  SoftBodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint,
               const Properties& _properties);
 
  BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint,
                  bool cloneNodes) const override;
 
  void configurePointMasses(ShapeNode* softNode);
 
  //--------------------------------------------------------------------------
  // Sub-functions for Recursive Kinematics Algorithms
  //--------------------------------------------------------------------------
  // Documentation inherited.
  void init(const SkeletonPtr& _skeleton) override;
 
  // Documentation inherited.
//  virtual void aggregateGenCoords(std::vector<GenCoord*>* _genCoords);
 
  // Documentation inherited.
//  virtual void aggregatePointMassGenCoords(std::vector<GenCoord*>* _genCoords);
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  void checkArticulatedInertiaUpdate() const;
 
  // Documentation inherited.
  void updateTransform() override;
 
  // Documentation inherited.
  void updateVelocity() override;
 
  // Documentation inherited.
  void updatePartialAcceleration() const override;
 
  // Documentation inherited.
  void updateArtInertia(double _timeStep) const override;
 
  // Documentation inherited.
  void updateBiasForce(const Eigen::Vector3d& _gravity,
                       double _timeStep) override;
 
  // Documentation inherited.
  void updateBiasImpulse() override;
 
  // Documentation inherited.
  void updateAccelerationID() override;
 
  // Documentation inherited.
  void updateAccelerationFD() override;
 
  // Documentation inherited.
  void updateVelocityChangeFD() override;
 
  // Documentation inherited.
  void updateTransmittedForceID(
      const Eigen::Vector3d& _gravity,
      bool _withExternalForces = false) override;
 
  // Documentation inherited.
  void updateTransmittedForceFD() override;
 
  // Documentation inherited.
  void updateTransmittedImpulse() override;
 
  // Documentation inherited.
  void updateJointForceID(double _timeStep,
                          bool _withDampingForces,
                          bool _withSpringForces) override;
 
  // Documentation inherited.
  void updateJointForceFD(double _timeStep,
                          bool _withDampingForces,
                          bool _withSpringForces) override;
 
  // Documentation inherited.
  void updateJointImpulseFD() override;
 
  // Documentation inherited.
  void updateConstrainedTerms(double _timeStep) override;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  // Documentation inherited.
  void updateMassMatrix() override;
 
  // Documentation inherited.
  void aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col) override;
 
  // Documentation inherited.
  void aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col,
                              double _timeStep) override;
 
  // Documentation inherited.
  void updateInvMassMatrix() override;
 
  // Documentation inherited.
  void updateInvAugMassMatrix() override;
 
  // Documentation inherited.
  void aggregateInvMassMatrix(Eigen::MatrixXd& _InvMCol, std::size_t _col) override;
 
  // Documentation inherited.
  void aggregateInvAugMassMatrix(Eigen::MatrixXd& _InvMCol, std::size_t _col,
                                 double _timeStep) override;
 
  // Documentation inherited.
  // TODO(JS): Not implemented yet.
  void aggregateCoriolisForceVector(Eigen::VectorXd& _C) override;
 
  // Documentation inherited.
  void aggregateGravityForceVector(Eigen::VectorXd& _g,
                                   const Eigen::Vector3d& _gravity) override;
 
  // Documentation inherited.
  void updateCombinedVector() override;
 
  // Documentation inherited.
  void aggregateCombinedVector(Eigen::VectorXd& _Cg,
                               const Eigen::Vector3d& _gravity) override;
 
  // Documentation inherited.
  void aggregateExternalForces(Eigen::VectorXd& _Fext) override;
 
 
  // Documentation inherited.
  void clearExternalForces() override;
 
  void clearInternalForces() override;
 
protected:
 
  std::vector<PointMass*> mPointMasses;
 
  PointMassNotifier* mNotifier;
 
  WeakShapeNodePtr mSoftShapeNode;
 
  math::Inertia mI2;
 
  math::Inertia mArtInertia2;
 
  math::Inertia mArtInertiaImplicit2;
 
private:
  void _addPiToArtInertia(const Eigen::Vector3d& _p, double _Pi) const;
 
  void _addPiToArtInertiaImplicit(const Eigen::Vector3d& _p,
                                  double _ImplicitPi) const;
 
  void updateInertiaWithPointMass();
};
 
class SoftBodyNodeHelper
{
public:
 
  static SoftBodyNode::UniqueProperties makeBoxProperties(
      const Eigen::Vector3d&   _size,
      const Eigen::Isometry3d& _localTransform,
      double                   _totalMass,
      double                   _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double                   _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double                   _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  static void setBox(SoftBodyNode*            _softBodyNode,
      const Eigen::Vector3d&   _size,
      const Eigen::Isometry3d& _localTransform,
      double                   _totalMass,
      double                   _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double                   _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double                   _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  static SoftBodyNode::UniqueProperties makeBoxProperties(
      const Eigen::Vector3d&   _size,
      const Eigen::Isometry3d& _localTransform,
      const Eigen::Vector3i&   _frags,
      double                   _totalMass,
      double                   _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double                   _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double                   _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  // TODO: The component of _frags should allow 2.
  static void setBox(SoftBodyNode*            _softBodyNode,
      const Eigen::Vector3d&   _size,
      const Eigen::Isometry3d& _localTransform,
      const Eigen::Vector3i&   _frags,
      double                   _totalMass,
      double                   _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double                   _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double                   _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  static SoftBodyNode::UniqueProperties makeSinglePointMassProperties(
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  static void setSinglePointMass(
      SoftBodyNode*          _softBodyNode,
      double                 _totalMass,
      double                 _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double                 _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double                 _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  static SoftBodyNode::UniqueProperties makeSphereProperties(
      double                 _radius,
      std::size_t            _nSlices,
      std::size_t            _nStacks,
      double                 _totalMass,
      double                 _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double                 _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double                 _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  static SoftBodyNode::UniqueProperties makeEllipsoidProperties(
      const Eigen::Vector3d& _size,
      std::size_t                 _nSlices,
      std::size_t                 _nStacks,
      double                 _totalMass,
      double                 _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double                 _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double                 _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  static void setEllipsoid(SoftBodyNode*          _softBodyNode,
      const Eigen::Vector3d& _size,
      std::size_t _nSlices,
      std::size_t _nStacks,
      double                 _totalMass,
      double                 _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double                 _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double                 _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  static SoftBodyNode::UniqueProperties makeCylinderProperties(
      double _radius,
      double _height,
      std::size_t _nSlices,
      std::size_t _nStacks,
      std::size_t _nRings,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
  static void setCylinder(SoftBodyNode*          _softBodyNode,
      double _radius,
      double _height,
      std::size_t _nSlices,
      std::size_t _nStacks,
      std::size_t _nRings,
      double                 _totalMass,
      double                 _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double                 _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
      double                 _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
};
 
}  // namespace dynamics
}  // namespace dart
 
#endif  // DART_DYNAMICS_SOFTBODYNODE_HPP_