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_SKELETON_HPP_ 34 #define DART_DYNAMICS_SKELETON_HPP_ 35 36 #include <mutex> 37 #include "dart/common/NameManager.hpp" 38 #include "dart/common/VersionCounter.hpp" 39 #include "dart/dynamics/EndEffector.hpp" 40 #include "dart/dynamics/HierarchicalIK.hpp" 41 #include "dart/dynamics/Joint.hpp" 42 #include "dart/dynamics/Marker.hpp" 43 #include "dart/dynamics/MetaSkeleton.hpp" 44 #include "dart/dynamics/ShapeNode.hpp" 45 #include "dart/dynamics/SmartPointer.hpp" 46 #include "dart/dynamics/SpecializedNodeManager.hpp" 47 #include "dart/dynamics/detail/BodyNodeAspect.hpp" 48 #include "dart/dynamics/detail/SkeletonAspect.hpp" 49 50 namespace dart { 51 namespace dynamics { 52 53 /// class Skeleton 54 DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN 55 class Skeleton : public virtual common::VersionCounter, 56 public MetaSkeleton, 57 public SkeletonSpecializedFor<ShapeNode, EndEffector, Marker>, 58 public detail::SkeletonAspectBase 59 { 60 public: 61 // Some of non-virtual functions of MetaSkeleton are hidden because of the 62 // functions of the same name in this class. We expose those functions as 63 // follows. 64 using MetaSkeleton::getAngularJacobian; 65 using MetaSkeleton::getAngularJacobianDeriv; 66 using MetaSkeleton::getJacobian; 67 using MetaSkeleton::getJacobianClassicDeriv; 68 using MetaSkeleton::getJacobianSpatialDeriv; 69 using MetaSkeleton::getLinearJacobian; 70 using MetaSkeleton::getLinearJacobianDeriv; 71 72 using AspectPropertiesData = detail::SkeletonAspectProperties; 73 using AspectProperties = common::Aspect::MakeProperties<AspectPropertiesData>; 74 75 using State = common::Composite::State; 76 using Properties = common::Composite::Properties; 77 78 enum ConfigFlags 79 { 80 CONFIG_NOTHING = 0, 81 CONFIG_POSITIONS = 1 << 1, 82 CONFIG_VELOCITIES = 1 << 2, 83 CONFIG_ACCELERATIONS = 1 << 3, 84 CONFIG_FORCES = 1 << 4, 85 CONFIG_COMMANDS = 1 << 5, 86 CONFIG_ALL = 0xFF 87 }; 88 89 /// The Configuration struct represents the joint configuration of a Skeleton. 90 /// The size of each Eigen::VectorXd member in this struct must be equal to 91 /// the number of degrees of freedom in the Skeleton or it must be zero. We 92 /// assume that any Eigen::VectorXd member with zero entries should be 93 /// ignored. 94 struct Configuration 95 { 96 Configuration( 97 const Eigen::VectorXd& positions = Eigen::VectorXd(), 98 const Eigen::VectorXd& velocities = Eigen::VectorXd(), 99 const Eigen::VectorXd& accelerations = Eigen::VectorXd(), 100 const Eigen::VectorXd& forces = Eigen::VectorXd(), 101 const Eigen::VectorXd& commands = Eigen::VectorXd()); 102 103 Configuration( 104 const std::vector<std::size_t>& indices, 105 const Eigen::VectorXd& positions = Eigen::VectorXd(), 106 const Eigen::VectorXd& velocities = Eigen::VectorXd(), 107 const Eigen::VectorXd& accelerations = Eigen::VectorXd(), 108 const Eigen::VectorXd& forces = Eigen::VectorXd(), 109 const Eigen::VectorXd& commands = Eigen::VectorXd()); 110 111 /// A list of degree of freedom indices that each entry in the 112 /// Eigen::VectorXd members correspond to. 113 std::vector<std::size_t> mIndices; 114 115 /// Joint positions 116 Eigen::VectorXd mPositions; 117 118 /// Joint velocities 119 Eigen::VectorXd mVelocities; 120 121 /// Joint accelerations 122 Eigen::VectorXd mAccelerations; 123 124 /// Joint forces 125 Eigen::VectorXd mForces; 126 127 /// Joint commands 128 Eigen::VectorXd mCommands; 129 130 /// Equality comparison operator 131 bool operator==(const Configuration& other) const; 132 133 /// Inequality comparison operator 134 bool operator!=(const Configuration& other) const; 135 }; 136 137 //---------------------------------------------------------------------------- 138 /// \{ \name Constructor and Destructor 139 //---------------------------------------------------------------------------- 140 141 /// Create a new Skeleton inside of a shared_ptr 142 static SkeletonPtr create(const std::string& _name = "Skeleton"); 143 144 /// Create a new Skeleton inside of a shared_ptr 145 static SkeletonPtr create(const AspectPropertiesData& properties); 146 147 /// Get the shared_ptr that manages this Skeleton 148 SkeletonPtr getPtr(); 149 150 /// Get the shared_ptr that manages this Skeleton 151 ConstSkeletonPtr getPtr() const; 152 153 /// Same as getPtr(), but this allows Skeleton to have a similar interface as 154 /// BodyNode and Joint for template programming. 155 SkeletonPtr getSkeleton(); 156 157 /// Same as getPtr(), but this allows Skeleton to have a similar interface as 158 /// BodyNode and Joint for template programming. 159 ConstSkeletonPtr getSkeleton() const; 160 161 /// Get the mutex that protects the state of this Skeleton 162 std::mutex& getMutex() const; 163 164 /// Get the mutex that protects the state of this Skeleton 165 std::unique_ptr<common::LockableReference> getLockableReference() 166 const override; 167 168 Skeleton(const Skeleton&) = delete; 169 170 /// Destructor 171 virtual ~Skeleton(); 172 173 /// Remove copy operator 174 Skeleton& operator=(const Skeleton& _other) = delete; 175 176 /// Create an identical clone of this Skeleton. 177 /// \deprecated Deprecated in DART 6.7. Please use cloneSkeleton() instead. 178 DART_DEPRECATED(6.7) 179 SkeletonPtr clone() const; 180 // TODO: In DART 7, change this function to override MetaSkeleton::clone() 181 // that returns MetaSkeletonPtr 182 183 /// Create an identical clone of this Skeleton, except that it has a new name. 184 /// \deprecated Deprecated in DART 6.7. Please use cloneSkeleton() instead. 185 DART_DEPRECATED(6.7) 186 SkeletonPtr clone(const std::string& cloneName) const; 187 // TODO: In DART 7, change this function to override MetaSkeleton::clone() 188 // that returns MetaSkeletonPtr 189 190 /// Creates and returns a clone of this Skeleton. 191 SkeletonPtr cloneSkeleton() const; 192 193 /// Creates and returns a clone of this Skeleton. 194 SkeletonPtr cloneSkeleton(const std::string& cloneName) const; 195 196 // Documentation inherited 197 MetaSkeletonPtr cloneMetaSkeleton( 198 const std::string& cloneName) const override; 199 200 /// \} 201 202 //---------------------------------------------------------------------------- 203 /// \{ \name Configuration 204 //---------------------------------------------------------------------------- 205 206 /// Set the configuration of this Skeleton 207 void setConfiguration(const Configuration& configuration); 208 209 /// Get the configuration of this Skeleton 210 Configuration getConfiguration(int flags = CONFIG_ALL) const; 211 212 /// Get the configuration of the specified indices in this Skeleton 213 Configuration getConfiguration( 214 const std::vector<std::size_t>& indices, int flags = CONFIG_ALL) const; 215 216 /// \} 217 218 //---------------------------------------------------------------------------- 219 /// \{ \name State 220 //---------------------------------------------------------------------------- 221 222 /// Set the State of this Skeleton [alias for setCompositeState(~)] 223 void setState(const State& state); 224 225 /// Get the State of this Skeleton [alias for getCompositeState()] 226 State getState() const; 227 228 /// \} 229 230 //---------------------------------------------------------------------------- 231 /// \{ \name Properties 232 //---------------------------------------------------------------------------- 233 234 /// Set all properties of this Skeleton 235 void setProperties(const Properties& properties); 236 237 /// Get all properties of this Skeleton 238 Properties getProperties() const; 239 240 /// Set the Properties of this Skeleton 241 void setProperties(const AspectProperties& properties); 242 243 /// Get the Properties of this Skeleton 244 DART_DEPRECATED(6.0) 245 const AspectProperties& getSkeletonProperties() const; 246 247 /// Set the AspectProperties of this Skeleton 248 void setAspectProperties(const AspectProperties& properties); 249 250 /// Set name. 251 const std::string& setName(const std::string& _name) override; 252 253 /// Get name. 254 const std::string& getName() const override; 255 256 /// Deprecated. Please use enableSelfCollisionCheck() and 257 /// setAdjacentBodyCheck() instead. 258 DART_DEPRECATED(6.0) 259 void enableSelfCollision(bool enableAdjacentBodyCheck = false); 260 261 /// Deprecated. Please use disableSelfCollisionCheck() instead. 262 DART_DEPRECATED(6.0) 263 void disableSelfCollision(); 264 265 /// Set whether to check self-collision. 266 void setSelfCollisionCheck(bool enable); 267 268 /// Return whether self-collision check is enabled. 269 bool getSelfCollisionCheck() const; 270 271 /// Enable self-collision check. 272 void enableSelfCollisionCheck(); 273 274 /// Disable self-collision check. 275 void disableSelfCollisionCheck(); 276 277 /// Return true if self-collision check is enabled 278 bool isEnabledSelfCollisionCheck() const; 279 280 /// Set whether to check adjacent bodies. This option is effective only when 281 /// the self-collision check is enabled. 282 void setAdjacentBodyCheck(bool enable); 283 284 /// Return whether adjacent body check is enabled. 285 bool getAdjacentBodyCheck() const; 286 287 /// Enable collision check for adjacent bodies. This option is effective only 288 /// when the self-collision check is enabled. 289 void enableAdjacentBodyCheck(); 290 291 /// Disable collision check for adjacent bodies. This option is effective only 292 /// when the self-collision check is enabled. 293 void disableAdjacentBodyCheck(); 294 295 /// Return true if self-collision check is enabled including adjacent bodies. 296 bool isEnabledAdjacentBodyCheck() const; 297 298 /// Set whether this skeleton will be updated by forward dynamics. 299 /// \param[in] _isMobile True if this skeleton is mobile. 300 void setMobile(bool _isMobile); 301 302 /// Get whether this skeleton will be updated by forward dynamics. 303 /// \return True if this skeleton is mobile. 304 bool isMobile() const; 305 306 /// Set time step. This timestep is used for implicit joint damping 307 /// force. 308 void setTimeStep(double _timeStep); 309 310 /// Get time step. 311 double getTimeStep() const; 312 313 /// Set 3-dim gravitational acceleration. The gravity is used for 314 /// calculating gravity force vector of the skeleton. 315 void setGravity(const Eigen::Vector3d& _gravity); 316 317 /// Get 3-dim gravitational acceleration. 318 const Eigen::Vector3d& getGravity() const; 319 320 /// \} 321 322 //---------------------------------------------------------------------------- 323 /// \{ \name Structural Properties 324 //---------------------------------------------------------------------------- 325 326 /// Create a Joint and child BodyNode pair of the given types. When creating 327 /// a root (parentless) BodyNode, pass in nullptr for the _parent argument. 328 template <class JointType, class NodeType = BodyNode> 329 std::pair<JointType*, NodeType*> createJointAndBodyNodePair( 330 BodyNode* _parent = nullptr, 331 const typename JointType::Properties& _jointProperties 332 = typename JointType::Properties(), 333 const typename NodeType::Properties& _bodyProperties 334 = typename NodeType::Properties()); 335 336 // Documentation inherited 337 std::size_t getNumBodyNodes() const override; 338 339 /// Get number of rigid body nodes. 340 std::size_t getNumRigidBodyNodes() const; 341 342 /// Get number of soft body nodes. 343 std::size_t getNumSoftBodyNodes() const; 344 345 /// Get the number of independent trees that this Skeleton contains 346 std::size_t getNumTrees() const; 347 348 /// Get the root BodyNode of the tree whose index in this Skeleton is _treeIdx 349 BodyNode* getRootBodyNode(std::size_t _treeIdx = 0); 350 351 /// Get the const root BodyNode of the tree whose index in this Skeleton is 352 /// _treeIdx 353 const BodyNode* getRootBodyNode(std::size_t _treeIdx = 0) const; 354 355 /// Get the root Joint of the tree whose index in this Skeleton is treeIdx 356 Joint* getRootJoint(std::size_t treeIdx = 0u); 357 358 /// Get the const root Joint of the tree whose index in this Skeleton is 359 /// treeIdx 360 const Joint* getRootJoint(std::size_t treeIdx = 0u) const; 361 362 // Documentation inherited 363 BodyNode* getBodyNode(std::size_t _idx) override; 364 365 // Documentation inherited 366 const BodyNode* getBodyNode(std::size_t _idx) const override; 367 368 /// Get SoftBodyNode whose index is _idx 369 SoftBodyNode* getSoftBodyNode(std::size_t _idx); 370 371 /// Get const SoftBodyNode whose index is _idx 372 const SoftBodyNode* getSoftBodyNode(std::size_t _idx) const; 373 374 // Documentation inherited 375 BodyNode* getBodyNode(const std::string& name) override; 376 377 // Documentation inherited 378 const BodyNode* getBodyNode(const std::string& name) const override; 379 380 /// Get soft body node whose name is _name 381 SoftBodyNode* getSoftBodyNode(const std::string& _name); 382 383 /// Get const soft body node whose name is _name 384 const SoftBodyNode* getSoftBodyNode(const std::string& _name) const; 385 386 // Documentation inherited 387 const std::vector<BodyNode*>& getBodyNodes() override; 388 389 // Documentation inherited 390 const std::vector<const BodyNode*>& getBodyNodes() const override; 391 392 /// \copydoc MetaSkeleton::getBodyNodes(const std::string&). 393 /// 394 /// \note Skeleton always guarantees name uniqueness for BodyNodes and Joints. 395 /// So this function returns the single BodyNode of the given name if it 396 /// exists. 397 std::vector<BodyNode*> getBodyNodes(const std::string& name) override; 398 399 /// \copydoc MetaSkeleton::getBodyNodes(const std::string&). 400 /// 401 /// \note Skeleton always guarantees name uniqueness for BodyNodes and Joints. 402 /// So this function returns the single BodyNode of the given name if it 403 /// exists. 404 std::vector<const BodyNode*> getBodyNodes( 405 const std::string& name) const override; 406 407 // Documentation inherited 408 bool hasBodyNode(const BodyNode* bodyNode) const override; 409 410 // Documentation inherited 411 std::size_t getIndexOf( 412 const BodyNode* _bn, bool _warning = true) const override; 413 414 /// Get the BodyNodes belonging to a tree in this Skeleton 415 const std::vector<BodyNode*>& getTreeBodyNodes(std::size_t _treeIdx); 416 417 /// Get the BodyNodes belonging to a tree in this Skeleton 418 std::vector<const BodyNode*> getTreeBodyNodes(std::size_t _treeIdx) const; 419 420 // Documentation inherited 421 std::size_t getNumJoints() const override; 422 423 // Documentation inherited 424 Joint* getJoint(std::size_t _idx) override; 425 426 // Documentation inherited 427 const Joint* getJoint(std::size_t _idx) const override; 428 429 // Documentation inherited 430 Joint* getJoint(const std::string& name) override; 431 432 // Documentation inherited 433 const Joint* getJoint(const std::string& name) const override; 434 435 // Documentation inherited 436 std::vector<Joint*> getJoints() override; 437 438 // Documentation inherited 439 std::vector<const Joint*> getJoints() const override; 440 441 /// \copydoc MetaSkeleton::getJoints(const std::string&). 442 /// 443 /// \note Skeleton always guarantees name uniqueness for BodyNodes and Joints. 444 /// So this function returns the single Joint of the given name if it exists. 445 std::vector<Joint*> getJoints(const std::string& name) override; 446 447 /// \copydoc MetaSkeleton::getJoints(const std::string&). 448 /// 449 /// \note Skeleton always guarantees name uniqueness for BodyNodes and Joints. 450 /// So this function returns the single Joint of the given name if it exists. 451 std::vector<const Joint*> getJoints(const std::string& name) const override; 452 453 // Documentation inherited 454 bool hasJoint(const Joint* joint) const override; 455 456 // Documentation inherited 457 std::size_t getIndexOf( 458 const Joint* _joint, bool _warning = true) const override; 459 460 // Documentation inherited 461 std::size_t getNumDofs() const override; 462 463 // Documentation inherited 464 DegreeOfFreedom* getDof(std::size_t _idx) override; 465 466 // Documentation inherited 467 const DegreeOfFreedom* getDof(std::size_t _idx) const override; 468 469 /// Get degree of freedom (aka generalized coordinate) whose name is _name 470 DegreeOfFreedom* getDof(const std::string& _name); 471 472 /// Get degree of freedom (aka generalized coordinate) whose name is _name 473 const DegreeOfFreedom* getDof(const std::string& _name) const; 474 475 // Documentation inherited 476 const std::vector<DegreeOfFreedom*>& getDofs() override; 477 478 // Documentation inherited 479 std::vector<const DegreeOfFreedom*> getDofs() const override; 480 481 // Documentation inherited 482 std::size_t getIndexOf( 483 const DegreeOfFreedom* _dof, bool _warning = true) const override; 484 485 /// Get the DegreesOfFreedom belonging to a tree in this Skeleton 486 const std::vector<DegreeOfFreedom*>& getTreeDofs(std::size_t _treeIdx); 487 488 /// Get the DegreesOfFreedom belonging to a tree in this Skeleton 489 const std::vector<const DegreeOfFreedom*>& getTreeDofs( 490 std::size_t _treeIdx) const; 491 492 /// This function is only meant for debugging purposes. It will verify that 493 /// all objects held in the Skeleton have the correct information about their 494 /// indexing. 495 bool checkIndexingConsistency() const; 496 497 /// Get a pointer to a WholeBodyIK module for this Skeleton. If _createIfNull 498 /// is true, then the IK module will be generated if one does not already 499 /// exist. 500 const std::shared_ptr<WholeBodyIK>& getIK(bool _createIfNull = false); 501 502 /// Get a pointer to a WholeBodyIK module for this Skeleton. The IK module 503 /// will be generated if one does not already exist. This function is actually 504 /// the same as getIK(true). 505 const std::shared_ptr<WholeBodyIK>& getOrCreateIK(); 506 507 /// Get a pointer to a WholeBodyIK module for this Skeleton. Because this is a 508 /// const function, a new IK module cannot be created if one does not already 509 /// exist. 510 std::shared_ptr<const WholeBodyIK> getIK() const; 511 512 /// Create a new WholeBodyIK module for this Skeleton. If an IK module already 513 /// exists in this Skeleton, it will be destroyed and replaced by a brand new 514 /// one. 515 const std::shared_ptr<WholeBodyIK>& createIK(); 516 517 /// Wipe away the WholeBodyIK module for this Skeleton, leaving it as a 518 /// nullptr 519 void clearIK(); 520 521 DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(Marker) 522 523 DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(ShapeNode) 524 525 DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(EndEffector) 526 527 /// \} 528 529 //---------------------------------------------------------------------------- 530 // Integration and finite difference 531 //---------------------------------------------------------------------------- 532 533 // Documentation inherited 534 void integratePositions(double _dt); 535 536 // Documentation inherited 537 void integrateVelocities(double _dt); 538 539 /// Return the difference of two generalized positions which are measured in 540 /// the configuration space of this Skeleton. If the configuration space is 541 /// Euclidean space, this function returns _q2 - _q1. Otherwise, it depends on 542 /// the type of the configuration space. 543 Eigen::VectorXd getPositionDifferences( 544 const Eigen::VectorXd& _q2, const Eigen::VectorXd& _q1) const; 545 546 /// Return the difference of two generalized velocities or accelerations which 547 /// are measured in the tangent space at the identity. Since the tangent 548 /// spaces are vector spaces, this function always returns _dq2 - _dq1. 549 Eigen::VectorXd getVelocityDifferences( 550 const Eigen::VectorXd& _dq2, const Eigen::VectorXd& _dq1) const; 551 552 //---------------------------------------------------------------------------- 553 /// \{ \name Support Polygon 554 //---------------------------------------------------------------------------- 555 556 /// Get the support polygon of this Skeleton, which is computed based on the 557 /// gravitational projection of the support geometries of all EndEffectors 558 /// in this Skeleton that are currently in support mode. 559 const math::SupportPolygon& getSupportPolygon() const; 560 561 /// Same as getSupportPolygon(), but it will only use EndEffectors within the 562 /// specified tree within this Skeleton 563 const math::SupportPolygon& getSupportPolygon(std::size_t _treeIdx) const; 564 565 /// Get a list of the EndEffector indices that correspond to each of the 566 /// points in the support polygon. 567 const std::vector<std::size_t>& getSupportIndices() const; 568 569 /// Same as getSupportIndices(), but it corresponds to the support polygon of 570 /// the specified tree within this Skeleton 571 const std::vector<std::size_t>& getSupportIndices(std::size_t _treeIdx) const; 572 573 /// Get the axes that correspond to each component in the support polygon. 574 /// These axes are needed in order to map the points on a support polygon 575 /// into 3D space. If gravity is along the z-direction, then these axes will 576 /// simply be <1,0,0> and <0,1,0>. 577 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& getSupportAxes() const; 578 579 /// Same as getSupportAxes(), but it corresponds to the support polygon of the 580 /// specified tree within this Skeleton 581 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& getSupportAxes( 582 std::size_t _treeIdx) const; 583 584 /// Get the centroid of the support polygon for this Skeleton. If the support 585 /// polygon is an empty set, the components of this vector will be nan. 586 const Eigen::Vector2d& getSupportCentroid() const; 587 588 /// Get the centroid of the support polygon for a tree in this Skeleton. If 589 /// the support polygon is an empty set, the components of this vector will be 590 /// nan. 591 const Eigen::Vector2d& getSupportCentroid(std::size_t _treeIdx) const; 592 593 /// The version number of a support polygon will be incremented each time the 594 /// support polygon needs to be recomputed. This number can be used to 595 /// immediately determine whether the support polygon has changed since the 596 /// last time you asked for it, allowing you to be more efficient in how you 597 /// handle the data. 598 std::size_t getSupportVersion() const; 599 600 /// Same as getSupportVersion(), but it corresponds to the support polygon of 601 /// the specified tree within this Skeleton 602 std::size_t getSupportVersion(std::size_t _treeIdx) const; 603 604 /// \} 605 606 //---------------------------------------------------------------------------- 607 // Kinematics algorithms 608 //---------------------------------------------------------------------------- 609 610 /// Compute forward kinematics 611 /// 612 /// In general, this function doesn't need to be called for forward kinematics 613 /// to update. Forward kinematics will always be computed when it's needed and 614 /// will only perform the computations that are necessary for what the user 615 /// requests. This works by performing some bookkeeping internally with dirty 616 /// flags whenever a position, velocity, or acceleration is set, either 617 /// internally or by the user. 618 /// 619 /// On one hand, this results in some overhead due to the extra effort of 620 /// bookkeeping, but on the other hand we have much greater code safety, and 621 /// in some cases performance can be dramatically improved with the auto- 622 /// updating. For example, this function is inefficient when only one portion 623 /// of the BodyNodes needed to be updated rather than the entire Skeleton, 624 /// which is common when performing inverse kinematics on a limb or on some 625 /// subsection of a Skeleton. 626 /// 627 /// This function might be useful in a case where the user wants to perform 628 /// all the forward kinematics computations during a particular time window 629 /// rather than waiting for it to be computed at the exact time that it's 630 /// needed. 631 /// 632 /// One example would be a real time controller. Let's say a controller gets 633 /// encoder data at time t0 but needs to wait until t1 before it receives the 634 /// force-torque sensor data that it needs in order to compute the output for 635 /// an operational space controller. Instead of being idle from t0 to t1, it 636 /// could use that time to compute the forward kinematics by calling this 637 /// function. 638 void computeForwardKinematics( 639 bool _updateTransforms = true, 640 bool _updateVels = true, 641 bool _updateAccs = true); 642 643 //---------------------------------------------------------------------------- 644 // Dynamics algorithms 645 //---------------------------------------------------------------------------- 646 647 /// Compute forward dynamics 648 void computeForwardDynamics(); 649 650 /// Computes inverse dynamics. 651 /// 652 /// The inverse dynamics is computed according to the following equations of 653 /// motion: 654 /// 655 /// \f$ M(q) \ddot{q} + C(q, \dot{q}) + N(q) - \tau_{\text{ext}} - \tau_d - 656 /// \tau_s = \tau \f$ 657 /// 658 /// where \f$ \tau_{\text{ext}} \f$, \f$ \tau_d \f$, and \f$ \tau_s \f$ are 659 /// external forces, joint damping forces, and joint spring forces projected 660 /// on to the joint space, respectively. This function provides three flags 661 /// whether to take into account each forces in computing the joint forces, 662 /// \f$ \tau_d \f$. Not accounting each forces implies that the forces is 663 /// added to \f$ \tau \f$ in order to keep the equation equivalent. If there 664 /// forces are zero (by setting external forces, damping coeff, spring 665 /// stiffness zeros), these flags have no effect. 666 /// 667 /// Once this function is called, the joint forces, \f$ \tau \f$, can be 668 /// obtained by calling getForces(). 669 /// 670 /// \param[in] _withExternalForces Set \c true to take external forces into 671 /// account. 672 /// \param[in] _withDampingForces Set \c true to take damping forces into 673 /// account. 674 /// \param[in] _withSpringForces Set \c true to take spring forces into 675 /// account. 676 void computeInverseDynamics( 677 bool _withExternalForces = false, 678 bool _withDampingForces = false, 679 bool _withSpringForces = false); 680 681 //---------------------------------------------------------------------------- 682 // Impulse-based dynamics algorithms 683 //---------------------------------------------------------------------------- 684 685 /// Clear constraint impulses and cache data used for impulse-based forward 686 /// dynamics algorithm, where the constraint impulses are spatial constraints 687 /// on the BodyNodes and generalized constraints on the Joints. 688 void clearConstraintImpulses(); 689 690 /// Update bias impulses 691 void updateBiasImpulse(BodyNode* _bodyNode); 692 693 /// \brief Update bias impulses due to impulse [_imp] on body node [_bodyNode] 694 /// \param _bodyNode Body node contraint impulse, _imp, is applied 695 /// \param _imp Constraint impulse expressed in body frame of _bodyNode 696 void updateBiasImpulse(BodyNode* _bodyNode, const Eigen::Vector6d& _imp); 697 698 /// \brief Update bias impulses due to impulse [_imp] on body node [_bodyNode] 699 /// \param _bodyNode1 Body node contraint impulse, _imp1, is applied 700 /// \param _imp1 Constraint impulse expressed in body frame of _bodyNode1 701 /// \param _bodyNode2 Body node contraint impulse, _imp2, is applied 702 /// \param _imp2 Constraint impulse expressed in body frame of _bodyNode2 703 void updateBiasImpulse( 704 BodyNode* _bodyNode1, 705 const Eigen::Vector6d& _imp1, 706 BodyNode* _bodyNode2, 707 const Eigen::Vector6d& _imp2); 708 709 /// \brief Update bias impulses due to impulse[_imp] on body node [_bodyNode] 710 void updateBiasImpulse( 711 SoftBodyNode* _softBodyNode, 712 PointMass* _pointMass, 713 const Eigen::Vector3d& _imp); 714 715 /// \brief Update velocity changes in body nodes and joints due to applied 716 /// impulse 717 void updateVelocityChange(); 718 719 // TODO(JS): Better naming 720 /// Set whether this skeleton is constrained. ConstraintSolver will 721 /// mark this. 722 void setImpulseApplied(bool _val); 723 724 /// Get whether this skeleton is constrained 725 bool isImpulseApplied() const; 726 727 /// Compute impulse-based forward dynamics 728 void computeImpulseForwardDynamics(); 729 730 //---------------------------------------------------------------------------- 731 /// \{ \name Jacobians 732 //---------------------------------------------------------------------------- 733 734 // Documentation inherited 735 math::Jacobian getJacobian(const JacobianNode* _node) const override; 736 737 // Documentation inherited 738 math::Jacobian getJacobian( 739 const JacobianNode* _node, const Frame* _inCoordinatesOf) const override; 740 741 // Documentation inherited 742 math::Jacobian getJacobian( 743 const JacobianNode* _node, 744 const Eigen::Vector3d& _localOffset) const override; 745 746 // Documentation inherited 747 math::Jacobian getJacobian( 748 const JacobianNode* _node, 749 const Eigen::Vector3d& _localOffset, 750 const Frame* _inCoordinatesOf) const override; 751 752 // Documentation inherited 753 math::Jacobian getWorldJacobian(const JacobianNode* _node) const override; 754 755 // Documentation inherited 756 math::Jacobian getWorldJacobian( 757 const JacobianNode* _node, 758 const Eigen::Vector3d& _localOffset) const override; 759 760 // Documentation inherited 761 math::LinearJacobian getLinearJacobian( 762 const JacobianNode* _node, 763 const Frame* _inCoordinatesOf = Frame::World()) const override; 764 765 // Documentation inherited 766 math::LinearJacobian getLinearJacobian( 767 const JacobianNode* _node, 768 const Eigen::Vector3d& _localOffset, 769 const Frame* _inCoordinatesOf = Frame::World()) const override; 770 771 // Documentation inherited 772 math::AngularJacobian getAngularJacobian( 773 const JacobianNode* _node, 774 const Frame* _inCoordinatesOf = Frame::World()) const override; 775 776 // Documentation inherited 777 math::Jacobian getJacobianSpatialDeriv( 778 const JacobianNode* _node) const override; 779 780 // Documentation inherited 781 math::Jacobian getJacobianSpatialDeriv( 782 const JacobianNode* _node, const Frame* _inCoordinatesOf) const override; 783 784 // Documentation inherited 785 math::Jacobian getJacobianSpatialDeriv( 786 const JacobianNode* _node, 787 const Eigen::Vector3d& _localOffset) const override; 788 789 // Documentation inherited 790 math::Jacobian getJacobianSpatialDeriv( 791 const JacobianNode* _node, 792 const Eigen::Vector3d& _localOffset, 793 const Frame* _inCoordinatesOf) const override; 794 795 // Documentation inherited 796 math::Jacobian getJacobianClassicDeriv( 797 const JacobianNode* _node) const override; 798 799 // Documentation inherited 800 math::Jacobian getJacobianClassicDeriv( 801 const JacobianNode* _node, const Frame* _inCoordinatesOf) const override; 802 803 // Documentation inherited 804 math::Jacobian getJacobianClassicDeriv( 805 const JacobianNode* _node, 806 const Eigen::Vector3d& _localOffset, 807 const Frame* _inCoordinatesOf = Frame::World()) const override; 808 809 // Documentation inherited 810 math::LinearJacobian getLinearJacobianDeriv( 811 const JacobianNode* _node, 812 const Frame* _inCoordinatesOf = Frame::World()) const override; 813 814 // Documentation inherited 815 math::LinearJacobian getLinearJacobianDeriv( 816 const JacobianNode* _node, 817 const Eigen::Vector3d& _localOffset, 818 const Frame* _inCoordinatesOf = Frame::World()) const override; 819 820 // Documentation inherited 821 math::AngularJacobian getAngularJacobianDeriv( 822 const JacobianNode* _node, 823 const Frame* _inCoordinatesOf = Frame::World()) const override; 824 825 /// \} 826 827 //---------------------------------------------------------------------------- 828 /// \{ \name Equations of Motion 829 //---------------------------------------------------------------------------- 830 831 /// Get total mass of the skeleton. The total mass is calculated as BodyNodes 832 /// are added and is updated as BodyNode mass is changed, so this is a 833 /// constant-time O(1) operation for the Skeleton class. 834 double getMass() const override; 835 836 /// Get the mass matrix of a specific tree in the Skeleton 837 const Eigen::MatrixXd& getMassMatrix(std::size_t _treeIdx) const; 838 839 // Documentation inherited 840 const Eigen::MatrixXd& getMassMatrix() const override; 841 842 /// Get the augmented mass matrix of a specific tree in the Skeleton 843 const Eigen::MatrixXd& getAugMassMatrix(std::size_t _treeIdx) const; 844 845 // Documentation inherited 846 const Eigen::MatrixXd& getAugMassMatrix() const override; 847 848 /// Get the inverse mass matrix of a specific tree in the Skeleton 849 const Eigen::MatrixXd& getInvMassMatrix(std::size_t _treeIdx) const; 850 851 // Documentation inherited 852 const Eigen::MatrixXd& getInvMassMatrix() const override; 853 854 /// Get the inverse augmented mass matrix of a tree 855 const Eigen::MatrixXd& getInvAugMassMatrix(std::size_t _treeIdx) const; 856 857 // Documentation inherited 858 const Eigen::MatrixXd& getInvAugMassMatrix() const override; 859 860 /// Get the Coriolis force vector of a tree in this Skeleton 861 const Eigen::VectorXd& getCoriolisForces(std::size_t _treeIdx) const; 862 863 // Documentation inherited 864 const Eigen::VectorXd& getCoriolisForces() const override; 865 866 /// Get the gravity forces for a tree in this Skeleton 867 const Eigen::VectorXd& getGravityForces(std::size_t _treeIdx) const; 868 869 // Documentation inherited 870 const Eigen::VectorXd& getGravityForces() const override; 871 872 /// Get the combined vector of Coriolis force and gravity force of a tree 873 const Eigen::VectorXd& getCoriolisAndGravityForces( 874 std::size_t _treeIdx) const; 875 876 // Documentation inherited 877 const Eigen::VectorXd& getCoriolisAndGravityForces() const override; 878 879 /// Get the external force vector of a tree in the Skeleton 880 const Eigen::VectorXd& getExternalForces(std::size_t _treeIdx) const; 881 882 // Documentation inherited 883 const Eigen::VectorXd& getExternalForces() const override; 884 885 /// Get damping force of the skeleton. 886 // const Eigen::VectorXd& getDampingForceVector(); 887 888 /// Get constraint force vector for a tree 889 const Eigen::VectorXd& getConstraintForces(std::size_t _treeIdx) const; 890 891 /// Get constraint force vector 892 const Eigen::VectorXd& getConstraintForces() const override; 893 894 // Documentation inherited 895 void clearExternalForces() override; 896 897 // Documentation inherited 898 void clearInternalForces() override; 899 900 /// Notify that the articulated inertia and everything that depends on it 901 /// needs to be updated 902 DART_DEPRECATED(6.2) 903 void notifyArticulatedInertiaUpdate(std::size_t _treeIdx); 904 905 /// Notify that the articulated inertia and everything that depends on it 906 /// needs to be updated 907 void dirtyArticulatedInertia(std::size_t _treeIdx); 908 909 /// Notify that the support polygon of a tree needs to be updated 910 DART_DEPRECATED(6.2) 911 void notifySupportUpdate(std::size_t _treeIdx); 912 913 /// Notify that the support polygon of a tree needs to be updated 914 void dirtySupportPolygon(std::size_t _treeIdx); 915 916 // Documentation inherited 917 double computeKineticEnergy() const override; 918 919 // Documentation inherited 920 double computePotentialEnergy() const override; 921 922 // Documentation inherited 923 DART_DEPRECATED(6.0) 924 void clearCollidingBodies() override; 925 926 /// \} 927 928 //---------------------------------------------------------------------------- 929 /// \{ \name Center of Mass Jacobian 930 //---------------------------------------------------------------------------- 931 932 /// Get the Skeleton's COM with respect to any Frame (default is World Frame) 933 Eigen::Vector3d getCOM( 934 const Frame* _withRespectTo = Frame::World()) const override; 935 936 /// Get the Skeleton's COM spatial velocity in terms of any Frame (default is 937 /// World Frame) 938 Eigen::Vector6d getCOMSpatialVelocity( 939 const Frame* _relativeTo = Frame::World(), 940 const Frame* _inCoordinatesOf = Frame::World()) const override; 941 942 /// Get the Skeleton's COM linear velocity in terms of any Frame (default is 943 /// World Frame) 944 Eigen::Vector3d getCOMLinearVelocity( 945 const Frame* _relativeTo = Frame::World(), 946 const Frame* _inCoordinatesOf = Frame::World()) const override; 947 948 /// Get the Skeleton's COM spatial acceleration in terms of any Frame (default 949 /// is World Frame) 950 Eigen::Vector6d getCOMSpatialAcceleration( 951 const Frame* _relativeTo = Frame::World(), 952 const Frame* _inCoordinatesOf = Frame::World()) const override; 953 954 /// Get the Skeleton's COM linear acceleration in terms of any Frame (default 955 /// is World Frame) 956 Eigen::Vector3d getCOMLinearAcceleration( 957 const Frame* _relativeTo = Frame::World(), 958 const Frame* _inCoordinatesOf = Frame::World()) const override; 959 960 /// Get the Skeleton's COM Jacobian in terms of any Frame (default is World 961 /// Frame) 962 math::Jacobian getCOMJacobian( 963 const Frame* _inCoordinatesOf = Frame::World()) const override; 964 965 /// Get the Skeleton's COM Linear Jacobian in terms of any Frame (default is 966 /// World Frame) 967 math::LinearJacobian getCOMLinearJacobian( 968 const Frame* _inCoordinatesOf = Frame::World()) const override; 969 970 /// Get the Skeleton's COM Jacobian spatial time derivative in terms of any 971 /// Frame (default is World Frame). 972 /// 973 /// NOTE: Since this is a spatial time derivative, it is only meant to be used 974 /// with spatial acceleration vectors. If you are using classical linear 975 /// vectors, then use getCOMLinearJacobianDeriv() instead. 976 math::Jacobian getCOMJacobianSpatialDeriv( 977 const Frame* _inCoordinatesOf = Frame::World()) const override; 978 979 /// Get the Skeleton's COM Linear Jacobian time derivative in terms of any 980 /// Frame (default is World Frame). 981 /// 982 /// NOTE: Since this is a classical time derivative, it is only meant to be 983 /// used with classical acceleration vectors. If you are using spatial 984 /// vectors, then use getCOMJacobianSpatialDeriv() instead. 985 math::LinearJacobian getCOMLinearJacobianDeriv( 986 const Frame* _inCoordinatesOf = Frame::World()) const override; 987 988 /// \} 989 990 //---------------------------------------------------------------------------- 991 // Friendship 992 //---------------------------------------------------------------------------- 993 friend class BodyNode; 994 friend class SoftBodyNode; 995 friend class Joint; 996 template <class> 997 friend class GenericJoint; 998 friend class DegreeOfFreedom; 999 friend class Node; 1000 friend class ShapeNode; 1001 friend class EndEffector; 1002 1003 protected: 1004 struct DataCache; 1005 1006 /// Constructor called by create() 1007 Skeleton(const AspectPropertiesData& _properties); 1008 1009 /// Setup this Skeleton with its shared_ptr 1010 void setPtr(const SkeletonPtr& _ptr); 1011 1012 /// Construct a new tree in the Skeleton 1013 void constructNewTree(); 1014 1015 /// Register a BodyNode with the Skeleton. Internal use only. 1016 void registerBodyNode(BodyNode* _newBodyNode); 1017 1018 /// Register a Joint with the Skeleton. Internal use only. 1019 void registerJoint(Joint* _newJoint); 1020 1021 /// Register a Node with the Skeleton. Internal use only. 1022 void registerNode(NodeMap& nodeMap, Node* _newNode, std::size_t& _index); 1023 1024 /// Register a Node with the Skeleton. Internal use only. 1025 void registerNode(Node* _newNode); 1026 1027 /// Remove an old tree from the Skeleton 1028 void destructOldTree(std::size_t tree); 1029 1030 /// Remove a BodyNode from the Skeleton. Internal use only. 1031 void unregisterBodyNode(BodyNode* _oldBodyNode); 1032 1033 /// Remove a Joint from the Skeleton. Internal use only. 1034 void unregisterJoint(Joint* _oldJoint); 1035 1036 /// Remove a Node from the Skeleton. Internal use only. 1037 void unregisterNode(NodeMap& nodeMap, Node* _oldNode, std::size_t& _index); 1038 1039 /// Remove a Node from the Skeleton. Internal use only. 1040 void unregisterNode(Node* _oldNode); 1041 1042 /// Move a subtree of BodyNodes from this Skeleton to another Skeleton 1043 bool moveBodyNodeTree( 1044 Joint* _parentJoint, 1045 BodyNode* _bodyNode, 1046 SkeletonPtr _newSkeleton, 1047 BodyNode* _parentNode); 1048 1049 /// Move a subtree of BodyNodes from this Skeleton to another Skeleton while 1050 /// changing the Joint type of the top parent Joint. 1051 /// 1052 /// Returns a nullptr if the move failed for any reason. 1053 template <class JointType> 1054 JointType* moveBodyNodeTree( 1055 BodyNode* _bodyNode, 1056 const SkeletonPtr& _newSkeleton, 1057 BodyNode* _parentNode, 1058 const typename JointType::Properties& _joint); 1059 1060 /// Copy a subtree of BodyNodes onto another Skeleton while leaving the 1061 /// originals intact 1062 std::pair<Joint*, BodyNode*> cloneBodyNodeTree( 1063 Joint* _parentJoint, 1064 const BodyNode* _bodyNode, 1065 const SkeletonPtr& _newSkeleton, 1066 BodyNode* _parentNode, 1067 bool _recursive) const; 1068 1069 /// Copy a subtree of BodyNodes onto another Skeleton while leaving the 1070 /// originals intact, but alter the top parent Joint to a new type 1071 template <class JointType> 1072 std::pair<JointType*, BodyNode*> cloneBodyNodeTree( 1073 const BodyNode* _bodyNode, 1074 const SkeletonPtr& _newSkeleton, 1075 BodyNode* _parentNode, 1076 const typename JointType::Properties& _joint, 1077 bool _recursive) const; 1078 1079 /// Create a vector representation of a subtree of BodyNodes 1080 std::vector<const BodyNode*> constructBodyNodeTree( 1081 const BodyNode* _bodyNode) const; 1082 1083 std::vector<BodyNode*> constructBodyNodeTree(BodyNode* _bodyNode); 1084 1085 /// Create a vector representation of a subtree of BodyNodes and remove that 1086 /// subtree from this Skeleton without deleting them 1087 std::vector<BodyNode*> extractBodyNodeTree(BodyNode* _bodyNode); 1088 1089 /// Take in and register a subtree of BodyNodes 1090 void receiveBodyNodeTree(const std::vector<BodyNode*>& _tree); 1091 1092 /// Update the computation for total mass 1093 void updateTotalMass(); 1094 1095 /// Update the dimensions for a specific cache 1096 void updateCacheDimensions(DataCache& _cache); 1097 1098 /// Update the dimensions for a tree's cache 1099 void updateCacheDimensions(std::size_t _treeIdx); 1100 1101 /// Update the articulated inertia of a tree 1102 void updateArticulatedInertia(std::size_t _tree) const; 1103 1104 /// Update the articulated inertias of the skeleton 1105 void updateArticulatedInertia() const; 1106 1107 /// Update the mass matrix of a tree 1108 void updateMassMatrix(std::size_t _treeIdx) const; 1109 1110 /// Update mass matrix of the skeleton. 1111 void updateMassMatrix() const; 1112 1113 void updateAugMassMatrix(std::size_t _treeIdx) const; 1114 1115 /// Update augmented mass matrix of the skeleton. 1116 void updateAugMassMatrix() const; 1117 1118 /// Update the inverse mass matrix of a tree 1119 void updateInvMassMatrix(std::size_t _treeIdx) const; 1120 1121 /// Update inverse of mass matrix of the skeleton. 1122 void updateInvMassMatrix() const; 1123 1124 /// Update the inverse augmented mass matrix of a tree 1125 void updateInvAugMassMatrix(std::size_t _treeIdx) const; 1126 1127 /// Update inverse of augmented mass matrix of the skeleton. 1128 void updateInvAugMassMatrix() const; 1129 1130 /// Update Coriolis force vector for a tree in the Skeleton 1131 void updateCoriolisForces(std::size_t _treeIdx) const; 1132 1133 /// Update Coriolis force vector of the skeleton. 1134 void updateCoriolisForces() const; 1135 1136 /// Update the gravity force vector of a tree 1137 void updateGravityForces(std::size_t _treeIdx) const; 1138 1139 /// Update gravity force vector of the skeleton. 1140 void updateGravityForces() const; 1141 1142 /// Update the combined vector for a tree in this Skeleton 1143 void updateCoriolisAndGravityForces(std::size_t _treeIdx) const; 1144 1145 /// Update combined vector of the skeleton. 1146 void updateCoriolisAndGravityForces() const; 1147 1148 /// Update external force vector to generalized forces for a tree 1149 void updateExternalForces(std::size_t _treeIdx) const; 1150 1151 // TODO(JS): Not implemented yet 1152 /// update external force vector to generalized forces. 1153 void updateExternalForces() const; 1154 1155 /// Compute the constraint force vector for a tree 1156 const Eigen::VectorXd& computeConstraintForces(DataCache& cache) const; 1157 1158 // /// Update damping force vector. 1159 // virtual void updateDampingForceVector(); 1160 1161 /// Add a BodyNode to the BodyNode NameManager 1162 const std::string& addEntryToBodyNodeNameMgr(BodyNode* _newNode); 1163 1164 /// Add a Joint to to the Joint NameManager 1165 const std::string& addEntryToJointNameMgr( 1166 Joint* _newJoint, bool _updateDofNames = true); 1167 1168 /// Add a SoftBodyNode to the SoftBodyNode NameManager 1169 void addEntryToSoftBodyNodeNameMgr(SoftBodyNode* _newNode); 1170 1171 void updateNameManagerNames(); 1172 1173 protected: 1174 /// The resource-managing pointer to this Skeleton 1175 std::weak_ptr<Skeleton> mPtr; 1176 1177 /// List of Soft body node list in the skeleton 1178 std::vector<SoftBodyNode*> mSoftBodyNodes; 1179 1180 /// NameManager for tracking BodyNodes 1181 dart::common::NameManager<BodyNode*> mNameMgrForBodyNodes; 1182 1183 /// NameManager for tracking Joints 1184 dart::common::NameManager<Joint*> mNameMgrForJoints; 1185 1186 /// NameManager for tracking DegreesOfFreedom 1187 dart::common::NameManager<DegreeOfFreedom*> mNameMgrForDofs; 1188 1189 /// NameManager for tracking SoftBodyNodes 1190 dart::common::NameManager<SoftBodyNode*> mNameMgrForSoftBodyNodes; 1191 1192 /// WholeBodyIK module for this Skeleton 1193 std::shared_ptr<WholeBodyIK> mWholeBodyIK; 1194 1195 struct DirtyFlags 1196 { 1197 /// Default constructor 1198 DirtyFlags(); 1199 1200 /// Dirty flag for articulated body inertia 1201 bool mArticulatedInertia; 1202 1203 /// Dirty flag for the mass matrix. 1204 bool mMassMatrix; 1205 1206 /// Dirty flag for the mass matrix. 1207 bool mAugMassMatrix; 1208 1209 /// Dirty flag for the inverse of mass matrix. 1210 bool mInvMassMatrix; 1211 1212 /// Dirty flag for the inverse of augmented mass matrix. 1213 bool mInvAugMassMatrix; 1214 1215 /// Dirty flag for the gravity force vector. 1216 bool mGravityForces; 1217 1218 /// Dirty flag for the Coriolis force vector. 1219 bool mCoriolisForces; 1220 1221 /// Dirty flag for the combined vector of Coriolis and gravity. 1222 bool mCoriolisAndGravityForces; 1223 1224 /// Dirty flag for the external force vector. 1225 bool mExternalForces; 1226 1227 /// Dirty flag for the damping force vector. 1228 bool mDampingForces; 1229 1230 /// Dirty flag for the support polygon 1231 bool mSupport; 1232 1233 /// Increments each time a new support polygon is computed to help keep 1234 /// track of changes in the support polygon 1235 std::size_t mSupportVersion; 1236 }; 1237 1238 struct DataCache 1239 { 1240 DirtyFlags mDirty; 1241 1242 /// BodyNodes belonging to this tree 1243 std::vector<BodyNode*> mBodyNodes; 1244 1245 /// Cache for const BodyNodes, for the sake of the API 1246 std::vector<const BodyNode*> mConstBodyNodes; 1247 1248 /// Degrees of Freedom belonging to this tree 1249 std::vector<DegreeOfFreedom*> mDofs; 1250 1251 /// Cache for const Degrees of Freedom, for the sake of the API 1252 std::vector<const DegreeOfFreedom*> mConstDofs; 1253 1254 /// Mass matrix cache 1255 Eigen::MatrixXd mM; 1256 1257 /// Mass matrix for the skeleton. 1258 Eigen::MatrixXd mAugM; 1259 1260 /// Inverse of mass matrix for the skeleton. 1261 Eigen::MatrixXd mInvM; 1262 1263 /// Inverse of augmented mass matrix for the skeleton. 1264 Eigen::MatrixXd mInvAugM; 1265 1266 /// Coriolis vector for the skeleton which is C(q,dq)*dq. 1267 Eigen::VectorXd mCvec; 1268 1269 /// Gravity vector for the skeleton; computed in nonrecursive 1270 /// dynamics only. 1271 Eigen::VectorXd mG; 1272 1273 /// Combined coriolis and gravity vector which is C(q, dq)*dq + g(q). 1274 Eigen::VectorXd mCg; 1275 1276 /// External force vector for the skeleton. 1277 Eigen::VectorXd mFext; 1278 1279 /// Constraint force vector. 1280 Eigen::VectorXd mFc; 1281 1282 /// Support polygon 1283 math::SupportPolygon mSupportPolygon; 1284 1285 /// A map of which EndEffectors correspond to the individual points in the 1286 /// support polygon 1287 std::vector<std::size_t> mSupportIndices; 1288 1289 /// A pair of vectors which map the 2D coordinates of the support polygon 1290 /// into 3D space 1291 std::pair<Eigen::Vector3d, Eigen::Vector3d> mSupportAxes; 1292 1293 /// Support geometry -- only used for temporary storage purposes 1294 math::SupportGeometry mSupportGeometry; 1295 1296 /// Centroid of the support polygon 1297 Eigen::Vector2d mSupportCentroid; 1298 1299 // To get byte-aligned Eigen vectors 1300 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 1301 }; 1302 1303 mutable common::aligned_vector<DataCache> mTreeCache; 1304 1305 mutable DataCache mSkelCache; 1306 1307 using SpecializedTreeNodes 1308 = std::map<std::type_index, std::vector<NodeMap::iterator>*>; 1309 1310 SpecializedTreeNodes mSpecializedTreeNodes; 1311 1312 /// Total mass. 1313 double mTotalMass; 1314 1315 // TODO(JS): Better naming 1316 /// Flag for status of impulse testing. 1317 bool mIsImpulseApplied; 1318 1319 mutable std::mutex mMutex; 1320 1321 public: 1322 //-------------------------------------------------------------------------- 1323 // Union finding 1324 //-------------------------------------------------------------------------- 1325 /// resetUnion()1326 void resetUnion() 1327 { 1328 mUnionRootSkeleton = mPtr; 1329 mUnionSize = 1; 1330 } 1331 1332 /// 1333 std::weak_ptr<Skeleton> mUnionRootSkeleton; 1334 1335 /// 1336 std::size_t mUnionSize; 1337 1338 /// 1339 std::size_t mUnionIndex; 1340 }; 1341 DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END 1342 1343 } // namespace dynamics 1344 } // namespace dart 1345 1346 #include "dart/dynamics/detail/Skeleton.hpp" 1347 1348 #endif // DART_DYNAMICS_SKELETON_HPP_ 1349