DART  6.10.1
PointMass.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_DYNAMICS_POINTMASS_HPP_
34 #define DART_DYNAMICS_POINTMASS_HPP_
35 
36 #include <vector>
37 #include <Eigen/Dense>
38 #include "dart/dynamics/Entity.hpp"
39 #include "dart/math/Helpers.hpp"
40 
41 namespace dart {
42 namespace dynamics {
43 
44 class EllipsoidShape;
45 class SoftBodyNode;
46 
47 class PointMassNotifier;
48 
50 class PointMass : public common::Subject
51 {
52 public:
53  friend class SoftBodyNode;
54 
56  struct State
57  {
59  Eigen::Vector3d mPositions;
60 
62  Eigen::Vector3d mVelocities;
63 
65  Eigen::Vector3d mAccelerations;
66 
68  Eigen::Vector3d mForces;
69 
71  State(
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());
76 
77  bool operator==(const State& other) const;
78 
79  virtual ~State() = default;
80  };
81 
83  struct Properties
84  {
86  Eigen::Vector3d mX0;
87 
89  double mMass;
90 
92  std::vector<std::size_t> mConnectedPointMassIndices;
93 
95  Eigen::Vector3d mPositionLowerLimits; // Currently unused
96 
98  Eigen::Vector3d mPositionUpperLimits; // Currently unused
99 
101  Eigen::Vector3d mVelocityLowerLimits; // Currently unused
102 
104  Eigen::Vector3d mVelocityUpperLimits; // Currently unused
105 
107  Eigen::Vector3d mAccelerationLowerLimits; // Currently unused
108 
110  Eigen::Vector3d mAccelerationUpperLimits; // Currently unused
111 
113  Eigen::Vector3d mForceLowerLimits; // Currently unused
114 
116  Eigen::Vector3d mForceUpperLimits; // Currently unused
117 
118  Properties(
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
124  = Eigen::Vector3d::Constant(-math::constantsd::inf()),
125  const Eigen::Vector3d& _positionUpperLimits
126  = Eigen::Vector3d::Constant(math::constantsd::inf()),
127  const Eigen::Vector3d& _velocityLowerLimits
128  = Eigen::Vector3d::Constant(-math::constantsd::inf()),
129  const Eigen::Vector3d& _velocityUpperLimits
130  = Eigen::Vector3d::Constant(math::constantsd::inf()),
131  const Eigen::Vector3d& _accelerationLowerLimits
132  = Eigen::Vector3d::Constant(-math::constantsd::inf()),
133  const Eigen::Vector3d& _accelerationUpperLimits
134  = Eigen::Vector3d::Constant(math::constantsd::inf()),
135  const Eigen::Vector3d& _forceLowerLimits
136  = Eigen::Vector3d::Constant(-math::constantsd::inf()),
137  const Eigen::Vector3d& _forceUpperLimits
138  = Eigen::Vector3d::Constant(math::constantsd::inf()));
139 
140  void setRestingPosition(const Eigen::Vector3d& _x);
141 
142  void setMass(double _mass);
143 
144  bool operator==(const Properties& other) const;
145 
146  bool operator!=(const Properties& other) const;
147 
148  virtual ~Properties() = default;
149  };
150 
151  //--------------------------------------------------------------------------
152  // Constructor and Desctructor
153  //--------------------------------------------------------------------------
154 
156  virtual ~PointMass();
157 
159  State& getState();
160 
162  const State& getState() const;
163 
165  std::size_t getIndexInSoftBodyNode() const;
166 
168  void setMass(double _mass);
169 
171  double getMass() const;
172 
174  double getPsi() const;
175 
177  double getImplicitPsi() const;
178 
180  double getPi() const;
181 
183  double getImplicitPi() const;
184 
186  void addConnectedPointMass(PointMass* _pointMass);
187 
189  std::size_t getNumConnectedPointMasses() const;
190 
192  PointMass* getConnectedPointMass(std::size_t _idx);
193 
195  const PointMass* getConnectedPointMass(std::size_t _idx) const;
196 
201  void setColliding(bool _isColliding);
202 
205  bool isColliding();
206 
207  //----------------------------------------------------------------------------
208 
209  // Documentation inherited
210  std::size_t getNumDofs() const;
211 
212  // // Documentation inherited
213  // void setIndexInSkeleton(std::size_t _index, std::size_t _indexInSkeleton);
214 
215  // // Documentation inherited
216  // std::size_t getIndexInSkeleton(std::size_t _index) const;
217 
218  //----------------------------------------------------------------------------
219  // Position
220  //----------------------------------------------------------------------------
221 
222  // Documentation inherited
223  void setPosition(std::size_t _index, double _position);
224 
225  // Documentation inherited
226  double getPosition(std::size_t _index) const;
227 
228  // Documentation inherited
229  void setPositions(const Eigen::Vector3d& _positions);
230 
231  // Documentation inherited
232  const Eigen::Vector3d& getPositions() const;
233 
234  // Documentation inherited
235  void resetPositions();
236 
237  //----------------------------------------------------------------------------
238  // Velocity
239  //----------------------------------------------------------------------------
240 
241  // Documentation inherited
242  void setVelocity(std::size_t _index, double _velocity);
243 
244  // Documentation inherited
245  double getVelocity(std::size_t _index) const;
246 
247  // Documentation inherited
248  void setVelocities(const Eigen::Vector3d& _velocities);
249 
250  // Documentation inherited
251  const Eigen::Vector3d& getVelocities() const;
252 
253  // Documentation inherited
254  void resetVelocities();
255 
256  //----------------------------------------------------------------------------
257  // Acceleration
258  //----------------------------------------------------------------------------
259 
260  // Documentation inherited
261  void setAcceleration(std::size_t _index, double _acceleration);
262 
263  // Documentation inherited
264  double getAcceleration(std::size_t _index) const;
265 
266  // Documentation inherited
267  void setAccelerations(const Eigen::Vector3d& _accelerations);
268 
269  // Documentation inherited
270  const Eigen::Vector3d& getAccelerations() const;
271 
273  const Eigen::Vector3d& getPartialAccelerations() const;
274 
275  // Documentation inherited
276  void resetAccelerations();
277 
278  //----------------------------------------------------------------------------
279  // Force
280  //----------------------------------------------------------------------------
281 
282  // Documentation inherited
283  void setForce(std::size_t _index, double _force);
284 
285  // Documentation inherited
286  double getForce(std::size_t _index);
287 
288  // Documentation inherited
289  void setForces(const Eigen::Vector3d& _forces);
290 
291  // Documentation inherited
292  const Eigen::Vector3d& getForces() const;
293 
294  // Documentation inherited
295  void resetForces();
296 
297  //----------------------------------------------------------------------------
298  // Velocity change
299  //----------------------------------------------------------------------------
300 
301  // Documentation inherited
302  void setVelocityChange(std::size_t _index, double _velocityChange);
303 
304  // Documentation inherited
305  double getVelocityChange(std::size_t _index);
306 
307  // Documentation inherited
308  void resetVelocityChanges();
309 
310  //----------------------------------------------------------------------------
311  // Constraint impulse
312  //----------------------------------------------------------------------------
313 
314  // Documentation inherited
315  void setConstraintImpulse(std::size_t _index, double _impulse);
316 
317  // Documentation inherited
318  double getConstraintImpulse(std::size_t _index);
319 
320  // Documentation inherited
322 
323  //----------------------------------------------------------------------------
324  // Integration
325  //----------------------------------------------------------------------------
326 
327  // Documentation inherited
328  void integratePositions(double _dt);
329 
330  // Documentation inherited
331  void integrateVelocities(double _dt);
332 
333  //----------------------------------------------------------------------------
334 
340  void addExtForce(const Eigen::Vector3d& _force, bool _isForceLocal = false);
341 
343  void clearExtForce();
344 
345  //----------------------------------------------------------------------------
346  // Constraints
347  // - Following functions are managed by constraint solver.
348  //----------------------------------------------------------------------------
351  const Eigen::Vector3d& _constImp, bool _isLocal = false);
352 
355  const Eigen::Vector3d& _constImp, bool _isLocal = false);
356 
358  void clearConstraintImpulse();
359 
361  Eigen::Vector3d getConstraintImpulses() const;
362 
363  //----------------------------------------------------------------------------
365  void setRestingPosition(const Eigen::Vector3d& _p);
366 
368  const Eigen::Vector3d& getRestingPosition() const;
369 
371  const Eigen::Vector3d& getLocalPosition() const;
372 
374  const Eigen::Vector3d& getWorldPosition() const;
375 
377  Eigen::Matrix<double, 3, Eigen::Dynamic> getBodyJacobian();
378  Eigen::Matrix<double, 3, Eigen::Dynamic> getWorldJacobian();
379 
381  const Eigen::Vector3d& getBodyVelocityChange() const;
382 
385 
387  const SoftBodyNode* getParentSoftBodyNode() const;
388 
391  // int getNumDependentGenCoords() const;
392 
395  // int getDependentGenCoord(int _arrayIndex) const;
396 
399  const Eigen::Vector3d& getBodyVelocity() const;
400 
403  Eigen::Vector3d getWorldVelocity() const;
404 
408  const Eigen::Vector3d& getBodyAcceleration() const;
409 
412  Eigen::Vector3d getWorldAcceleration() const;
413 
414 protected:
416  explicit PointMass(SoftBodyNode* _softBodyNode);
417 
419  void init();
420 
421  //----------------------------------------------------------------------------
423  //----------------------------------------------------------------------------
424 
426  void updateTransform() const;
427 
429  void updateVelocity() const;
430 
432  void updatePartialAcceleration() const;
433 
436  void updateArtInertiaFD(double _timeStep) const;
437 
442  void updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity);
443 
446  void updateBiasImpulseFD();
447 
449  void updateAccelerationID() const;
450 
452  void updateAccelerationFD();
453 
456  void updateVelocityChangeFD();
457 
460  const Eigen::Vector3d& _gravity, bool _withExternalForces = false);
461 
463  void updateTransmittedForce();
464 
467 
469  void updateJointForceID(
470  double _timeStep, double _withDampingForces, double _withSpringForces);
471 
474  void updateConstrainedTermsFD(double _timeStep);
475 
477 
478  //----------------------------------------------------------------------------
480  //----------------------------------------------------------------------------
481 
483  void updateMassMatrix();
484 
486  void aggregateMassMatrix(Eigen::MatrixXd& _MCol, int _col);
487 
490  Eigen::MatrixXd& _MCol, int _col, double _timeStep);
491 
493  void updateInvMassMatrix();
494 
496  void updateInvAugMassMatrix();
497 
499  void aggregateInvMassMatrix(Eigen::MatrixXd& _MInvCol, int _col);
500 
503  Eigen::MatrixXd& _MInvCol, int _col, double _timeStep);
504 
507  Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity);
508 
510  void updateCombinedVector();
511 
514  Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity);
515 
518  void aggregateExternalForces(Eigen::VectorXd& _Fext);
519 
521 
522  //-------------------- Cache Data for Mass Matrix ----------------------------
524  Eigen::Vector3d mM_dV;
525 
527  Eigen::Vector3d mM_F;
528 
529  //----------------- Cache Data for Mass Inverse Matrix -----------------------
531  Eigen::Vector3d mBiasForceForInvMeta;
532 
533  //---------------- Cache Data for Gravity Force Vector -----------------------
535  Eigen::Vector3d mG_F;
536 
537  //------------------- Cache Data for Combined Vector -------------------------
539  Eigen::Vector3d mCg_dV;
540 
542  Eigen::Vector3d mCg_F;
543 
544 protected:
545  // TODO(JS): Need?
547  // Eigen::Matrix<std::size_t, 3, 1> mIndexInSkeleton;
548 
551 
553  std::size_t mIndex;
554 
555  //----------------------------------------------------------------------------
556  // Configuration
557  //----------------------------------------------------------------------------
558 
560  Eigen::Vector3d mPositionDeriv;
561 
562  //----------------------------------------------------------------------------
563  // Velocity
564  //----------------------------------------------------------------------------
565 
567  Eigen::Vector3d mVelocitiesDeriv;
568 
569  //----------------------------------------------------------------------------
570  // Acceleration
571  //----------------------------------------------------------------------------
572 
574  Eigen::Vector3d mAccelerationsDeriv;
575 
576  //----------------------------------------------------------------------------
577  // Force
578  //----------------------------------------------------------------------------
579 
581  Eigen::Vector3d mForcesDeriv;
582 
583  //----------------------------------------------------------------------------
584  // Impulse
585  //----------------------------------------------------------------------------
586 
588  Eigen::Vector3d mVelocityChanges;
589 
590  // /// Generalized impulse
591  // Eigen::Vector3d mImpulse;
592 
594  Eigen::Vector3d mConstraintImpulses;
595 
596  //----------------------------------------------------------------------------
597 
599  mutable Eigen::Vector3d mW;
600 
602  mutable Eigen::Vector3d mX;
603 
605  mutable Eigen::Vector3d mV;
606 
608  mutable Eigen::Vector3d mEta;
609 
611  Eigen::Vector3d mAlpha;
612 
614  Eigen::Vector3d mBeta;
615 
617  mutable Eigen::Vector3d mA;
618 
620  Eigen::Vector3d mF;
621 
623  mutable double mPsi;
624 
626  mutable double mImplicitPsi;
627 
629  mutable double mPi;
630 
632  mutable double mImplicitPi;
633 
635  Eigen::Vector3d mB;
636 
638  Eigen::Vector3d mFext;
639 
641  std::vector<std::size_t> mDependentGenCoordIndices;
642 
645 
646  //------------------------- Impulse-based Dyanmics ---------------------------
648  Eigen::Vector3d mDelV;
649 
652  Eigen::Vector3d mImpB;
653 
655  Eigen::Vector3d mImpAlpha;
656 
658  Eigen::Vector3d mImpBeta;
659 
661  Eigen::Vector3d mImpF;
662 
664 };
665 
666 // struct PointMassPair
667 //{
668 // PointMass* pm1;
669 // PointMass* pm2;
670 //};
671 
672 class PointMassNotifier : public Entity
673 {
674 public:
675  PointMassNotifier(SoftBodyNode* _parentSoftBody, const std::string& _name);
676 
677  bool needsPartialAccelerationUpdate() const;
678 
679  void clearTransformNotice();
680  void clearVelocityNotice();
683 
684  void dirtyTransform() override;
685  void dirtyVelocity() override;
686  void dirtyAcceleration() override;
687 
688  // Documentation inherited
689  const std::string& setName(const std::string& _name) override;
690 
691  // Documentation inherited
692  const std::string& getName() const override;
693 
694 protected:
695  std::string mName;
696 
698  // TODO(JS): Rename this to mIsPartialAccelerationDirty in DART 7
699 
701 };
702 
703 } // namespace dynamics
704 } // namespace dart
705 
706 #endif // DART_DYNAMICS_POINTMASS_HPP_
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
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