DART  6.7.3
PointMass.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, 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/math/Helpers.hpp"
39 #include "dart/dynamics/Entity.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(const Eigen::Vector3d& positions = Eigen::Vector3d::Zero(),
72  const Eigen::Vector3d& velocities = Eigen::Vector3d::Zero(),
73  const Eigen::Vector3d& accelerations = Eigen::Vector3d::Zero(),
74  const Eigen::Vector3d& forces = Eigen::Vector3d::Zero());
75 
76  bool operator==(const State& other) const;
77 
78  virtual ~State() = default;
79  };
80 
82  struct Properties
83  {
85  Eigen::Vector3d mX0;
86 
88  double mMass;
89 
91  std::vector<std::size_t> mConnectedPointMassIndices;
92 
94  Eigen::Vector3d mPositionLowerLimits; // Currently unused
95 
97  Eigen::Vector3d mPositionUpperLimits; // Currently unused
98 
100  Eigen::Vector3d mVelocityLowerLimits; // Currently unused
101 
103  Eigen::Vector3d mVelocityUpperLimits; // Currently unused
104 
106  Eigen::Vector3d mAccelerationLowerLimits; // Currently unused
107 
109  Eigen::Vector3d mAccelerationUpperLimits; // Currently unused
110 
112  Eigen::Vector3d mForceLowerLimits; // Currently unused
113 
115  Eigen::Vector3d mForceUpperLimits; // Currently unused
116 
117  Properties(const Eigen::Vector3d& _X0 = Eigen::Vector3d::Zero(),
118  double _mass = 0.0005,
119  const std::vector<std::size_t>& _connections = std::vector<std::size_t>(),
120  const Eigen::Vector3d& _positionLowerLimits =
121  Eigen::Vector3d::Constant(-math::constantsd::inf()),
122  const Eigen::Vector3d& _positionUpperLimits =
123  Eigen::Vector3d::Constant( math::constantsd::inf()),
124  const Eigen::Vector3d& _velocityLowerLimits =
125  Eigen::Vector3d::Constant(-math::constantsd::inf()),
126  const Eigen::Vector3d& _velocityUpperLimits =
127  Eigen::Vector3d::Constant( math::constantsd::inf()),
128  const Eigen::Vector3d& _accelerationLowerLimits =
129  Eigen::Vector3d::Constant(-math::constantsd::inf()),
130  const Eigen::Vector3d& _accelerationUpperLimits =
131  Eigen::Vector3d::Constant( math::constantsd::inf()),
132  const Eigen::Vector3d& _forceLowerLimits =
133  Eigen::Vector3d::Constant(-math::constantsd::inf()),
134  const Eigen::Vector3d& _forceUpperLimits =
135  Eigen::Vector3d::Constant( math::constantsd::inf()));
136 
137  void setRestingPosition(const Eigen::Vector3d& _x);
138 
139  void setMass(double _mass);
140 
141  bool operator==(const Properties& other) const;
142 
143  bool operator!=(const Properties& other) const;
144 
145  virtual ~Properties() = default;
146  };
147 
148  //--------------------------------------------------------------------------
149  // Constructor and Desctructor
150  //--------------------------------------------------------------------------
151 
153  virtual ~PointMass();
154 
156  State& getState();
157 
159  const State& getState() const;
160 
162  std::size_t getIndexInSoftBodyNode() const;
163 
165  void setMass(double _mass);
166 
168  double getMass() const;
169 
171  double getPsi() const;
172 
174  double getImplicitPsi() const;
175 
177  double getPi() const;
178 
180  double getImplicitPi() const;
181 
183  void addConnectedPointMass(PointMass* _pointMass);
184 
186  std::size_t getNumConnectedPointMasses() const;
187 
189  PointMass* getConnectedPointMass(std::size_t _idx);
190 
192  const PointMass* getConnectedPointMass(std::size_t _idx) const;
193 
194 
199  void setColliding(bool _isColliding);
200 
203  bool isColliding();
204 
205  //----------------------------------------------------------------------------
206 
207  // Documentation inherited
208  std::size_t getNumDofs() const;
209 
210 // // Documentation inherited
211 // void setIndexInSkeleton(std::size_t _index, std::size_t _indexInSkeleton);
212 
213 // // Documentation inherited
214 // std::size_t getIndexInSkeleton(std::size_t _index) const;
215 
216  //----------------------------------------------------------------------------
217  // Position
218  //----------------------------------------------------------------------------
219 
220  // Documentation inherited
221  void setPosition(std::size_t _index, double _position);
222 
223  // Documentation inherited
224  double getPosition(std::size_t _index) const;
225 
226  // Documentation inherited
227  void setPositions(const Eigen::Vector3d& _positions);
228 
229  // Documentation inherited
230  const Eigen::Vector3d& getPositions() const;
231 
232  // Documentation inherited
233  void resetPositions();
234 
235  //----------------------------------------------------------------------------
236  // Velocity
237  //----------------------------------------------------------------------------
238 
239  // Documentation inherited
240  void setVelocity(std::size_t _index, double _velocity);
241 
242  // Documentation inherited
243  double getVelocity(std::size_t _index) const;
244 
245  // Documentation inherited
246  void setVelocities(const Eigen::Vector3d& _velocities);
247 
248  // Documentation inherited
249  const Eigen::Vector3d& getVelocities() const;
250 
251  // Documentation inherited
252  void resetVelocities();
253 
254  //----------------------------------------------------------------------------
255  // Acceleration
256  //----------------------------------------------------------------------------
257 
258  // Documentation inherited
259  void setAcceleration(std::size_t _index, double _acceleration);
260 
261  // Documentation inherited
262  double getAcceleration(std::size_t _index) const;
263 
264  // Documentation inherited
265  void setAccelerations(const Eigen::Vector3d& _accelerations);
266 
267  // Documentation inherited
268  const Eigen::Vector3d& getAccelerations() const;
269 
271  const Eigen::Vector3d& getPartialAccelerations() const;
272 
273  // Documentation inherited
274  void resetAccelerations();
275 
276  //----------------------------------------------------------------------------
277  // Force
278  //----------------------------------------------------------------------------
279 
280  // Documentation inherited
281  void setForce(std::size_t _index, double _force);
282 
283  // Documentation inherited
284  double getForce(std::size_t _index);
285 
286  // Documentation inherited
287  void setForces(const Eigen::Vector3d& _forces);
288 
289  // Documentation inherited
290  const Eigen::Vector3d& getForces() const;
291 
292  // Documentation inherited
293  void resetForces();
294 
295  //----------------------------------------------------------------------------
296  // Velocity change
297  //----------------------------------------------------------------------------
298 
299  // Documentation inherited
300  void setVelocityChange(std::size_t _index, double _velocityChange);
301 
302  // Documentation inherited
303  double getVelocityChange(std::size_t _index);
304 
305  // Documentation inherited
306  void resetVelocityChanges();
307 
308  //----------------------------------------------------------------------------
309  // Constraint impulse
310  //----------------------------------------------------------------------------
311 
312  // Documentation inherited
313  void setConstraintImpulse(std::size_t _index, double _impulse);
314 
315  // Documentation inherited
316  double getConstraintImpulse(std::size_t _index);
317 
318  // Documentation inherited
320 
321  //----------------------------------------------------------------------------
322  // Integration
323  //----------------------------------------------------------------------------
324 
325  // Documentation inherited
326  void integratePositions(double _dt);
327 
328  // Documentation inherited
329  void integrateVelocities(double _dt);
330 
331  //----------------------------------------------------------------------------
332 
338  void addExtForce(const Eigen::Vector3d& _force, bool _isForceLocal = false);
339 
341  void clearExtForce();
342 
343  //----------------------------------------------------------------------------
344  // Constraints
345  // - Following functions are managed by constraint solver.
346  //----------------------------------------------------------------------------
348  void setConstraintImpulse(const Eigen::Vector3d& _constImp,
349  bool _isLocal = false);
350 
352  void addConstraintImpulse(const Eigen::Vector3d& _constImp,
353  bool _isLocal = false);
354 
356  void clearConstraintImpulse();
357 
359  Eigen::Vector3d getConstraintImpulses() const;
360 
361  //----------------------------------------------------------------------------
363  void setRestingPosition(const Eigen::Vector3d& _p);
364 
366  const Eigen::Vector3d& getRestingPosition() const;
367 
369  const Eigen::Vector3d& getLocalPosition() const;
370 
372  const Eigen::Vector3d& getWorldPosition() const;
373 
375  Eigen::Matrix<double, 3, Eigen::Dynamic> getBodyJacobian();
376  Eigen::Matrix<double, 3, Eigen::Dynamic> getWorldJacobian();
377 
379  const Eigen::Vector3d& getBodyVelocityChange() const;
380 
383 
385  const SoftBodyNode* getParentSoftBodyNode() const;
386 
389 // int getNumDependentGenCoords() const;
390 
393 // int getDependentGenCoord(int _arrayIndex) const;
394 
397  const Eigen::Vector3d& getBodyVelocity() const;
398 
401  Eigen::Vector3d getWorldVelocity() const;
402 
406  const Eigen::Vector3d& getBodyAcceleration() const;
407 
410  Eigen::Vector3d getWorldAcceleration() const;
411 
412 protected:
414  explicit PointMass(SoftBodyNode* _softBodyNode);
415 
417  void init();
418 
419  //----------------------------------------------------------------------------
421  //----------------------------------------------------------------------------
422 
424  void updateTransform() const;
425 
427  void updateVelocity() const;
428 
430  void updatePartialAcceleration() const;
431 
434  void updateArtInertiaFD(double _timeStep) const;
435 
440  void updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity);
441 
444  void updateBiasImpulseFD();
445 
447  void updateAccelerationID() const;
448 
450  void updateAccelerationFD();
451 
454  void updateVelocityChangeFD();
455 
457  void updateTransmittedForceID(const Eigen::Vector3d& _gravity,
458  bool _withExternalForces = false);
459 
461  void updateTransmittedForce();
462 
465 
467  void updateJointForceID(double _timeStep,
468  double _withDampingForces,
469  double _withSpringForces);
470 
473  void updateConstrainedTermsFD(double _timeStep);
474 
476 
477  //----------------------------------------------------------------------------
479  //----------------------------------------------------------------------------
480 
482  void updateMassMatrix();
483 
485  void aggregateMassMatrix(Eigen::MatrixXd& _MCol, int _col);
486 
488  void aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, int _col,
489  double _timeStep);
490 
492  void updateInvMassMatrix();
493 
495  void updateInvAugMassMatrix();
496 
498  void aggregateInvMassMatrix(Eigen::MatrixXd& _MInvCol, int _col);
499 
501  void aggregateInvAugMassMatrix(Eigen::MatrixXd& _MInvCol, int _col,
502  double _timeStep);
503 
505  void aggregateGravityForceVector(Eigen::VectorXd& _g,
506  const Eigen::Vector3d& _gravity);
507 
509  void updateCombinedVector();
510 
512  void aggregateCombinedVector(Eigen::VectorXd& _Cg,
513  const Eigen::Vector3d& _gravity);
514 
517  void aggregateExternalForces(Eigen::VectorXd& _Fext);
518 
520 
521  //-------------------- Cache Data for Mass Matrix ----------------------------
523  Eigen::Vector3d mM_dV;
524 
526  Eigen::Vector3d mM_F;
527 
528  //----------------- Cache Data for Mass Inverse Matrix -----------------------
530  Eigen::Vector3d mBiasForceForInvMeta;
531 
532  //---------------- Cache Data for Gravity Force Vector -----------------------
534  Eigen::Vector3d mG_F;
535 
536  //------------------- Cache Data for Combined Vector -------------------------
538  Eigen::Vector3d mCg_dV;
539 
541  Eigen::Vector3d mCg_F;
542 
543 protected:
544  // TODO(JS): Need?
546 // Eigen::Matrix<std::size_t, 3, 1> mIndexInSkeleton;
547 
550 
552  std::size_t mIndex;
553 
554  //----------------------------------------------------------------------------
555  // Configuration
556  //----------------------------------------------------------------------------
557 
559  Eigen::Vector3d mPositionDeriv;
560 
561  //----------------------------------------------------------------------------
562  // Velocity
563  //----------------------------------------------------------------------------
564 
566  Eigen::Vector3d mVelocitiesDeriv;
567 
568  //----------------------------------------------------------------------------
569  // Acceleration
570  //----------------------------------------------------------------------------
571 
573  Eigen::Vector3d mAccelerationsDeriv;
574 
575  //----------------------------------------------------------------------------
576  // Force
577  //----------------------------------------------------------------------------
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 
676  PointMassNotifier(SoftBodyNode* _parentSoftBody, const std::string& _name);
677 
678  bool needsPartialAccelerationUpdate() const;
679 
680  void clearTransformNotice();
681  void clearVelocityNotice();
684 
685  void dirtyTransform() override;
686  void dirtyVelocity() override;
687  void dirtyAcceleration() override;
688 
689  // Documentation inherited
690  const std::string& setName(const std::string& _name) override;
691 
692  // Documentation inherited
693  const std::string& getName() const override;
694 
695 protected:
696 
697  std::string mName;
698 
700  // TODO(JS): Rename this to mIsPartialAccelerationDirty in DART 7
701 
703 
704 };
705 
706 } // namespace dynamics
707 } // namespace dart
708 
709 #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:60
Definition: PointMass.hpp:673
void dirtyAcceleration() override
Notify the acceleration of this Entity that its parent Frame's acceleration is needed.
Definition: PointMass.cpp:1142
bool mNeedPartialAccelerationUpdate
Definition: PointMass.hpp:699
SoftBodyNode * mParentSoftBodyNode
Definition: PointMass.hpp:702
void dirtyTransform() override
Notify the transformation update of this Entity that its parent Frame's pose is needed.
Definition: PointMass.cpp:1120
void clearTransformNotice()
Definition: PointMass.cpp:1096
const std::string & setName(const std::string &_name) override
Set name.
Definition: PointMass.cpp:1148
PointMassNotifier(SoftBodyNode *_parentSoftBody, const std::string &_name)
Definition: PointMass.cpp:1080
bool needsPartialAccelerationUpdate() const
Definition: PointMass.cpp:1090
const std::string & getName() const override
Return the name of this Entity.
Definition: PointMass.cpp:1163
void clearVelocityNotice()
Definition: PointMass.cpp:1102
std::string mName
Definition: PointMass.hpp:697
void clearPartialAccelerationNotice()
Definition: PointMass.cpp:1108
void clearAccelerationNotice()
Definition: PointMass.cpp:1114
void dirtyVelocity() override
Notify the velocity update of this Entity that its parent Frame's velocity is needed.
Definition: PointMass.cpp:1132
Definition: PointMass.hpp:51
Eigen::Vector3d mF
Definition: PointMass.hpp:620
Eigen::Vector3d mM_dV
Definition: PointMass.hpp:523
void updateJointForceID(double _timeStep, double _withDampingForces, double _withSpringForces)
Update the joint force. Inverse dynamics routine.
Definition: PointMass.cpp:807
void setPosition(std::size_t _index, double _position)
Definition: PointMass.cpp:315
void aggregateMassMatrix(Eigen::MatrixXd &_MCol, int _col)
Definition: PointMass.cpp:964
void updateBiasForceFD(double _dt, const Eigen::Vector3d &_gravity)
Update bias force associated with the articulated body inertia.
Definition: PointMass.cpp:817
double getPsi() const
Definition: PointMass.cpp:220
double getMass() const
Definition: PointMass.cpp:214
Eigen::Vector3d mDelV
Velocity change due to constraint impulse.
Definition: PointMass.hpp:648
void integratePositions(double _dt)
Definition: PointMass.cpp:512
void updateInvAugMassMatrix()
Definition: PointMass.cpp:997
void integrateVelocities(double _dt)
Definition: PointMass.cpp:518
double getVelocityChange(std::size_t _index)
Definition: PointMass.cpp:476
Eigen::Vector3d mB
Bias force.
Definition: PointMass.hpp:635
void aggregateInvAugMassMatrix(Eigen::MatrixXd &_MInvCol, int _col, double _timeStep)
Definition: PointMass.cpp:1017
double getImplicitPsi() const
Definition: PointMass.cpp:227
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:991
void setVelocityChange(std::size_t _index, double _velocityChange)
Definition: PointMass.cpp:468
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:652
void setMass(double _mass)
Definition: PointMass.cpp:202
void resetConstraintImpulses()
Definition: PointMass.cpp:506
SoftBodyNode * getParentSoftBodyNode()
Definition: PointMass.cpp:665
void aggregateGravityForceVector(Eigen::VectorXd &_g, const Eigen::Vector3d &_gravity)
Definition: PointMass.cpp:1033
std::size_t getNumConnectedPointMasses() const
Definition: PointMass.cpp:258
Eigen::Vector3d mBiasForceForInvMeta
Definition: PointMass.hpp:530
const Eigen::Vector3d & getForces() const
Definition: PointMass.cpp:456
Eigen::Vector3d mBeta
Definition: PointMass.hpp:614
void clearConstraintImpulse()
Clear constraint impulse.
Definition: PointMass.cpp:582
Eigen::Vector3d mVelocitiesDeriv
Derivatives w.r.t. an arbitrary scalr variable.
Definition: PointMass.hpp:566
void updateAccelerationFD()
Update body acceleration. Forward dynamics routine.
Definition: PointMass.cpp:858
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:293
const Eigen::Vector3d & getVelocities() const
Definition: PointMass.cpp:376
const Eigen::Vector3d & getWorldPosition() const
Definition: PointMass.cpp:622
void updateTransform() const
Update transformation.
Definition: PointMass.cpp:724
double getForce(std::size_t _index)
Definition: PointMass.cpp:442
Eigen::Vector3d mCg_F
Definition: PointMass.hpp:541
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:324
void updateVelocityChangeFD()
Update body velocity change.
Definition: PointMass.cpp:909
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:756
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:538
void updateTransmittedForceID(const Eigen::Vector3d &_gravity, bool _withExternalForces=false)
Update body force. Inverse dynamics routine.
Definition: PointMass.cpp:766
void setForce(std::size_t _index, double _force)
Definition: PointMass.cpp:434
void setConstraintImpulse(std::size_t _index, double _impulse)
Definition: PointMass.cpp:490
Eigen::Vector3d mFext
External force.
Definition: PointMass.hpp:638
const Eigen::Vector3d & getPositions() const
Definition: PointMass.cpp:339
double mImplicitPi
Definition: PointMass.hpp:632
const Eigen::Vector3d & getLocalPosition() const
Definition: PointMass.cpp:614
PointMass * getConnectedPointMass(std::size_t _idx)
Definition: PointMass.cpp:265
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:287
double getVelocity(std::size_t _index) const
Definition: PointMass.cpp:361
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:382
void updateTransmittedImpulse()
Update body force. Impulse-based forward dynamics routine.
Definition: PointMass.cpp:936
void updateVelocity() const
Update body velocity.
Definition: PointMass.cpp:737
Eigen::Vector3d getConstraintImpulses() const
Get constraint impulse.
Definition: PointMass.cpp:576
const Eigen::Vector3d & getAccelerations() const
Definition: PointMass.cpp:413
const Eigen::Vector3d & getRestingPosition() const
Definition: PointMass.cpp:608
void updateCombinedVector()
Definition: PointMass.cpp:1047
void updateTransmittedForce()
Update body force. Forward dynamics routine.
Definition: PointMass.cpp:876
void updateArtInertiaFD(double _timeStep) const
Update articulated body inertia.
Definition: PointMass.cpp:782
void resetVelocityChanges()
Definition: PointMass.cpp:484
Eigen::Vector3d mAccelerationsDeriv
Derivatives w.r.t. an arbitrary scalr variable.
Definition: PointMass.hpp:573
void addConstraintImpulse(const Eigen::Vector3d &_constImp, bool _isLocal=false)
Add constraint impulse.
Definition: PointMass.cpp:560
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:894
Eigen::Vector3d mX
Current position viewed in parent soft body node frame.
Definition: PointMass.hpp:602
void resetPositions()
Definition: PointMass.cpp:345
double getAcceleration(std::size_t _index) const
Definition: PointMass.cpp:398
double getImplicitPi() const
Definition: PointMass.cpp:241
void updateMassMatrix()
Definition: PointMass.cpp:885
void aggregateExternalForces(Eigen::VectorXd &_Fext)
Aggregate the external forces mFext in the generalized coordinates recursively.
Definition: PointMass.cpp:1072
void setVelocities(const Eigen::Vector3d &_velocities)
Definition: PointMass.cpp:369
Eigen::Vector3d mPositionDeriv
Derivatives w.r.t. an arbitrary scalr variable.
Definition: PointMass.hpp:559
void setForces(const Eigen::Vector3d &_forces)
Definition: PointMass.cpp:450
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:524
void setRestingPosition(const Eigen::Vector3d &_p)
Definition: PointMass.cpp:594
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:281
void updateConstrainedTermsFD(double _timeStep)
Update constrained terms due to the constraint impulses.
Definition: PointMass.cpp:944
void clearExtForce()
Definition: PointMass.cpp:538
void aggregateCombinedVector(Eigen::VectorXd &_Cg, const Eigen::Vector3d &_gravity)
Definition: PointMass.cpp:1055
void addConnectedPointMass(PointMass *_pointMass)
Definition: PointMass.cpp:248
void aggregateAugMassMatrix(Eigen::MatrixXd &_MCol, int _col, double _timeStep)
Definition: PointMass.cpp:975
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:1003
double mImplicitPsi
Definition: PointMass.hpp:626
void resetForces()
Definition: PointMass.cpp:462
double mPi
Definition: PointMass.hpp:629
void setAcceleration(std::size_t _index, double _acceleration)
Definition: PointMass.cpp:389
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:352
Eigen::Vector3d mM_F
Definition: PointMass.hpp:526
Eigen::Vector3d mG_F
Definition: PointMass.hpp:534
void resetAccelerations()
Definition: PointMass.cpp:427
double getConstraintImpulse(std::size_t _index)
Definition: PointMass.cpp:498
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:718
void updatePartialAcceleration() const
Update partial body acceleration due to parent joint's velocity.
Definition: PointMass.cpp:747
const Eigen::Vector3d & getPartialAccelerations() const
Get the Eta term of this PointMass.
Definition: PointMass.cpp:419
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:234
SoftBodyNode * mParentSoftBodyNode
SoftBodyNode that this PointMass belongs to.
Definition: PointMass.hpp:549
std::size_t mIndex
Index of this PointMass within the SoftBodyNode.
Definition: PointMass.hpp:552
void setAccelerations(const Eigen::Vector3d &_accelerations)
Definition: PointMass.cpp:406
void setPositions(const Eigen::Vector3d &_positions)
Definition: PointMass.cpp:332
Eigen::Matrix< double, 3, Eigen::Dynamic > getBodyJacobian()
Definition: PointMass.cpp:630
SoftBodyNode represent a soft body that has one deformable skin.
Definition: SoftBodyNode.hpp:46
Definition: BulletCollisionDetector.cpp:63
Properties for each PointMass.
Definition: PointMass.hpp:83
bool operator!=(const Properties &other) const
Definition: PointMass.cpp:135
Eigen::Vector3d mVelocityUpperLimits
Max value allowed.
Definition: PointMass.hpp:103
Eigen::Vector3d mAccelerationLowerLimits
Min value allowed.
Definition: PointMass.hpp:106
Eigen::Vector3d mAccelerationUpperLimits
upper limit of generalized acceleration
Definition: PointMass.hpp:109
Eigen::Vector3d mForceLowerLimits
Min value allowed.
Definition: PointMass.hpp:112
Eigen::Vector3d mX0
Resting position viewed in the parent SoftBodyNode frame.
Definition: PointMass.hpp:85
Eigen::Vector3d mPositionUpperLimits
Upper limit of position.
Definition: PointMass.hpp:97
double mMass
Mass.
Definition: PointMass.hpp:88
Eigen::Vector3d mForceUpperLimits
Max value allowed.
Definition: PointMass.hpp:115
void setRestingPosition(const Eigen::Vector3d &_x)
Definition: PointMass.cpp:104
std::vector< std::size_t > mConnectedPointMassIndices
Indices of connected Point Masses.
Definition: PointMass.hpp:91
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:100
Eigen::Vector3d mPositionLowerLimits
Lower limit of position.
Definition: PointMass.hpp:94
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:54