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 49 /// 50 class PointMass : public common::Subject 51 { 52 public: 53 friend class SoftBodyNode; 54 55 /// State for each PointMass 56 struct State 57 { 58 /// Position 59 Eigen::Vector3d mPositions; 60 61 /// Generalized velocity 62 Eigen::Vector3d mVelocities; 63 64 /// Generalized acceleration 65 Eigen::Vector3d mAccelerations; 66 67 /// Generalized force 68 Eigen::Vector3d mForces; 69 70 /// Default constructor 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 82 /// Properties for each PointMass 83 struct Properties 84 { 85 /// Resting position viewed in the parent SoftBodyNode frame 86 Eigen::Vector3d mX0; 87 88 /// Mass. 89 double mMass; 90 91 /// Indices of connected Point Masses 92 std::vector<std::size_t> mConnectedPointMassIndices; 93 94 /// Lower limit of position 95 Eigen::Vector3d mPositionLowerLimits; // Currently unused 96 97 /// Upper limit of position 98 Eigen::Vector3d mPositionUpperLimits; // Currently unused 99 100 /// Min value allowed. 101 Eigen::Vector3d mVelocityLowerLimits; // Currently unused 102 103 /// Max value allowed. 104 Eigen::Vector3d mVelocityUpperLimits; // Currently unused 105 106 /// Min value allowed. 107 Eigen::Vector3d mAccelerationLowerLimits; // Currently unused 108 109 /// upper limit of generalized acceleration 110 Eigen::Vector3d mAccelerationUpperLimits; // Currently unused 111 112 /// Min value allowed. 113 Eigen::Vector3d mForceLowerLimits; // Currently unused 114 115 /// Max value allowed. 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 155 /// Default destructor 156 virtual ~PointMass(); 157 158 /// State of this PointMass 159 State& getState(); 160 161 /// State of this PointMass 162 const State& getState() const; 163 164 /// 165 std::size_t getIndexInSoftBodyNode() const; 166 167 /// 168 void setMass(double _mass); 169 170 /// 171 double getMass() const; 172 173 /// 174 double getPsi() const; 175 176 /// 177 double getImplicitPsi() const; 178 179 /// 180 double getPi() const; 181 182 /// 183 double getImplicitPi() const; 184 185 /// 186 void addConnectedPointMass(PointMass* _pointMass); 187 188 /// 189 std::size_t getNumConnectedPointMasses() const; 190 191 /// 192 PointMass* getConnectedPointMass(std::size_t _idx); 193 194 /// 195 const PointMass* getConnectedPointMass(std::size_t _idx) const; 196 197 /// Set whether this point mass is colliding with other objects. Note that 198 /// this status is set by the constraint solver during dynamics simulation but 199 /// not by collision detector. 200 /// \param[in] _isColliding True if this point mass is colliding. 201 void setColliding(bool _isColliding); 202 203 /// Return whether this point mass is set to be colliding with other objects. 204 /// \return True if this point mass is colliding. 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 272 /// Get the Eta term of this PointMass 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 321 void resetConstraintImpulses(); 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 335 /// Add linear Cartesian force to this node. 336 /// \param[in] _force External force. 337 /// \param[in] _isForceLocal True if _force's reference frame is of the parent 338 /// soft body node. False if _force's reference frame 339 /// is of the world. 340 void addExtForce(const Eigen::Vector3d& _force, bool _isForceLocal = false); 341 342 /// 343 void clearExtForce(); 344 345 //---------------------------------------------------------------------------- 346 // Constraints 347 // - Following functions are managed by constraint solver. 348 //---------------------------------------------------------------------------- 349 /// Set constraint impulse 350 void setConstraintImpulse( 351 const Eigen::Vector3d& _constImp, bool _isLocal = false); 352 353 /// Add constraint impulse 354 void addConstraintImpulse( 355 const Eigen::Vector3d& _constImp, bool _isLocal = false); 356 357 /// Clear constraint impulse 358 void clearConstraintImpulse(); 359 360 /// Get constraint impulse 361 Eigen::Vector3d getConstraintImpulses() const; 362 363 //---------------------------------------------------------------------------- 364 /// 365 void setRestingPosition(const Eigen::Vector3d& _p); 366 367 /// 368 const Eigen::Vector3d& getRestingPosition() const; 369 370 /// 371 const Eigen::Vector3d& getLocalPosition() const; 372 373 /// 374 const Eigen::Vector3d& getWorldPosition() const; 375 376 /// \todo Temporary function. 377 Eigen::Matrix<double, 3, Eigen::Dynamic> getBodyJacobian(); 378 Eigen::Matrix<double, 3, Eigen::Dynamic> getWorldJacobian(); 379 380 /// Return velocity change due to impulse 381 const Eigen::Vector3d& getBodyVelocityChange() const; 382 383 /// 384 SoftBodyNode* getParentSoftBodyNode(); 385 386 /// 387 const SoftBodyNode* getParentSoftBodyNode() const; 388 389 /// The number of the generalized coordinates by which this node is 390 /// affected. 391 // int getNumDependentGenCoords() const; 392 393 /// Return a generalized coordinate index from the array index 394 /// (< getNumDependentDofs). 395 // int getDependentGenCoord(int _arrayIndex) const; 396 397 /// Get the generalized velocity at the position of this point mass 398 /// where the velocity is expressed in the parent soft body node frame. 399 const Eigen::Vector3d& getBodyVelocity() const; 400 401 /// Get the generalized velocity at the position of this point mass 402 /// where the velocity is expressed in the world frame. 403 Eigen::Vector3d getWorldVelocity() const; 404 405 /// Get the generalized acceleration at the position of this point mass 406 /// where the acceleration is expressed in the parent soft body node 407 /// frame. 408 const Eigen::Vector3d& getBodyAcceleration() const; 409 410 /// Get the generalized acceleration at the position of this point mass 411 /// where the acceleration is expressed in the world frame. 412 Eigen::Vector3d getWorldAcceleration() const; 413 414 protected: 415 /// Constructor used by SoftBodyNode 416 explicit PointMass(SoftBodyNode* _softBodyNode); 417 418 /// 419 void init(); 420 421 //---------------------------------------------------------------------------- 422 /// \{ \name Recursive dynamics routines 423 //---------------------------------------------------------------------------- 424 425 /// \brief Update transformation. 426 void updateTransform() const; 427 428 /// \brief Update body velocity. 429 void updateVelocity() const; 430 431 /// \brief Update partial body acceleration due to parent joint's velocity. 432 void updatePartialAcceleration() const; 433 434 /// \brief Update articulated body inertia. Forward dynamics routine. 435 /// \param[in] _timeStep Rquired for implicit joint stiffness and damping. 436 void updateArtInertiaFD(double _timeStep) const; 437 438 /// \brief Update bias force associated with the articulated body inertia. 439 /// Forward dynamics routine. 440 /// \param[in] _dt Required for implicit joint stiffness and damping. 441 /// \param[in] _gravity Vector of gravitational acceleration 442 void updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity); 443 444 /// \brief Update bias impulse associated with the articulated body inertia. 445 /// Impulse-based forward dynamics routine. 446 void updateBiasImpulseFD(); 447 448 /// \brief Update body acceleration with the partial body acceleration. 449 void updateAccelerationID() const; 450 451 /// \brief Update body acceleration. Forward dynamics routine. 452 void updateAccelerationFD(); 453 454 /// \brief Update body velocity change. Impluse-based forward dynamics 455 /// routine. 456 void updateVelocityChangeFD(); 457 458 /// \brief Update body force. Inverse dynamics routine. 459 void updateTransmittedForceID( 460 const Eigen::Vector3d& _gravity, bool _withExternalForces = false); 461 462 /// \brief Update body force. Forward dynamics routine. 463 void updateTransmittedForce(); 464 465 /// \brief Update body force. Impulse-based forward dynamics routine. 466 void updateTransmittedImpulse(); 467 468 /// \brief Update the joint force. Inverse dynamics routine. 469 void updateJointForceID( 470 double _timeStep, double _withDampingForces, double _withSpringForces); 471 472 /// \brief Update constrained terms due to the constraint impulses. Foward 473 /// dynamics routine. 474 void updateConstrainedTermsFD(double _timeStep); 475 476 /// \} 477 478 //---------------------------------------------------------------------------- 479 /// \{ \name Equations of motion related routines 480 //---------------------------------------------------------------------------- 481 482 /// 483 void updateMassMatrix(); 484 485 /// 486 void aggregateMassMatrix(Eigen::MatrixXd& _MCol, int _col); 487 488 /// 489 void aggregateAugMassMatrix( 490 Eigen::MatrixXd& _MCol, int _col, double _timeStep); 491 492 /// 493 void updateInvMassMatrix(); 494 495 /// 496 void updateInvAugMassMatrix(); 497 498 /// 499 void aggregateInvMassMatrix(Eigen::MatrixXd& _MInvCol, int _col); 500 501 /// 502 void aggregateInvAugMassMatrix( 503 Eigen::MatrixXd& _MInvCol, int _col, double _timeStep); 504 505 /// 506 void aggregateGravityForceVector( 507 Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity); 508 509 /// 510 void updateCombinedVector(); 511 512 /// 513 void aggregateCombinedVector( 514 Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity); 515 516 /// Aggregate the external forces mFext in the generalized 517 /// coordinates recursively. 518 void aggregateExternalForces(Eigen::VectorXd& _Fext); 519 520 /// \} 521 522 //-------------------- Cache Data for Mass Matrix ---------------------------- 523 /// 524 Eigen::Vector3d mM_dV; 525 526 /// 527 Eigen::Vector3d mM_F; 528 529 //----------------- Cache Data for Mass Inverse Matrix ----------------------- 530 /// 531 Eigen::Vector3d mBiasForceForInvMeta; 532 533 //---------------- Cache Data for Gravity Force Vector ----------------------- 534 /// 535 Eigen::Vector3d mG_F; 536 537 //------------------- Cache Data for Combined Vector ------------------------- 538 /// 539 Eigen::Vector3d mCg_dV; 540 541 /// 542 Eigen::Vector3d mCg_F; 543 544 protected: 545 // TODO(JS): Need? 546 /// 547 // Eigen::Matrix<std::size_t, 3, 1> mIndexInSkeleton; 548 549 /// SoftBodyNode that this PointMass belongs to 550 SoftBodyNode* mParentSoftBodyNode; 551 552 /// Index of this PointMass within the SoftBodyNode 553 std::size_t mIndex; 554 555 //---------------------------------------------------------------------------- 556 // Configuration 557 //---------------------------------------------------------------------------- 558 559 /// Derivatives w.r.t. an arbitrary scalr variable 560 Eigen::Vector3d mPositionDeriv; 561 562 //---------------------------------------------------------------------------- 563 // Velocity 564 //---------------------------------------------------------------------------- 565 566 /// Derivatives w.r.t. an arbitrary scalr variable 567 Eigen::Vector3d mVelocitiesDeriv; 568 569 //---------------------------------------------------------------------------- 570 // Acceleration 571 //---------------------------------------------------------------------------- 572 573 /// Derivatives w.r.t. an arbitrary scalr variable 574 Eigen::Vector3d mAccelerationsDeriv; 575 576 //---------------------------------------------------------------------------- 577 // Force 578 //---------------------------------------------------------------------------- 579 580 /// Derivatives w.r.t. an arbitrary scalr variable 581 Eigen::Vector3d mForcesDeriv; 582 583 //---------------------------------------------------------------------------- 584 // Impulse 585 //---------------------------------------------------------------------------- 586 587 /// Change of generalized velocity 588 Eigen::Vector3d mVelocityChanges; 589 590 // /// Generalized impulse 591 // Eigen::Vector3d mImpulse; 592 593 /// Generalized constraint impulse 594 Eigen::Vector3d mConstraintImpulses; 595 596 //---------------------------------------------------------------------------- 597 598 /// Current position viewed in world frame. 599 mutable Eigen::Vector3d mW; 600 601 /// Current position viewed in parent soft body node frame. 602 mutable Eigen::Vector3d mX; 603 604 /// Current velocity viewed in parent soft body node frame. 605 mutable Eigen::Vector3d mV; 606 607 /// Partial Acceleration of this PointMass 608 mutable Eigen::Vector3d mEta; 609 610 /// 611 Eigen::Vector3d mAlpha; 612 613 /// 614 Eigen::Vector3d mBeta; 615 616 /// Current acceleration viewed in parent body node frame. 617 mutable Eigen::Vector3d mA; 618 619 /// 620 Eigen::Vector3d mF; 621 622 /// 623 mutable double mPsi; 624 625 /// 626 mutable double mImplicitPsi; 627 628 /// 629 mutable double mPi; 630 631 /// 632 mutable double mImplicitPi; 633 634 /// Bias force 635 Eigen::Vector3d mB; 636 637 /// External force. 638 Eigen::Vector3d mFext; 639 640 /// A increasingly sorted list of dependent dof indices. 641 std::vector<std::size_t> mDependentGenCoordIndices; 642 643 /// Whether the node is currently in collision with another node. 644 bool mIsColliding; 645 646 //------------------------- Impulse-based Dyanmics --------------------------- 647 /// Velocity change due to constraint impulse 648 Eigen::Vector3d mDelV; 649 650 /// Impulsive bias force due to external impulsive force exerted on 651 /// bodies of the parent skeleton. 652 Eigen::Vector3d mImpB; 653 654 /// Cache data for mImpB 655 Eigen::Vector3d mImpAlpha; 656 657 /// Cache data for mImpB 658 Eigen::Vector3d mImpBeta; 659 660 /// Generalized impulsive body force w.r.t. body frame. 661 Eigen::Vector3d mImpF; 662 663 PointMassNotifier* mNotifier; 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(); 681 void clearPartialAccelerationNotice(); 682 void clearAccelerationNotice(); 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 697 bool mNeedPartialAccelerationUpdate; 698 // TODO(JS): Rename this to mIsPartialAccelerationDirty in DART 7 699 700 SoftBodyNode* mParentSoftBodyNode; 701 }; 702 703 } // namespace dynamics 704 } // namespace dart 705 706 #endif // DART_DYNAMICS_POINTMASS_HPP_ 707