PointMass.hpp

Go to the documentation of this file.

/*
 * 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_POINTMASS_HPP_
#define DART_DYNAMICS_POINTMASS_HPP_
 
#include <vector>
 
#include <Eigen/Dense>
 
#include "dart/dynamics/Entity.hpp"
#include "dart/math/Helpers.hpp"
 
namespace dart {
namespace dynamics {
 
class EllipsoidShape;
class SoftBodyNode;
 
class PointMassNotifier;
 
class PointMass : public common::Subject
{
public:
  friend class SoftBodyNode;
 
  struct State
  {
    Eigen::Vector3d mPositions;
 
    Eigen::Vector3d mVelocities;
 
    Eigen::Vector3d mAccelerations;
 
    Eigen::Vector3d mForces;
 
    State(
        const Eigen::Vector3d& positions = Eigen::Vector3d::Zero(),
        const Eigen::Vector3d& velocities = Eigen::Vector3d::Zero(),
        const Eigen::Vector3d& accelerations = Eigen::Vector3d::Zero(),
        const Eigen::Vector3d& forces = Eigen::Vector3d::Zero());
 
    bool operator==(const State& other) const;
 
    virtual ~State() = default;
  };
 
  struct Properties
  {
    Eigen::Vector3d mX0;
 
    double mMass;
 
    std::vector<std::size_t> mConnectedPointMassIndices;
 
    Eigen::Vector3d mPositionLowerLimits; // Currently unused
 
    Eigen::Vector3d mPositionUpperLimits; // Currently unused
 
    Eigen::Vector3d mVelocityLowerLimits; // Currently unused
 
    Eigen::Vector3d mVelocityUpperLimits; // Currently unused
 
    Eigen::Vector3d mAccelerationLowerLimits; // Currently unused
 
    Eigen::Vector3d mAccelerationUpperLimits; // Currently unused
 
    Eigen::Vector3d mForceLowerLimits; // Currently unused
 
    Eigen::Vector3d mForceUpperLimits; // Currently unused
 
    Properties(
        const Eigen::Vector3d& _X0 = Eigen::Vector3d::Zero(),
        double _mass = 0.0005,
        const std::vector<std::size_t>& _connections
        = std::vector<std::size_t>(),
        const Eigen::Vector3d& _positionLowerLimits
        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
        const Eigen::Vector3d& _positionUpperLimits
        = Eigen::Vector3d::Constant(math::constantsd::inf()),
        const Eigen::Vector3d& _velocityLowerLimits
        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
        const Eigen::Vector3d& _velocityUpperLimits
        = Eigen::Vector3d::Constant(math::constantsd::inf()),
        const Eigen::Vector3d& _accelerationLowerLimits
        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
        const Eigen::Vector3d& _accelerationUpperLimits
        = Eigen::Vector3d::Constant(math::constantsd::inf()),
        const Eigen::Vector3d& _forceLowerLimits
        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
        const Eigen::Vector3d& _forceUpperLimits
        = Eigen::Vector3d::Constant(math::constantsd::inf()));
 
    void setRestingPosition(const Eigen::Vector3d& _x);
 
    void setMass(double _mass);
 
    bool operator==(const Properties& other) const;
 
    bool operator!=(const Properties& other) const;
 
    virtual ~Properties() = default;
  };
 
  //--------------------------------------------------------------------------
  // Constructor and Desctructor
  //--------------------------------------------------------------------------
 
  virtual ~PointMass();
 
  State& getState();
 
  const State& getState() const;
 
  std::size_t getIndexInSoftBodyNode() const;
 
  void setMass(double _mass);
 
  double getMass() const;
 
  double getPsi() const;
 
  double getImplicitPsi() const;
 
  double getPi() const;
 
  double getImplicitPi() const;
 
  void addConnectedPointMass(PointMass* _pointMass);
 
  std::size_t getNumConnectedPointMasses() const;
 
  PointMass* getConnectedPointMass(std::size_t _idx);
 
  const PointMass* getConnectedPointMass(std::size_t _idx) const;
 
  void setColliding(bool _isColliding);
 
  bool isColliding();
 
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  std::size_t getNumDofs() const;
 
  //  // Documentation inherited
  //  void setIndexInSkeleton(std::size_t _index, std::size_t _indexInSkeleton);
 
  //  // Documentation inherited
  //  std::size_t getIndexInSkeleton(std::size_t _index) const;
 
  //----------------------------------------------------------------------------
  // Position
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  void setPosition(std::size_t _index, double _position);
 
  // Documentation inherited
  double getPosition(std::size_t _index) const;
 
  // Documentation inherited
  void setPositions(const Eigen::Vector3d& _positions);
 
  // Documentation inherited
  const Eigen::Vector3d& getPositions() const;
 
  // Documentation inherited
  void resetPositions();
 
  //----------------------------------------------------------------------------
  // Velocity
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  void setVelocity(std::size_t _index, double _velocity);
 
  // Documentation inherited
  double getVelocity(std::size_t _index) const;
 
  // Documentation inherited
  void setVelocities(const Eigen::Vector3d& _velocities);
 
  // Documentation inherited
  const Eigen::Vector3d& getVelocities() const;
 
  // Documentation inherited
  void resetVelocities();
 
  //----------------------------------------------------------------------------
  // Acceleration
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  void setAcceleration(std::size_t _index, double _acceleration);
 
  // Documentation inherited
  double getAcceleration(std::size_t _index) const;
 
  // Documentation inherited
  void setAccelerations(const Eigen::Vector3d& _accelerations);
 
  // Documentation inherited
  const Eigen::Vector3d& getAccelerations() const;
 
  const Eigen::Vector3d& getPartialAccelerations() const;
 
  // Documentation inherited
  void resetAccelerations();
 
  //----------------------------------------------------------------------------
  // Force
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  void setForce(std::size_t _index, double _force);
 
  // Documentation inherited
  double getForce(std::size_t _index);
 
  // Documentation inherited
  void setForces(const Eigen::Vector3d& _forces);
 
  // Documentation inherited
  const Eigen::Vector3d& getForces() const;
 
  // Documentation inherited
  void resetForces();
 
  //----------------------------------------------------------------------------
  // Velocity change
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  void setVelocityChange(std::size_t _index, double _velocityChange);
 
  // Documentation inherited
  double getVelocityChange(std::size_t _index);
 
  // Documentation inherited
  void resetVelocityChanges();
 
  //----------------------------------------------------------------------------
  // Constraint impulse
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  void setConstraintImpulse(std::size_t _index, double _impulse);
 
  // Documentation inherited
  double getConstraintImpulse(std::size_t _index);
 
  // Documentation inherited
  void resetConstraintImpulses();
 
  //----------------------------------------------------------------------------
  // Integration
  //----------------------------------------------------------------------------
 
  // Documentation inherited
  void integratePositions(double _dt);
 
  // Documentation inherited
  void integrateVelocities(double _dt);
 
  //----------------------------------------------------------------------------
 
  void addExtForce(const Eigen::Vector3d& _force, bool _isForceLocal = false);
 
  void clearExtForce();
 
  //----------------------------------------------------------------------------
  // Constraints
  //   - Following functions are managed by constraint solver.
  //----------------------------------------------------------------------------
  void setConstraintImpulse(
      const Eigen::Vector3d& _constImp, bool _isLocal = false);
 
  void addConstraintImpulse(
      const Eigen::Vector3d& _constImp, bool _isLocal = false);
 
  void clearConstraintImpulse();
 
  Eigen::Vector3d getConstraintImpulses() const;
 
  //----------------------------------------------------------------------------
  void setRestingPosition(const Eigen::Vector3d& _p);
 
  const Eigen::Vector3d& getRestingPosition() const;
 
  const Eigen::Vector3d& getLocalPosition() const;
 
  const Eigen::Vector3d& getWorldPosition() const;
 
  Eigen::Matrix<double, 3, Eigen::Dynamic> getBodyJacobian();
  Eigen::Matrix<double, 3, Eigen::Dynamic> getWorldJacobian();
 
  const Eigen::Vector3d& getBodyVelocityChange() const;
 
  SoftBodyNode* getParentSoftBodyNode();
 
  const SoftBodyNode* getParentSoftBodyNode() const;
 
  //  int getNumDependentGenCoords() const;
 
  //  int getDependentGenCoord(int _arrayIndex) const;
 
  const Eigen::Vector3d& getBodyVelocity() const;
 
  Eigen::Vector3d getWorldVelocity() const;
 
  const Eigen::Vector3d& getBodyAcceleration() const;
 
  Eigen::Vector3d getWorldAcceleration() const;
 
protected:
  explicit PointMass(SoftBodyNode* _softBodyNode);
 
  void init();
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  void updateTransform() const;
 
  void updateVelocity() const;
 
  void updatePartialAcceleration() const;
 
  void updateArtInertiaFD(double _timeStep) const;
 
  void updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity);
 
  void updateBiasImpulseFD();
 
  void updateAccelerationID() const;
 
  void updateAccelerationFD();
 
  void updateVelocityChangeFD();
 
  void updateTransmittedForceID(
      const Eigen::Vector3d& _gravity, bool _withExternalForces = false);
 
  void updateTransmittedForce();
 
  void updateTransmittedImpulse();
 
  void updateJointForceID(
      double _timeStep, double _withDampingForces, double _withSpringForces);
 
  void updateConstrainedTermsFD(double _timeStep);
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  void updateMassMatrix();
 
  void aggregateMassMatrix(Eigen::MatrixXd& _MCol, int _col);
 
  void aggregateAugMassMatrix(
      Eigen::MatrixXd& _MCol, int _col, double _timeStep);
 
  void updateInvMassMatrix();
 
  void updateInvAugMassMatrix();
 
  void aggregateInvMassMatrix(Eigen::MatrixXd& _MInvCol, int _col);
 
  void aggregateInvAugMassMatrix(
      Eigen::MatrixXd& _MInvCol, int _col, double _timeStep);
 
  void aggregateGravityForceVector(
      Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity);
 
  void updateCombinedVector();
 
  void aggregateCombinedVector(
      Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity);
 
  void aggregateExternalForces(Eigen::VectorXd& _Fext);
 
 
  //-------------------- Cache Data for Mass Matrix ----------------------------
  Eigen::Vector3d mM_dV;
 
  Eigen::Vector3d mM_F;
 
  //----------------- Cache Data for Mass Inverse Matrix -----------------------
  Eigen::Vector3d mBiasForceForInvMeta;
 
  //---------------- Cache Data for Gravity Force Vector -----------------------
  Eigen::Vector3d mG_F;
 
  //------------------- Cache Data for Combined Vector -------------------------
  Eigen::Vector3d mCg_dV;
 
  Eigen::Vector3d mCg_F;
 
protected:
  // TODO(JS): Need?
  //  Eigen::Matrix<std::size_t, 3, 1> mIndexInSkeleton;
 
  SoftBodyNode* mParentSoftBodyNode;
 
  std::size_t mIndex;
 
  //----------------------------------------------------------------------------
  // Configuration
  //----------------------------------------------------------------------------
 
  Eigen::Vector3d mPositionDeriv;
 
  //----------------------------------------------------------------------------
  // Velocity
  //----------------------------------------------------------------------------
 
  Eigen::Vector3d mVelocitiesDeriv;
 
  //----------------------------------------------------------------------------
  // Acceleration
  //----------------------------------------------------------------------------
 
  Eigen::Vector3d mAccelerationsDeriv;
 
  //----------------------------------------------------------------------------
  // Force
  //----------------------------------------------------------------------------
 
  Eigen::Vector3d mForcesDeriv;
 
  //----------------------------------------------------------------------------
  // Impulse
  //----------------------------------------------------------------------------
 
  Eigen::Vector3d mVelocityChanges;
 
  //  /// Generalized impulse
  //  Eigen::Vector3d mImpulse;
 
  Eigen::Vector3d mConstraintImpulses;
 
  //----------------------------------------------------------------------------
 
  mutable Eigen::Vector3d mW;
 
  mutable Eigen::Vector3d mX;
 
  mutable Eigen::Vector3d mV;
 
  mutable Eigen::Vector3d mEta;
 
  Eigen::Vector3d mAlpha;
 
  Eigen::Vector3d mBeta;
 
  mutable Eigen::Vector3d mA;
 
  Eigen::Vector3d mF;
 
  mutable double mPsi;
 
  mutable double mImplicitPsi;
 
  mutable double mPi;
 
  mutable double mImplicitPi;
 
  Eigen::Vector3d mB;
 
  Eigen::Vector3d mFext;
 
  std::vector<std::size_t> mDependentGenCoordIndices;
 
  bool mIsColliding;
 
  //------------------------- Impulse-based Dyanmics ---------------------------
  Eigen::Vector3d mDelV;
 
  Eigen::Vector3d mImpB;
 
  Eigen::Vector3d mImpAlpha;
 
  Eigen::Vector3d mImpBeta;
 
  Eigen::Vector3d mImpF;
 
  PointMassNotifier* mNotifier;
};
 
// struct PointMassPair
//{
//  PointMass* pm1;
//  PointMass* pm2;
//};
 
class PointMassNotifier : public Entity
{
public:
  PointMassNotifier(SoftBodyNode* _parentSoftBody, const std::string& _name);
 
  bool needsPartialAccelerationUpdate() const;
 
  void clearTransformNotice();
  void clearVelocityNotice();
  void clearPartialAccelerationNotice();
  void clearAccelerationNotice();
 
  void dirtyTransform() override;
  void dirtyVelocity() override;
  void dirtyAcceleration() override;
 
  // Documentation inherited
  const std::string& setName(const std::string& _name) override;
 
  // Documentation inherited
  const std::string& getName() const override;
 
protected:
  std::string mName;
 
  bool mNeedPartialAccelerationUpdate;
  // TODO(JS): Rename this to mIsPartialAccelerationDirty in DART 7
 
  SoftBodyNode* mParentSoftBodyNode;
};
 
} // namespace dynamics
} // namespace dart
 
#endif // DART_DYNAMICS_POINTMASS_HPP_