33 #ifndef DART_DYNAMICS_POINTMASS_HPP_
34 #define DART_DYNAMICS_POINTMASS_HPP_
37 #include <Eigen/Dense>
47 class PointMassNotifier;
72 const Eigen::Vector3d& positions = Eigen::Vector3d::Zero(),
73 const Eigen::Vector3d& velocities = Eigen::Vector3d::Zero(),
74 const Eigen::Vector3d& accelerations = Eigen::Vector3d::Zero(),
75 const Eigen::Vector3d& forces = Eigen::Vector3d::Zero());
119 const Eigen::Vector3d& _X0 = Eigen::Vector3d::Zero(),
120 double _mass = 0.0005,
121 const std::vector<std::size_t>& _connections
122 = std::vector<std::size_t>(),
123 const Eigen::Vector3d& _positionLowerLimits
125 const Eigen::Vector3d& _positionUpperLimits
127 const Eigen::Vector3d& _velocityLowerLimits
129 const Eigen::Vector3d& _velocityUpperLimits
131 const Eigen::Vector3d& _accelerationLowerLimits
133 const Eigen::Vector3d& _accelerationUpperLimits
135 const Eigen::Vector3d& _forceLowerLimits
137 const Eigen::Vector3d& _forceUpperLimits
180 double getPi()
const;
223 void setPosition(std::size_t _index,
double _position);
242 void setVelocity(std::size_t _index,
double _velocity);
283 void setForce(std::size_t _index,
double _force);
286 double getForce(std::size_t _index);
289 void setForces(
const Eigen::Vector3d& _forces);
292 const Eigen::Vector3d&
getForces()
const;
340 void addExtForce(
const Eigen::Vector3d& _force,
bool _isForceLocal =
false);
351 const Eigen::Vector3d& _constImp,
bool _isLocal =
false);
355 const Eigen::Vector3d& _constImp,
bool _isLocal =
false);
460 const Eigen::Vector3d& _gravity,
bool _withExternalForces =
false);
470 double _timeStep,
double _withDampingForces,
double _withSpringForces);
490 Eigen::MatrixXd& _MCol,
int _col,
double _timeStep);
503 Eigen::MatrixXd& _MInvCol,
int _col,
double _timeStep);
507 Eigen::VectorXd& _g,
const Eigen::Vector3d& _gravity);
514 Eigen::VectorXd& _Cg,
const Eigen::Vector3d& _gravity);
599 mutable Eigen::Vector3d
mW;
602 mutable Eigen::Vector3d
mX;
605 mutable Eigen::Vector3d
mV;
617 mutable Eigen::Vector3d
mA;
689 const std::string&
setName(
const std::string& _name)
override;
692 const std::string&
getName()
const override;
The Subject class is a base class for any object that wants to report when it gets destroyed.
Definition: Subject.hpp:58
Entity class is a base class for any objects that exist in the kinematic tree structure of DART.
Definition: Entity.hpp:61
Definition: PointMass.hpp:673
void dirtyAcceleration() override
Notify the acceleration of this Entity that its parent Frame's acceleration is needed.
Definition: PointMass.cpp:1152
bool mNeedPartialAccelerationUpdate
Definition: PointMass.hpp:697
SoftBodyNode * mParentSoftBodyNode
Definition: PointMass.hpp:700
void dirtyTransform() override
Notify the transformation update of this Entity that its parent Frame's pose is needed.
Definition: PointMass.cpp:1130
void clearTransformNotice()
Definition: PointMass.cpp:1106
const std::string & setName(const std::string &_name) override
Set name.
Definition: PointMass.cpp:1158
PointMassNotifier(SoftBodyNode *_parentSoftBody, const std::string &_name)
Definition: PointMass.cpp:1090
bool needsPartialAccelerationUpdate() const
Definition: PointMass.cpp:1100
const std::string & getName() const override
Return the name of this Entity.
Definition: PointMass.cpp:1173
void clearVelocityNotice()
Definition: PointMass.cpp:1112
std::string mName
Definition: PointMass.hpp:695
void clearPartialAccelerationNotice()
Definition: PointMass.cpp:1118
void clearAccelerationNotice()
Definition: PointMass.cpp:1124
void dirtyVelocity() override
Notify the velocity update of this Entity that its parent Frame's velocity is needed.
Definition: PointMass.cpp:1142
Definition: PointMass.hpp:51
Eigen::Vector3d mF
Definition: PointMass.hpp:620
Eigen::Vector3d mM_dV
Definition: PointMass.hpp:524
void updateJointForceID(double _timeStep, double _withDampingForces, double _withSpringForces)
Update the joint force. Inverse dynamics routine.
Definition: PointMass.cpp:812
void setPosition(std::size_t _index, double _position)
Definition: PointMass.cpp:317
void aggregateMassMatrix(Eigen::MatrixXd &_MCol, int _col)
Definition: PointMass.cpp:972
void updateBiasForceFD(double _dt, const Eigen::Vector3d &_gravity)
Update bias force associated with the articulated body inertia.
Definition: PointMass.cpp:823
double getPsi() const
Definition: PointMass.cpp:221
double getMass() const
Definition: PointMass.cpp:215
Eigen::Vector3d mDelV
Velocity change due to constraint impulse.
Definition: PointMass.hpp:648
void integratePositions(double _dt)
Definition: PointMass.cpp:514
void updateInvAugMassMatrix()
Definition: PointMass.cpp:1006
void integrateVelocities(double _dt)
Definition: PointMass.cpp:520
double getVelocityChange(std::size_t _index)
Definition: PointMass.cpp:478
Eigen::Vector3d mB
Bias force.
Definition: PointMass.hpp:635
void aggregateInvAugMassMatrix(Eigen::MatrixXd &_MInvCol, int _col, double _timeStep)
Definition: PointMass.cpp:1026
double getImplicitPsi() const
Definition: PointMass.cpp:228
Eigen::Vector3d mImpB
Impulsive bias force due to external impulsive force exerted on bodies of the parent skeleton.
Definition: PointMass.hpp:652
Eigen::Vector3d mW
Current position viewed in world frame.
Definition: PointMass.hpp:599
void updateInvMassMatrix()
Definition: PointMass.cpp:1000
void setVelocityChange(std::size_t _index, double _velocityChange)
Definition: PointMass.cpp:470
Eigen::Vector3d mImpAlpha
Cache data for mImpB.
Definition: PointMass.hpp:655
Eigen::Vector3d getWorldAcceleration() const
Get the generalized acceleration at the position of this point mass where the acceleration is express...
Definition: PointMass.cpp:712
Eigen::Matrix< double, 3, Eigen::Dynamic > getWorldJacobian()
Definition: PointMass.cpp:653
void setMass(double _mass)
Definition: PointMass.cpp:202
void resetConstraintImpulses()
Definition: PointMass.cpp:508
SoftBodyNode * getParentSoftBodyNode()
Definition: PointMass.cpp:665
void aggregateGravityForceVector(Eigen::VectorXd &_g, const Eigen::Vector3d &_gravity)
Definition: PointMass.cpp:1041
std::size_t getNumConnectedPointMasses() const
Definition: PointMass.cpp:259
Eigen::Vector3d mBiasForceForInvMeta
Definition: PointMass.hpp:531
const Eigen::Vector3d & getForces() const
Definition: PointMass.cpp:458
Eigen::Vector3d mBeta
Definition: PointMass.hpp:614
void clearConstraintImpulse()
Clear constraint impulse.
Definition: PointMass.cpp:584
Eigen::Vector3d mVelocitiesDeriv
Derivatives w.r.t. an arbitrary scalr variable.
Definition: PointMass.hpp:567
void updateAccelerationFD()
Update body acceleration. Forward dynamics routine.
Definition: PointMass.cpp:864
std::vector< std::size_t > mDependentGenCoordIndices
A increasingly sorted list of dependent dof indices.
Definition: PointMass.hpp:641
std::size_t getNumDofs() const
Definition: PointMass.cpp:294
const Eigen::Vector3d & getVelocities() const
Definition: PointMass.cpp:378
const Eigen::Vector3d & getWorldPosition() const
Definition: PointMass.cpp:624
void updateTransform() const
Update transformation.
Definition: PointMass.cpp:726
double getForce(std::size_t _index)
Definition: PointMass.cpp:444
Eigen::Vector3d mCg_F
Definition: PointMass.hpp:542
double mPsi
Definition: PointMass.hpp:623
Eigen::Vector3d mEta
Partial Acceleration of this PointMass.
Definition: PointMass.hpp:608
double getPosition(std::size_t _index) const
Definition: PointMass.cpp:326
void updateVelocityChangeFD()
Update body velocity change.
Definition: PointMass.cpp:917
std::size_t getIndexInSoftBodyNode() const
Definition: PointMass.cpp:196
Eigen::Vector3d mImpF
Generalized impulsive body force w.r.t. body frame.
Definition: PointMass.hpp:661
virtual ~PointMass()
Default destructor.
Definition: PointMass.cpp:178
State & getState()
State of this PointMass.
Definition: PointMass.cpp:184
void updateAccelerationID() const
Update body acceleration with the partial body acceleration.
Definition: PointMass.cpp:758
Eigen::Vector3d getWorldVelocity() const
Get the generalized velocity at the position of this point mass where the velocity is expressed in th...
Definition: PointMass.cpp:698
Eigen::Vector3d mCg_dV
Definition: PointMass.hpp:539
void updateTransmittedForceID(const Eigen::Vector3d &_gravity, bool _withExternalForces=false)
Update body force. Inverse dynamics routine.
Definition: PointMass.cpp:769
void setForce(std::size_t _index, double _force)
Definition: PointMass.cpp:436
void setConstraintImpulse(std::size_t _index, double _impulse)
Definition: PointMass.cpp:492
Eigen::Vector3d mFext
External force.
Definition: PointMass.hpp:638
const Eigen::Vector3d & getPositions() const
Definition: PointMass.cpp:341
double mImplicitPi
Definition: PointMass.hpp:632
const Eigen::Vector3d & getLocalPosition() const
Definition: PointMass.cpp:616
PointMass * getConnectedPointMass(std::size_t _idx)
Definition: PointMass.cpp:266
const Eigen::Vector3d & getBodyAcceleration() const
Get the generalized acceleration at the position of this point mass where the acceleration is express...
Definition: PointMass.cpp:704
bool isColliding()
Return whether this point mass is set to be colliding with other objects.
Definition: PointMass.cpp:288
double getVelocity(std::size_t _index) const
Definition: PointMass.cpp:363
const Eigen::Vector3d & getBodyVelocityChange() const
Return velocity change due to impulse.
Definition: PointMass.cpp:659
PointMassNotifier * mNotifier
Definition: PointMass.hpp:663
void resetVelocities()
Definition: PointMass.cpp:384
void updateTransmittedImpulse()
Update body force. Impulse-based forward dynamics routine.
Definition: PointMass.cpp:944
void updateVelocity() const
Update body velocity.
Definition: PointMass.cpp:739
Eigen::Vector3d getConstraintImpulses() const
Get constraint impulse.
Definition: PointMass.cpp:578
const Eigen::Vector3d & getAccelerations() const
Definition: PointMass.cpp:415
const Eigen::Vector3d & getRestingPosition() const
Definition: PointMass.cpp:610
void updateCombinedVector()
Definition: PointMass.cpp:1056
void updateTransmittedForce()
Update body force. Forward dynamics routine.
Definition: PointMass.cpp:884
void updateArtInertiaFD(double _timeStep) const
Update articulated body inertia.
Definition: PointMass.cpp:787
void resetVelocityChanges()
Definition: PointMass.cpp:486
Eigen::Vector3d mAccelerationsDeriv
Derivatives w.r.t. an arbitrary scalr variable.
Definition: PointMass.hpp:574
void addConstraintImpulse(const Eigen::Vector3d &_constImp, bool _isLocal=false)
Add constraint impulse.
Definition: PointMass.cpp:562
Eigen::Vector3d mForcesDeriv
Derivatives w.r.t. an arbitrary scalr variable.
Definition: PointMass.hpp:581
Eigen::Vector3d mVelocityChanges
Change of generalized velocity.
Definition: PointMass.hpp:588
void updateBiasImpulseFD()
Update bias impulse associated with the articulated body inertia.
Definition: PointMass.cpp:902
Eigen::Vector3d mX
Current position viewed in parent soft body node frame.
Definition: PointMass.hpp:602
void resetPositions()
Definition: PointMass.cpp:347
double getAcceleration(std::size_t _index) const
Definition: PointMass.cpp:400
double getImplicitPi() const
Definition: PointMass.cpp:242
void updateMassMatrix()
Definition: PointMass.cpp:893
void aggregateExternalForces(Eigen::VectorXd &_Fext)
Aggregate the external forces mFext in the generalized coordinates recursively.
Definition: PointMass.cpp:1082
void setVelocities(const Eigen::Vector3d &_velocities)
Definition: PointMass.cpp:371
Eigen::Vector3d mPositionDeriv
Derivatives w.r.t. an arbitrary scalr variable.
Definition: PointMass.hpp:560
void setForces(const Eigen::Vector3d &_forces)
Definition: PointMass.cpp:452
Eigen::Vector3d mImpBeta
Cache data for mImpB.
Definition: PointMass.hpp:658
void addExtForce(const Eigen::Vector3d &_force, bool _isForceLocal=false)
Add linear Cartesian force to this node.
Definition: PointMass.cpp:526
void setRestingPosition(const Eigen::Vector3d &_p)
Definition: PointMass.cpp:596
Eigen::Vector3d mConstraintImpulses
Generalized constraint impulse.
Definition: PointMass.hpp:594
void setColliding(bool _isColliding)
Set whether this point mass is colliding with other objects.
Definition: PointMass.cpp:282
void updateConstrainedTermsFD(double _timeStep)
Update constrained terms due to the constraint impulses.
Definition: PointMass.cpp:952
void clearExtForce()
Definition: PointMass.cpp:540
void aggregateCombinedVector(Eigen::VectorXd &_Cg, const Eigen::Vector3d &_gravity)
Definition: PointMass.cpp:1064
void addConnectedPointMass(PointMass *_pointMass)
Definition: PointMass.cpp:249
void aggregateAugMassMatrix(Eigen::MatrixXd &_MCol, int _col, double _timeStep)
Definition: PointMass.cpp:983
Eigen::Vector3d mV
Current velocity viewed in parent soft body node frame.
Definition: PointMass.hpp:605
void aggregateInvMassMatrix(Eigen::MatrixXd &_MInvCol, int _col)
Definition: PointMass.cpp:1012
double mImplicitPsi
Definition: PointMass.hpp:626
void resetForces()
Definition: PointMass.cpp:464
double mPi
Definition: PointMass.hpp:629
void setAcceleration(std::size_t _index, double _acceleration)
Definition: PointMass.cpp:391
Eigen::Vector3d mA
Current acceleration viewed in parent body node frame.
Definition: PointMass.hpp:617
void setVelocity(std::size_t _index, double _velocity)
Definition: PointMass.cpp:354
Eigen::Vector3d mM_F
Definition: PointMass.hpp:527
Eigen::Vector3d mG_F
Definition: PointMass.hpp:535
void resetAccelerations()
Definition: PointMass.cpp:429
double getConstraintImpulse(std::size_t _index)
Definition: PointMass.cpp:500
PointMass(SoftBodyNode *_softBodyNode)
Constructor used by SoftBodyNode.
Definition: PointMass.cpp:141
const Eigen::Vector3d & getBodyVelocity() const
The number of the generalized coordinates by which this node is affected.
Definition: PointMass.cpp:690
void init()
Definition: PointMass.cpp:719
void updatePartialAcceleration() const
Update partial body acceleration due to parent joint's velocity.
Definition: PointMass.cpp:749
const Eigen::Vector3d & getPartialAccelerations() const
Get the Eta term of this PointMass.
Definition: PointMass.cpp:421
bool mIsColliding
Whether the node is currently in collision with another node.
Definition: PointMass.hpp:644
Eigen::Vector3d mAlpha
Definition: PointMass.hpp:611
double getPi() const
Definition: PointMass.cpp:235
SoftBodyNode * mParentSoftBodyNode
SoftBodyNode that this PointMass belongs to.
Definition: PointMass.hpp:550
std::size_t mIndex
Index of this PointMass within the SoftBodyNode.
Definition: PointMass.hpp:553
void setAccelerations(const Eigen::Vector3d &_accelerations)
Definition: PointMass.cpp:408
void setPositions(const Eigen::Vector3d &_positions)
Definition: PointMass.cpp:334
Eigen::Matrix< double, 3, Eigen::Dynamic > getBodyJacobian()
Definition: PointMass.cpp:632
SoftBodyNode represent a soft body that has one deformable skin.
Definition: SoftBodyNode.hpp:46
Definition: BulletCollisionDetector.cpp:65
Properties for each PointMass.
Definition: PointMass.hpp:84
bool operator!=(const Properties &other) const
Definition: PointMass.cpp:135
Eigen::Vector3d mVelocityUpperLimits
Max value allowed.
Definition: PointMass.hpp:104
Eigen::Vector3d mAccelerationLowerLimits
Min value allowed.
Definition: PointMass.hpp:107
Eigen::Vector3d mAccelerationUpperLimits
upper limit of generalized acceleration
Definition: PointMass.hpp:110
Eigen::Vector3d mForceLowerLimits
Min value allowed.
Definition: PointMass.hpp:113
Eigen::Vector3d mX0
Resting position viewed in the parent SoftBodyNode frame.
Definition: PointMass.hpp:86
Eigen::Vector3d mPositionUpperLimits
Upper limit of position.
Definition: PointMass.hpp:98
double mMass
Mass.
Definition: PointMass.hpp:89
virtual ~Properties()=default
Eigen::Vector3d mForceUpperLimits
Max value allowed.
Definition: PointMass.hpp:116
void setRestingPosition(const Eigen::Vector3d &_x)
Definition: PointMass.cpp:104
std::vector< std::size_t > mConnectedPointMassIndices
Indices of connected Point Masses.
Definition: PointMass.hpp:92
bool operator==(const Properties &other) const
Definition: PointMass.cpp:116
void setMass(double _mass)
Definition: PointMass.cpp:110
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()))
Definition: PointMass.cpp:76
Eigen::Vector3d mVelocityLowerLimits
Min value allowed.
Definition: PointMass.hpp:101
Eigen::Vector3d mPositionLowerLimits
Lower limit of position.
Definition: PointMass.hpp:95
State for each PointMass.
Definition: PointMass.hpp:57
Eigen::Vector3d mPositions
Position.
Definition: PointMass.hpp:59
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())
Default constructor.
Definition: PointMass.cpp:51
Eigen::Vector3d mForces
Generalized force.
Definition: PointMass.hpp:68
Eigen::Vector3d mAccelerations
Generalized acceleration.
Definition: PointMass.hpp:65
bool operator==(const State &other) const
Definition: PointMass.cpp:65
Eigen::Vector3d mVelocities
Generalized velocity.
Definition: PointMass.hpp:62
static constexpr T inf()
Definition: Constants.hpp:68