DART 6.7.3
Loading...
Searching...
No Matches
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"
40
41namespace dart {
42namespace dynamics {
43
44class EllipsoidShape;
45class SoftBodyNode;
46
47class PointMassNotifier;
48
51{
52public:
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
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
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
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
412protected:
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
451
455
457 void updateTransmittedForceID(const Eigen::Vector3d& _gravity,
458 bool _withExternalForces = false);
459
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
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
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
543protected:
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
673{
674public:
675
676 PointMassNotifier(SoftBodyNode* _parentSoftBody, const std::string& _name);
677
679
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
695protected:
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
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
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
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
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