DART 6.7.3
Loading...
Searching...
No Matches
Skeleton.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_SKELETON_HPP_
34#define DART_DYNAMICS_SKELETON_HPP_
35
36#include <mutex>
49
50namespace dart {
51namespace dynamics {
52
54class Skeleton :
55 public virtual common::VersionCounter,
56 public MetaSkeleton,
57 public SkeletonSpecializedFor<ShapeNode, EndEffector, Marker>,
59{
60public:
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.
71
74
77
88
95 {
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
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
113 std::vector<std::size_t> mIndices;
114
116 Eigen::VectorXd mPositions;
117
119 Eigen::VectorXd mVelocities;
120
122 Eigen::VectorXd mAccelerations;
123
125 Eigen::VectorXd mForces;
126
128 Eigen::VectorXd mCommands;
129
131 bool operator==(const Configuration& other) const;
132
134 bool operator!=(const Configuration& other) const;
135 };
136
137 //----------------------------------------------------------------------------
139 //----------------------------------------------------------------------------
140
142 static SkeletonPtr create(const std::string& _name="Skeleton");
143
146
149
151 ConstSkeletonPtr getPtr() const;
152
156
160
162 std::mutex& getMutex() const;
163
165 std::unique_ptr<common::LockableReference> getLockableReference() const
166 override;
167
168 Skeleton(const Skeleton&) = delete;
169
171 virtual ~Skeleton();
172
174 Skeleton& operator=(const Skeleton& _other) = delete;
175
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
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
192
194 SkeletonPtr cloneSkeleton(const std::string& cloneName) const;
195
196 // Documentation inherited
198 const std::string& cloneName) const override;
199
201
202 //----------------------------------------------------------------------------
204 //----------------------------------------------------------------------------
205
207 void setConfiguration(const Configuration& configuration);
208
210 Configuration getConfiguration(int flags = CONFIG_ALL) const;
211
213 Configuration getConfiguration(const std::vector<std::size_t>& indices,
214 int flags = CONFIG_ALL) const;
215
217
218 //----------------------------------------------------------------------------
220 //----------------------------------------------------------------------------
221
223 void setState(const State& state);
224
226 State getState() const;
227
229
230 //----------------------------------------------------------------------------
232 //----------------------------------------------------------------------------
233
236
239
242
244 DART_DEPRECATED(6.0)
246
249
251 const std::string& setName(const std::string& _name) override;
252
254 const std::string& getName() const override;
255
258 DART_DEPRECATED(6.0)
260
262 DART_DEPRECATED(6.0)
264
266 void setSelfCollisionCheck(bool enable);
267
269 bool getSelfCollisionCheck() const;
270
273
276
278 bool isEnabledSelfCollisionCheck() const;
279
282 void setAdjacentBodyCheck(bool enable);
283
285 bool getAdjacentBodyCheck() const;
286
290
294
296 bool isEnabledAdjacentBodyCheck() const;
297
300 void setMobile(bool _isMobile);
301
304 bool isMobile() const;
305
308 void setTimeStep(double _timeStep);
309
311 double getTimeStep() const;
312
315 void setGravity(const Eigen::Vector3d& _gravity);
316
318 const Eigen::Vector3d& getGravity() const;
319
321
322 //----------------------------------------------------------------------------
324 //----------------------------------------------------------------------------
325
326#ifdef _WIN32
327 template <typename JointType>
328 static typename JointType::Properties createJointProperties()
329 {
330 return typename JointType::Properties();
331 }
332
333 template <typename NodeType>
334 static typename NodeType::Properties createBodyNodeProperties()
335 {
336 return typename NodeType::Properties();
337 }
338#endif
339 // TODO: Workaround for MSVC bug on template function specialization with
340 // default argument. Please see #487 for detail
341
344 template <class JointType, class NodeType = BodyNode>
345 std::pair<JointType*, NodeType*> createJointAndBodyNodePair(
346 BodyNode* _parent = nullptr,
347#ifdef _WIN32
348 const typename JointType::Properties& _jointProperties
349 = Skeleton::createJointProperties<JointType>(),
350 const typename NodeType::Properties& _bodyProperties
351 = Skeleton::createBodyNodeProperties<NodeType>());
352#else
353 const typename JointType::Properties& _jointProperties
354 = typename JointType::Properties(),
355 const typename NodeType::Properties& _bodyProperties
356 = typename NodeType::Properties());
357#endif
358 // TODO: Workaround for MSVC bug on template function specialization with
359 // default argument. Please see #487 for detail
360
361 // Documentation inherited
362 std::size_t getNumBodyNodes() const override;
363
365 std::size_t getNumRigidBodyNodes() const;
366
368 std::size_t getNumSoftBodyNodes() const;
369
371 std::size_t getNumTrees() const;
372
374 BodyNode* getRootBodyNode(std::size_t _treeIdx = 0);
375
378 const BodyNode* getRootBodyNode(std::size_t _treeIdx = 0) const;
379
381 Joint* getRootJoint(std::size_t treeIdx = 0u);
382
385 const Joint* getRootJoint(std::size_t treeIdx = 0u) const;
386
387 // Documentation inherited
388 BodyNode* getBodyNode(std::size_t _idx) override;
389
390 // Documentation inherited
391 const BodyNode* getBodyNode(std::size_t _idx) const override;
392
394 SoftBodyNode* getSoftBodyNode(std::size_t _idx);
395
397 const SoftBodyNode* getSoftBodyNode(std::size_t _idx) const;
398
399 // Documentation inherited
400 BodyNode* getBodyNode(const std::string& name) override;
401
402 // Documentation inherited
403 const BodyNode* getBodyNode(const std::string& name) const override;
404
406 SoftBodyNode* getSoftBodyNode(const std::string& _name);
407
409 const SoftBodyNode* getSoftBodyNode(const std::string& _name) const;
410
411 // Documentation inherited
412 const std::vector<BodyNode*>& getBodyNodes() override;
413
414 // Documentation inherited
415 const std::vector<const BodyNode*>& getBodyNodes() const override;
416
422 std::vector<BodyNode*> getBodyNodes(const std::string& name) override;
423
429 std::vector<const BodyNode*> getBodyNodes(
430 const std::string& name) const override;
431
432 // Documentation inherited
433 bool hasBodyNode(const BodyNode* bodyNode) const override;
434
435 // Documentation inherited
436 std::size_t getIndexOf(const BodyNode* _bn, bool _warning=true) const override;
437
439 const std::vector<BodyNode*>& getTreeBodyNodes(std::size_t _treeIdx);
440
442 std::vector<const BodyNode*> getTreeBodyNodes(std::size_t _treeIdx) const;
443
444 // Documentation inherited
445 std::size_t getNumJoints() const override;
446
447 // Documentation inherited
448 Joint* getJoint(std::size_t _idx) override;
449
450 // Documentation inherited
451 const Joint* getJoint(std::size_t _idx) const override;
452
453 // Documentation inherited
454 Joint* getJoint(const std::string& name) override;
455
456 // Documentation inherited
457 const Joint* getJoint(const std::string& name) const override;
458
459 // Documentation inherited
460 std::vector<Joint*> getJoints() override;
461
462 // Documentation inherited
463 std::vector<const Joint*> getJoints() const override;
464
469 std::vector<Joint*> getJoints(const std::string& name) override;
470
475 std::vector<const Joint*> getJoints(const std::string& name) const override;
476
477 // Documentation inherited
478 bool hasJoint(const Joint* joint) const override;
479
480 // Documentation inherited
481 std::size_t getIndexOf(const Joint* _joint, bool _warning=true) const override;
482
483 // Documentation inherited
484 std::size_t getNumDofs() const override;
485
486 // Documentation inherited
487 DegreeOfFreedom* getDof(std::size_t _idx) override;
488
489 // Documentation inherited
490 const DegreeOfFreedom* getDof(std::size_t _idx) const override;
491
493 DegreeOfFreedom* getDof(const std::string& _name);
494
496 const DegreeOfFreedom* getDof(const std::string& _name) const;
497
498 // Documentation inherited
499 const std::vector<DegreeOfFreedom*>& getDofs() override;
500
501 // Documentation inherited
502 std::vector<const DegreeOfFreedom*> getDofs() const override;
503
504 // Documentation inherited
505 std::size_t getIndexOf(const DegreeOfFreedom* _dof,
506 bool _warning=true) const override;
507
509 const std::vector<DegreeOfFreedom*>& getTreeDofs(std::size_t _treeIdx);
510
512 const std::vector<const DegreeOfFreedom*>& getTreeDofs(std::size_t _treeIdx) const;
513
517 bool checkIndexingConsistency() const;
518
522 const std::shared_ptr<WholeBodyIK>& getIK(bool _createIfNull = false);
523
527 const std::shared_ptr<WholeBodyIK>& getOrCreateIK();
528
532 std::shared_ptr<const WholeBodyIK> getIK() const;
533
537 const std::shared_ptr<WholeBodyIK>& createIK();
538
541 void clearIK();
542
544
546
548
549
550
551 //----------------------------------------------------------------------------
552 // Integration and finite difference
553 //----------------------------------------------------------------------------
554
555 // Documentation inherited
556 void integratePositions(double _dt);
557
558 // Documentation inherited
559 void integrateVelocities(double _dt);
560
565 Eigen::VectorXd getPositionDifferences(
566 const Eigen::VectorXd& _q2, const Eigen::VectorXd& _q1) const;
567
571 Eigen::VectorXd getVelocityDifferences(
572 const Eigen::VectorXd& _dq2, const Eigen::VectorXd& _dq1) const;
573
574 //----------------------------------------------------------------------------
576 //----------------------------------------------------------------------------
577
582
585 const math::SupportPolygon& getSupportPolygon(std::size_t _treeIdx) const;
586
589 const std::vector<std::size_t>& getSupportIndices() const;
590
593 const std::vector<std::size_t>& getSupportIndices(std::size_t _treeIdx) const;
594
599 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& getSupportAxes() const;
600
603 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& getSupportAxes(
604 std::size_t _treeIdx) const;
605
608 const Eigen::Vector2d& getSupportCentroid() const;
609
613 const Eigen::Vector2d& getSupportCentroid(std::size_t _treeIdx) const;
614
620 std::size_t getSupportVersion() const;
621
624 std::size_t getSupportVersion(std::size_t _treeIdx) const;
625
627
628 //----------------------------------------------------------------------------
629 // Kinematics algorithms
630 //----------------------------------------------------------------------------
631
660 void computeForwardKinematics(bool _updateTransforms = true,
661 bool _updateVels = true,
662 bool _updateAccs = true);
663
664 //----------------------------------------------------------------------------
665 // Dynamics algorithms
666 //----------------------------------------------------------------------------
667
670
672 void computeInverseDynamics(bool _withExternalForces = false,
673 bool _withDampingForces = false,
674 bool _withSpringForces = false);
675
676 //----------------------------------------------------------------------------
677 // Impulse-based dynamics algorithms
678 //----------------------------------------------------------------------------
679
684
686 void updateBiasImpulse(BodyNode* _bodyNode);
687
691 void updateBiasImpulse(BodyNode* _bodyNode, const Eigen::Vector6d& _imp);
692
698 void updateBiasImpulse(BodyNode* _bodyNode1, const Eigen::Vector6d& _imp1,
699 BodyNode* _bodyNode2, const Eigen::Vector6d& _imp2);
700
702 void updateBiasImpulse(SoftBodyNode* _softBodyNode,
703 PointMass* _pointMass,
704 const Eigen::Vector3d& _imp);
705
709
710 // TODO(JS): Better naming
713 void setImpulseApplied(bool _val);
714
716 bool isImpulseApplied() const;
717
720
721 //----------------------------------------------------------------------------
723 //----------------------------------------------------------------------------
724
725 // Documentation inherited
726 math::Jacobian getJacobian(const JacobianNode* _node) const override;
727
728 // Documentation inherited
730 const JacobianNode* _node,
731 const Frame* _inCoordinatesOf) const override;
732
733 // Documentation inherited
735 const JacobianNode* _node,
736 const Eigen::Vector3d& _localOffset) const override;
737
738 // Documentation inherited
740 const JacobianNode* _node,
741 const Eigen::Vector3d& _localOffset,
742 const Frame* _inCoordinatesOf) const override;
743
744 // Documentation inherited
746 const JacobianNode* _node) const override;
747
748 // Documentation inherited
750 const JacobianNode* _node,
751 const Eigen::Vector3d& _localOffset) const override;
752
753 // Documentation inherited
755 const JacobianNode* _node,
756 const Frame* _inCoordinatesOf = Frame::World()) const override;
757
758 // Documentation inherited
760 const JacobianNode* _node,
761 const Eigen::Vector3d& _localOffset,
762 const Frame* _inCoordinatesOf = Frame::World()) const override;
763
764 // Documentation inherited
766 const JacobianNode* _node,
767 const Frame* _inCoordinatesOf = Frame::World()) const override;
768
769 // Documentation inherited
771 const JacobianNode* _node) const override;
772
773 // Documentation inherited
775 const JacobianNode* _node,
776 const Frame* _inCoordinatesOf) const override;
777
778 // Documentation inherited
780 const JacobianNode* _node,
781 const Eigen::Vector3d& _localOffset) const override;
782
783 // Documentation inherited
785 const JacobianNode* _node,
786 const Eigen::Vector3d& _localOffset,
787 const Frame* _inCoordinatesOf) const override;
788
789 // Documentation inherited
791 const JacobianNode* _node) const override;
792
793 // Documentation inherited
795 const JacobianNode* _node,
796 const Frame* _inCoordinatesOf) const override;
797
798 // Documentation inherited
800 const JacobianNode* _node,
801 const Eigen::Vector3d& _localOffset,
802 const Frame* _inCoordinatesOf = Frame::World()) const override;
803
804 // Documentation inherited
806 const JacobianNode* _node,
807 const Frame* _inCoordinatesOf = Frame::World()) const override;
808
809 // Documentation inherited
811 const JacobianNode* _node,
812 const Eigen::Vector3d& _localOffset,
813 const Frame* _inCoordinatesOf = Frame::World()) const override;
814
815 // Documentation inherited
817 const JacobianNode* _node,
818 const Frame* _inCoordinatesOf = Frame::World()) const override;
819
821
822 //----------------------------------------------------------------------------
824 //----------------------------------------------------------------------------
825
829 double getMass() const override;
830
832 const Eigen::MatrixXd& getMassMatrix(std::size_t _treeIdx) const;
833
834 // Documentation inherited
835 const Eigen::MatrixXd& getMassMatrix() const override;
836
838 const Eigen::MatrixXd& getAugMassMatrix(std::size_t _treeIdx) const;
839
840 // Documentation inherited
841 const Eigen::MatrixXd& getAugMassMatrix() const override;
842
844 const Eigen::MatrixXd& getInvMassMatrix(std::size_t _treeIdx) const;
845
846 // Documentation inherited
847 const Eigen::MatrixXd& getInvMassMatrix() const override;
848
850 const Eigen::MatrixXd& getInvAugMassMatrix(std::size_t _treeIdx) const;
851
852 // Documentation inherited
853 const Eigen::MatrixXd& getInvAugMassMatrix() const override;
854
856 const Eigen::VectorXd& getCoriolisForces(std::size_t _treeIdx) const;
857
858 // Documentation inherited
859 const Eigen::VectorXd& getCoriolisForces() const override;
860
862 const Eigen::VectorXd& getGravityForces(std::size_t _treeIdx) const;
863
864 // Documentation inherited
865 const Eigen::VectorXd& getGravityForces() const override;
866
868 const Eigen::VectorXd& getCoriolisAndGravityForces(std::size_t _treeIdx) const;
869
870 // Documentation inherited
871 const Eigen::VectorXd& getCoriolisAndGravityForces() const override;
872
874 const Eigen::VectorXd& getExternalForces(std::size_t _treeIdx) const;
875
876 // Documentation inherited
877 const Eigen::VectorXd& getExternalForces() const override;
878
880// const Eigen::VectorXd& getDampingForceVector();
881
883 const Eigen::VectorXd& getConstraintForces(std::size_t _treeIdx) const;
884
886 const Eigen::VectorXd& getConstraintForces() const override;
887
888 // Documentation inherited
889 void clearExternalForces() override;
890
891 // Documentation inherited
892 void clearInternalForces() override;
893
896 DART_DEPRECATED(6.2)
897 void notifyArticulatedInertiaUpdate(std::size_t _treeIdx);
898
901 void dirtyArticulatedInertia(std::size_t _treeIdx);
902
904 DART_DEPRECATED(6.2)
905 void notifySupportUpdate(std::size_t _treeIdx);
906
908 void dirtySupportPolygon(std::size_t _treeIdx);
909
910 // Documentation inherited
911 double computeKineticEnergy() const override;
912
913 // Documentation inherited
914 double computePotentialEnergy() const override;
915
916 // Documentation inherited
917 DART_DEPRECATED(6.0)
918 void clearCollidingBodies() override;
919
921
922 //----------------------------------------------------------------------------
924 //----------------------------------------------------------------------------
925
927 Eigen::Vector3d getCOM(
928 const Frame* _withRespectTo = Frame::World()) const override;
929
933 const Frame* _relativeTo = Frame::World(),
934 const Frame* _inCoordinatesOf = Frame::World()) const override;
935
938 Eigen::Vector3d getCOMLinearVelocity(
939 const Frame* _relativeTo = Frame::World(),
940 const Frame* _inCoordinatesOf = Frame::World()) const override;
941
945 const Frame* _relativeTo = Frame::World(),
946 const Frame* _inCoordinatesOf = Frame::World()) const override;
947
951 const Frame* _relativeTo = Frame::World(),
952 const Frame* _inCoordinatesOf = Frame::World()) const override;
953
956 math::Jacobian getCOMJacobian(
957 const Frame* _inCoordinatesOf = Frame::World()) const override;
958
961 math::LinearJacobian getCOMLinearJacobian(
962 const Frame* _inCoordinatesOf = Frame::World()) const override;
963
970 math::Jacobian getCOMJacobianSpatialDeriv(
971 const Frame* _inCoordinatesOf = Frame::World()) const override;
972
979 math::LinearJacobian getCOMLinearJacobianDeriv(
980 const Frame* _inCoordinatesOf = Frame::World()) const override;
981
983
984 //----------------------------------------------------------------------------
985 // Friendship
986 //----------------------------------------------------------------------------
987 friend class BodyNode;
988 friend class SoftBodyNode;
989 friend class Joint;
990 template<class> friend class GenericJoint;
991 friend class DegreeOfFreedom;
992 friend class Node;
993 friend class ShapeNode;
994 friend class EndEffector;
995
996protected:
997 struct DataCache;
998
1000 Skeleton(const AspectPropertiesData& _properties);
1001
1003 void setPtr(const SkeletonPtr& _ptr);
1004
1006 void constructNewTree();
1007
1009 void registerBodyNode(BodyNode* _newBodyNode);
1010
1012 void registerJoint(Joint* _newJoint);
1013
1015 void registerNode(NodeMap& nodeMap, Node* _newNode, std::size_t& _index);
1016
1018 void registerNode(Node* _newNode);
1019
1021 void destructOldTree(std::size_t tree);
1022
1024 void unregisterBodyNode(BodyNode* _oldBodyNode);
1025
1027 void unregisterJoint(Joint* _oldJoint);
1028
1030 void unregisterNode(NodeMap& nodeMap, Node* _oldNode, std::size_t& _index);
1031
1033 void unregisterNode(Node* _oldNode);
1034
1036 bool moveBodyNodeTree(Joint* _parentJoint, BodyNode* _bodyNode,
1037 SkeletonPtr _newSkeleton,
1038 BodyNode* _parentNode);
1039
1044 template <class JointType>
1045 JointType* moveBodyNodeTree(
1046 BodyNode* _bodyNode,
1047 const SkeletonPtr& _newSkeleton,
1048 BodyNode* _parentNode,
1049 const typename JointType::Properties& _joint);
1050
1054 Joint* _parentJoint,
1055 const BodyNode* _bodyNode,
1056 const SkeletonPtr& _newSkeleton,
1057 BodyNode* _parentNode,
1058 bool _recursive) const;
1059
1062 template <class JointType>
1063 std::pair<JointType*, BodyNode*> cloneBodyNodeTree(
1064 const BodyNode* _bodyNode,
1065 const SkeletonPtr& _newSkeleton,
1066 BodyNode* _parentNode,
1067 const typename JointType::Properties& _joint,
1068 bool _recursive) const;
1069
1071 std::vector<const BodyNode*> constructBodyNodeTree(
1072 const BodyNode* _bodyNode) const;
1073
1074 std::vector<BodyNode*> constructBodyNodeTree(BodyNode* _bodyNode);
1075
1078 std::vector<BodyNode*> extractBodyNodeTree(BodyNode* _bodyNode);
1079
1081 void receiveBodyNodeTree(const std::vector<BodyNode*>& _tree);
1082
1084 void updateTotalMass();
1085
1087 void updateCacheDimensions(DataCache& _cache);
1088
1090 void updateCacheDimensions(std::size_t _treeIdx);
1091
1093 void updateArticulatedInertia(std::size_t _tree) const;
1094
1096 void updateArticulatedInertia() const;
1097
1099 void updateMassMatrix(std::size_t _treeIdx) const;
1100
1102 void updateMassMatrix() const;
1103
1104 void updateAugMassMatrix(std::size_t _treeIdx) const;
1105
1107 void updateAugMassMatrix() const;
1108
1110 void updateInvMassMatrix(std::size_t _treeIdx) const;
1111
1113 void updateInvMassMatrix() const;
1114
1116 void updateInvAugMassMatrix(std::size_t _treeIdx) const;
1117
1119 void updateInvAugMassMatrix() const;
1120
1122 void updateCoriolisForces(std::size_t _treeIdx) const;
1123
1125 void updateCoriolisForces() const;
1126
1128 void updateGravityForces(std::size_t _treeIdx) const;
1129
1131 void updateGravityForces() const;
1132
1134 void updateCoriolisAndGravityForces(std::size_t _treeIdx) const;
1135
1137 void updateCoriolisAndGravityForces() const;
1138
1140 void updateExternalForces(std::size_t _treeIdx) const;
1141
1142 // TODO(JS): Not implemented yet
1144 void updateExternalForces() const;
1145
1147 const Eigen::VectorXd& computeConstraintForces(DataCache& cache) const;
1148
1149// /// Update damping force vector.
1150// virtual void updateDampingForceVector();
1151
1153 const std::string& addEntryToBodyNodeNameMgr(BodyNode* _newNode);
1154
1156 const std::string& addEntryToJointNameMgr(Joint* _newJoint, bool _updateDofNames=true);
1157
1160
1161protected:
1162
1164 std::weak_ptr<Skeleton> mPtr;
1165
1168
1170 dart::common::NameManager<BodyNode*> mNameMgrForBodyNodes;
1171
1173 dart::common::NameManager<Joint*> mNameMgrForJoints;
1174
1176 dart::common::NameManager<DegreeOfFreedom*> mNameMgrForDofs;
1177
1180
1183
1185 {
1187 DirtyFlags();
1188
1191
1194
1197
1200
1203
1206
1209
1212
1215
1218
1221
1224 std::size_t mSupportVersion;
1225 };
1226
1228 {
1230
1232 std::vector<BodyNode*> mBodyNodes;
1233
1235 std::vector<const BodyNode*> mConstBodyNodes;
1236
1238 std::vector<DegreeOfFreedom*> mDofs;
1239
1241 std::vector<const DegreeOfFreedom*> mConstDofs;
1242
1244 Eigen::MatrixXd mM;
1245
1247 Eigen::MatrixXd mAugM;
1248
1250 Eigen::MatrixXd mInvM;
1251
1253 Eigen::MatrixXd mInvAugM;
1254
1256 Eigen::VectorXd mCvec;
1257
1260 Eigen::VectorXd mG;
1261
1263 Eigen::VectorXd mCg;
1264
1266 Eigen::VectorXd mFext;
1267
1269 Eigen::VectorXd mFc;
1270
1273
1276 std::vector<std::size_t> mSupportIndices;
1277
1280 std::pair<Eigen::Vector3d, Eigen::Vector3d> mSupportAxes;
1281
1284
1286 Eigen::Vector2d mSupportCentroid;
1287
1288 // To get byte-aligned Eigen vectors
1289 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1290 };
1291
1293
1295
1296 using SpecializedTreeNodes = std::map<std::type_index, std::vector<NodeMap::iterator>*>;
1297
1299
1302
1303 // TODO(JS): Better naming
1306
1307 mutable std::mutex mMutex;
1308
1309public:
1310 //--------------------------------------------------------------------------
1311 // Union finding
1312 //--------------------------------------------------------------------------
1315 {
1317 mUnionSize = 1;
1318 }
1319
1321 std::weak_ptr<Skeleton> mUnionRootSkeleton;
1322
1324 std::size_t mUnionSize;
1325
1327 std::size_t mUnionIndex;
1328};
1329
1330} // namespace dynamics
1331} // namespace dart
1332
1334
1335#endif // DART_DYNAMICS_SKELETON_HPP_
#define DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(AspectName)
Definition BasicNodeManager.hpp:320
#define DART_DEPRECATED(version)
Definition Deprecated.hpp:51
BodyPropPtr properties
Definition SdfParser.cpp:80
std::string * name
Definition SkelParser.cpp:1642
detail::CompositeState State
Definition Composite.hpp:55
detail::CompositeProperties Properties
Definition Composite.hpp:56
This is an alternative to EmbedProperties which allows your class to also inherit other Composite obj...
Definition EmbeddedAspect.hpp:248
The MakeCloneable class is used to easily create an Cloneable (such as Node::State) which simply take...
Definition Cloneable.hpp:85
VersionCounter is an interface for objects that count their versions.
Definition VersionCounter.hpp:43
Definition CompositeData.hpp:104
BodyNode class represents a single node of the skeleton.
Definition BodyNode.hpp:78
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition DegreeOfFreedom.hpp:53
Definition EndEffector.hpp:84
static Frame * World()
Definition Frame.cpp:72
Definition GenericJoint.hpp:49
class Joint
Definition Joint.hpp:59
MetaSkeleton is a pure abstract base class that provides a common interface for obtaining data (such ...
Definition MetaSkeleton.hpp:62
virtual math::LinearJacobian getLinearJacobianDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const =0
of a BodyNode.
virtual math::LinearJacobian getLinearJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the linear Jacobian targeting the origin of a BodyNode.
virtual math::AngularJacobian getAngularJacobianDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the angular Jacobian time derivative of a BodyNode.
virtual math::Jacobian getJacobianClassicDeriv(const JacobianNode *_node) const =0
Get the spatial Jacobian (classical) time derivative targeting the origin of a BodyNode.
virtual math::Jacobian getJacobian(const JacobianNode *_node) const =0
Get the spatial Jacobian targeting the origin of a BodyNode.
MetaSkeletonPtr cloneMetaSkeleton() const
Creates an identical clone of this MetaSkeleton.
Definition MetaSkeleton.cpp:318
virtual math::AngularJacobian getAngularJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the angular Jacobian of a BodyNode.
virtual math::Jacobian getJacobianSpatialDeriv(const JacobianNode *_node) const =0
Get the spatial Jacobian time derivative targeting the origin of a BodyNode.
The Node class is a base class for BodyNode and any object that attaches to a BodyNode.
Definition Node.hpp:84
Definition ShapeNode.hpp:49
Declaration of the variadic template.
Definition SpecializedNodeManager.hpp:120
class Skeleton
Definition Skeleton.hpp:59
Skeleton(const Skeleton &)=delete
void computeForwardDynamics()
Compute forward dynamics.
Definition Skeleton.cpp:3595
const std::vector< BodyNode * > & getBodyNodes() override
Get all the BodyNodes that are held by this MetaSkeleton.
Definition Skeleton.cpp:927
void updateInvAugMassMatrix() const
Update inverse of augmented mass matrix of the skeleton.
Definition Skeleton.cpp:3094
SoftBodyNode * getSoftBodyNode(std::size_t _idx)
Get SoftBodyNode whose index is _idx.
Definition Skeleton.cpp:878
State getState() const
Get the State of this Skeleton [alias for getCompositeState()].
Definition Skeleton.cpp:571
dart::common::NameManager< BodyNode * > mNameMgrForBodyNodes
NameManager for tracking BodyNodes.
Definition Skeleton.hpp:1170
const Eigen::MatrixXd & getMassMatrix() const override
Get the Mass Matrix of the MetaSkeleton.
Definition Skeleton.cpp:1921
SpecializedTreeNodes mSpecializedTreeNodes
Definition Skeleton.hpp:1298
dart::common::NameManager< SoftBodyNode * > mNameMgrForSoftBodyNodes
NameManager for tracking SoftBodyNodes.
Definition Skeleton.hpp:1179
void registerJoint(Joint *_newJoint)
Register a Joint with the Skeleton. Internal use only.
Definition Skeleton.cpp:2215
ConfigFlags
Definition Skeleton.hpp:79
@ CONFIG_FORCES
Definition Skeleton.hpp:84
@ CONFIG_VELOCITIES
Definition Skeleton.hpp:82
@ CONFIG_COMMANDS
Definition Skeleton.hpp:85
@ CONFIG_NOTHING
Definition Skeleton.hpp:80
@ CONFIG_ALL
Definition Skeleton.hpp:86
@ CONFIG_POSITIONS
Definition Skeleton.hpp:81
@ CONFIG_ACCELERATIONS
Definition Skeleton.hpp:83
void updateCoriolisAndGravityForces() const
Update combined vector of the skeleton.
Definition Skeleton.cpp:3265
double getMass() const override
Get total mass of the skeleton.
Definition Skeleton.cpp:1907
DegreeOfFreedom * getDof(std::size_t _idx) override
Get degree of freedom (aka generalized coordinate) whose index is _idx.
Definition Skeleton.cpp:1141
std::pair< JointType *, NodeType * > createJointAndBodyNodePair(BodyNode *_parent=nullptr, const typename JointType::Properties &_jointProperties=typename JointType::Properties(), const typename NodeType::Properties &_bodyProperties=typename NodeType::Properties())
Create a Joint and child BodyNode pair of the given types.
Definition Skeleton.hpp:77
void disableSelfCollisionCheck()
Disable self-collision check.
Definition Skeleton.cpp:709
bool isEnabledAdjacentBodyCheck() const
Return true if self-collision check is enabled including adjacent bodies.
Definition Skeleton.cpp:745
void setProperties(const Properties &properties)
Set all properties of this Skeleton.
Definition Skeleton.cpp:577
std::shared_ptr< const WholeBodyIK > getIK() const
Get a pointer to a WholeBodyIK module for this Skeleton.
Definition Skeleton.cpp:1464
void dirtyArticulatedInertia(std::size_t _treeIdx)
Notify that the articulated inertia and everything that depends on it needs to be updated.
Definition Skeleton.cpp:3654
std::size_t getIndexOf(const BodyNode *_bn, bool _warning=true) const override
Get the index of a specific BodyNode within this ReferentialSkeleton.
Definition Skeleton.cpp:1002
bool isMobile() const
Get whether this skeleton will be updated by forward dynamics.
Definition Skeleton.cpp:757
void updateCoriolisForces() const
Update Coriolis force vector of the skeleton.
Definition Skeleton.cpp:3157
void clearExternalForces() override
Clear the external forces of the BodyNodes in this MetaSkeleton.
Definition Skeleton.cpp:3634
void setImpulseApplied(bool _val)
Set whether this skeleton is constrained.
Definition Skeleton.cpp:3847
SkeletonPtr getSkeleton()
Same as getPtr(), but this allows Skeleton to have a similar interface as BodyNode and Joint for temp...
Definition Skeleton.cpp:367
bool getSelfCollisionCheck() const
Return whether self-collision check is enabled.
Definition Skeleton.cpp:697
void updateArticulatedInertia() const
Update the articulated inertias of the skeleton.
Definition Skeleton.cpp:2732
void updateAugMassMatrix() const
Update augmented mass matrix of the skeleton.
Definition Skeleton.cpp:2901
void notifyArticulatedInertiaUpdate(std::size_t _treeIdx)
Notify that the articulated inertia and everything that depends on it needs to be updated.
Definition Skeleton.cpp:3648
std::pair< Joint *, BodyNode * > cloneBodyNodeTree(Joint *_parentJoint, const BodyNode *_bodyNode, const SkeletonPtr &_newSkeleton, BodyNode *_parentNode, bool _recursive) const
Copy a subtree of BodyNodes onto another Skeleton while leaving the originals intact.
Definition Skeleton.cpp:2590
std::weak_ptr< Skeleton > mPtr
The resource-managing pointer to this Skeleton.
Definition Skeleton.hpp:1164
friend class Joint
Definition Skeleton.hpp:989
void constructNewTree()
Construct a new tree in the Skeleton.
Definition Skeleton.cpp:2092
math::AngularJacobian getAngularJacobianDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
Get the angular Jacobian time derivative of a BodyNode.
Definition Skeleton.cpp:1900
std::size_t getNumBodyNodes() const override
Get number of body nodes.
Definition Skeleton.cpp:794
std::size_t getNumJoints() const override
Get number of Joints.
Definition Skeleton.cpp:1032
void setMobile(bool _isMobile)
Set whether this skeleton will be updated by forward dynamics.
Definition Skeleton.cpp:751
void unregisterJoint(Joint *_oldJoint)
Remove a Joint from the Skeleton. Internal use only.
Definition Skeleton.cpp:2379
void unregisterBodyNode(BodyNode *_oldBodyNode)
Remove a BodyNode from the Skeleton. Internal use only.
Definition Skeleton.cpp:2314
void registerNode(NodeMap &nodeMap, Node *_newNode, std::size_t &_index)
Register a Node with the Skeleton. Internal use only.
Definition Skeleton.cpp:2242
math::LinearJacobian getCOMLinearJacobianDeriv(const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM Linear Jacobian time derivative in terms of any Frame (default is World Frame)...
Definition Skeleton.cpp:4069
const Eigen::Vector2d & getSupportCentroid() const
Get the centroid of the support polygon for this Skeleton.
Definition Skeleton.cpp:3529
bool hasBodyNode(const BodyNode *bodyNode) const override
Returns whether this Skeleton contains bodyNode.
Definition Skeleton.cpp:963
double mTotalMass
Total mass.
Definition Skeleton.hpp:1301
bool getAdjacentBodyCheck() const
Return whether adjacent body check is enabled.
Definition Skeleton.cpp:727
void computeInverseDynamics(bool _withExternalForces=false, bool _withDampingForces=false, bool _withSpringForces=false)
Compute inverse dynamics.
Definition Skeleton.cpp:3614
void updateMassMatrix() const
Update mass matrix of the skeleton.
Definition Skeleton.cpp:2806
static SkeletonPtr create(const std::string &_name="Skeleton")
Create a new Skeleton inside of a shared_ptr.
Definition Skeleton.cpp:341
const Eigen::MatrixXd & getAugMassMatrix() const override
Get augmented mass matrix of the skeleton.
Definition Skeleton.cpp:1938
void setSelfCollisionCheck(bool enable)
Set whether to check self-collision.
Definition Skeleton.cpp:691
double computeKineticEnergy() const override
Get the kinetic energy of this MetaSkeleton.
Definition Skeleton.cpp:3885
void updateInvMassMatrix() const
Update inverse of mass matrix of the skeleton.
Definition Skeleton.cpp:2997
void setTimeStep(double _timeStep)
Set time step.
Definition Skeleton.cpp:763
BodyNode * getBodyNode(std::size_t _idx) override
Get BodyNode whose index is _idx.
Definition Skeleton.cpp:864
math::Jacobian getWorldJacobian(const JacobianNode *_node) const override
Get the spatial Jacobian targeting the origin of a BodyNode.
Definition Skeleton.cpp:1674
void clearCollidingBodies() override
Clear collision flags of the BodyNodes in this MetaSkeleton.
Definition Skeleton.cpp:3911
void enableSelfCollision(bool enableAdjacentBodyCheck=false)
Deprecated.
Definition Skeleton.cpp:677
bool hasJoint(const Joint *joint) const override
Returns whether this Skeleton contains join.
Definition Skeleton.cpp:1116
const std::string & getName() const override
Get name.
Definition Skeleton.cpp:640
void setGravity(const Eigen::Vector3d &_gravity)
Set 3-dim gravitational acceleration.
Definition Skeleton.cpp:779
void updateExternalForces() const
update external force vector to generalized forces.
Definition Skeleton.cpp:3347
const Eigen::Vector3d & getGravity() const
Get 3-dim gravitational acceleration.
Definition Skeleton.cpp:788
const Eigen::VectorXd & getConstraintForces() const override
Get constraint force vector.
Definition Skeleton.cpp:2061
Eigen::VectorXd getVelocityDifferences(const Eigen::VectorXd &_dq2, const Eigen::VectorXd &_dq1) const
Return the difference of two generalized velocities or accelerations which are measured in the tangen...
Definition Skeleton.cpp:1550
std::size_t getNumSoftBodyNodes() const
Get number of soft body nodes.
Definition Skeleton.cpp:806
math::Jacobian getCOMJacobianSpatialDeriv(const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM Jacobian spatial time derivative in terms of any Frame (default is World Frame...
Definition Skeleton.cpp:4060
void setPtr(const SkeletonPtr &_ptr)
Setup this Skeleton with its shared_ptr.
Definition Skeleton.cpp:2085
const std::string & setName(const std::string &_name) override
Set name.
Definition Skeleton.cpp:612
math::LinearJacobian getLinearJacobianDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
of a BodyNode.
Definition Skeleton.cpp:1863
std::mutex mMutex
Definition Skeleton.hpp:1307
Configuration getConfiguration(int flags=CONFIG_ALL) const
Get the configuration of this Skeleton.
Definition Skeleton.cpp:529
Eigen::Vector3d getCOMLinearVelocity(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM linear velocity in terms of any Frame (default is World Frame)
Definition Skeleton.cpp:3980
std::vector< BodyNode * > extractBodyNodeTree(BodyNode *_bodyNode)
Create a vector representation of a subtree of BodyNodes and remove that subtree from this Skeleton w...
Definition Skeleton.cpp:2663
std::vector< SoftBodyNode * > mSoftBodyNodes
List of Soft body node list in the skeleton.
Definition Skeleton.hpp:1167
math::LinearJacobian getLinearJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
Get the linear Jacobian targeting the origin of a BodyNode.
Definition Skeleton.cpp:1707
friend class EndEffector
Definition Skeleton.hpp:994
virtual ~Skeleton()
Destructor.
Definition Skeleton.cpp:392
friend class DegreeOfFreedom
Definition Skeleton.hpp:991
math::AngularJacobian getAngularJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
Get the angular Jacobian of a BodyNode.
Definition Skeleton.cpp:1744
void integrateVelocities(double _dt)
Definition Skeleton.cpp:1505
math::Jacobian getJacobianSpatialDeriv(const JacobianNode *_node) const override
Get the spatial Jacobian time derivative targeting the origin of a BodyNode.
Definition Skeleton.cpp:1769
SkeletonPtr cloneSkeleton() const
Creates and returns a clone of this Skeleton.
Definition Skeleton.cpp:411
void unregisterNode(NodeMap &nodeMap, Node *_oldNode, std::size_t &_index)
Remove a Node from the Skeleton. Internal use only.
Definition Skeleton.cpp:2426
const std::vector< DegreeOfFreedom * > & getTreeDofs(std::size_t _treeIdx)
Get the DegreesOfFreedom belonging to a tree in this Skeleton.
Definition Skeleton.cpp:1188
SkeletonPtr getPtr()
Get the shared_ptr that manages this Skeleton.
Definition Skeleton.cpp:355
void computeForwardKinematics(bool _updateTransforms=true, bool _updateVels=true, bool _updateAccs=true)
Compute forward kinematics.
Definition Skeleton.cpp:3561
Properties getProperties() const
Get all properties of this Skeleton.
Definition Skeleton.cpp:583
const std::shared_ptr< WholeBodyIK > & getOrCreateIK()
Get a pointer to a WholeBodyIK module for this Skeleton.
Definition Skeleton.cpp:1458
const Eigen::VectorXd & getCoriolisForces() const override
Get Coriolis force vector of the MetaSkeleton's BodyNodes.
Definition Skeleton.cpp:1992
void updateBiasImpulse(BodyNode *_bodyNode)
Update bias impulses.
Definition Skeleton.cpp:3686
void setAspectProperties(const AspectProperties &properties)
Set the AspectProperties of this Skeleton.
Definition Skeleton.cpp:595
void disableSelfCollision()
Deprecated. Please use disableSelfCollisionCheck() instead.
Definition Skeleton.cpp:684
const Eigen::MatrixXd & getInvAugMassMatrix() const override
Get inverse of augmented Mass Matrix of the MetaSkeleton.
Definition Skeleton.cpp:1974
void resetUnion()
Definition Skeleton.hpp:1314
void enableSelfCollisionCheck()
Enable self-collision check.
Definition Skeleton.cpp:703
BodyNode * getRootBodyNode(std::size_t _treeIdx=0)
Get the root BodyNode of the tree whose index in this Skeleton is _treeIdx.
Definition Skeleton.cpp:818
friend class ShapeNode
Definition Skeleton.hpp:993
Eigen::Vector6d getCOMSpatialAcceleration(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM spatial acceleration in terms of any Frame (default is World Frame)
Definition Skeleton.cpp:3988
std::size_t getNumRigidBodyNodes() const
Get number of rigid body nodes.
Definition Skeleton.cpp:800
bool mIsImpulseApplied
Flag for status of impulse testing.
Definition Skeleton.hpp:1305
void setAdjacentBodyCheck(bool enable)
Set whether to check adjacent bodies.
Definition Skeleton.cpp:721
void dirtySupportPolygon(std::size_t _treeIdx)
Notify that the support polygon of a tree needs to be updated.
Definition Skeleton.cpp:3673
std::shared_ptr< WholeBodyIK > mWholeBodyIK
WholeBodyIK module for this Skeleton.
Definition Skeleton.hpp:1182
Eigen::Vector3d getCOM(const Frame *_withRespectTo=Frame::World()) const override
Get the Skeleton's COM with respect to any Frame (default is World Frame)
Definition Skeleton.cpp:3932
void updateGravityForces() const
Update gravity force vector of the skeleton.
Definition Skeleton.cpp:3208
std::unique_ptr< common::LockableReference > getLockableReference() const override
Get the mutex that protects the state of this Skeleton.
Definition Skeleton.cpp:385
const math::SupportPolygon & getSupportPolygon() const
Get the support polygon of this Skeleton, which is computed based on the gravitational projection of ...
Definition Skeleton.cpp:3461
friend class SoftBodyNode
Definition Skeleton.hpp:988
void computeImpulseForwardDynamics()
Compute impulse-based forward dynamics.
Definition Skeleton.cpp:3859
void updateVelocityChange()
Update velocity changes in body nodes and joints due to applied impulse.
Definition Skeleton.cpp:3840
Eigen::Vector6d getCOMSpatialVelocity(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM spatial velocity in terms of any Frame (default is World Frame)
Definition Skeleton.cpp:3972
double getTimeStep() const
Get time step.
Definition Skeleton.cpp:773
Joint * getJoint(std::size_t _idx) override
Get Joint whose index is _idx.
Definition Skeleton.cpp:1039
std::map< std::type_index, std::vector< NodeMap::iterator > * > SpecializedTreeNodes
Definition Skeleton.hpp:1296
void clearConstraintImpulses()
Clear constraint impulses and cache data used for impulse-based forward dynamics algorithm,...
Definition Skeleton.cpp:3679
bool isEnabledSelfCollisionCheck() const
Return true if self-collision check is enabled.
Definition Skeleton.cpp:715
math::LinearJacobian getCOMLinearJacobian(const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM Linear Jacobian in terms of any Frame (default is World Frame)
Definition Skeleton.cpp:4051
void disableAdjacentBodyCheck()
Disable collision check for adjacent bodies.
Definition Skeleton.cpp:739
const std::pair< Eigen::Vector3d, Eigen::Vector3d > & getSupportAxes() const
Get the axes that correspond to each component in the support polygon.
Definition Skeleton.cpp:3514
const std::vector< std::size_t > & getSupportIndices() const
Get a list of the EndEffector indices that correspond to each of the points in the support polygon.
Definition Skeleton.cpp:3499
void enableAdjacentBodyCheck()
Enable collision check for adjacent bodies.
Definition Skeleton.cpp:733
void receiveBodyNodeTree(const std::vector< BodyNode * > &_tree)
Take in and register a subtree of BodyNodes.
Definition Skeleton.cpp:2680
void notifySupportUpdate(std::size_t _treeIdx)
Notify that the support polygon of a tree needs to be updated.
Definition Skeleton.cpp:3667
bool checkIndexingConsistency() const
This function is only meant for debugging purposes.
Definition Skeleton.cpp:1202
const Eigen::VectorXd & getGravityForces() const override
Get gravity force vector of the MetaSkeleton.
Definition Skeleton.cpp:2010
const Eigen::VectorXd & getExternalForces() const override
Get external force vector of the MetaSkeleton.
Definition Skeleton.cpp:2046
friend class BodyNode
Definition Skeleton.hpp:987
std::vector< Joint * > getJoints() override
Returns all the joints that are held by this MetaSkeleton.
Definition Skeleton.cpp:1068
Eigen::VectorXd getPositionDifferences(const Eigen::VectorXd &_q2, const Eigen::VectorXd &_q1) const
Return the difference of two generalized positions which are measured in the configuration space of t...
Definition Skeleton.cpp:1518
void destructOldTree(std::size_t tree)
Remove an old tree from the Skeleton.
Definition Skeleton.cpp:2291
std::vector< const BodyNode * > constructBodyNodeTree(const BodyNode *_bodyNode) const
Create a vector representation of a subtree of BodyNodes.
Definition Skeleton.cpp:2644
std::size_t getNumTrees() const
Get the number of independent trees that this Skeleton contains.
Definition Skeleton.cpp:812
std::size_t getNumDofs() const override
Return the number of degrees of freedom in this skeleton.
Definition Skeleton.cpp:1135
const std::string & addEntryToJointNameMgr(Joint *_newJoint, bool _updateDofNames=true)
Add a Joint to to the Joint NameManager.
Definition Skeleton.cpp:655
SkeletonPtr clone() const
Create an identical clone of this Skeleton.
Definition Skeleton.cpp:399
bool moveBodyNodeTree(Joint *_parentJoint, BodyNode *_bodyNode, SkeletonPtr _newSkeleton, BodyNode *_parentNode)
Move a subtree of BodyNodes from this Skeleton to another Skeleton.
Definition Skeleton.cpp:2482
void updateCacheDimensions(DataCache &_cache)
Update the dimensions for a specific cache.
Definition Skeleton.cpp:2695
Joint * getRootJoint(std::size_t treeIdx=0u)
Get the root Joint of the tree whose index in this Skeleton is treeIdx.
Definition Skeleton.cpp:847
const Eigen::MatrixXd & getInvMassMatrix() const override
Get inverse of Mass Matrix of the MetaSkeleton.
Definition Skeleton.cpp:1956
std::mutex & getMutex() const
Get the mutex that protects the state of this Skeleton.
Definition Skeleton.cpp:379
void setState(const State &state)
Set the State of this Skeleton [alias for setCompositeState(~)].
Definition Skeleton.cpp:565
const std::shared_ptr< WholeBodyIK > & createIK()
Create a new WholeBodyIK module for this Skeleton.
Definition Skeleton.cpp:1470
void integratePositions(double _dt)
Definition Skeleton.cpp:1492
dart::common::NameManager< DegreeOfFreedom * > mNameMgrForDofs
NameManager for tracking DegreesOfFreedom.
Definition Skeleton.hpp:1176
void setConfiguration(const Configuration &configuration)
Set the configuration of this Skeleton.
Definition Skeleton.cpp:519
const AspectProperties & getSkeletonProperties() const
Get the Properties of this Skeleton.
Definition Skeleton.cpp:606
void updateTotalMass()
Update the computation for total mass.
Definition Skeleton.cpp:2687
common::aligned_vector< DataCache > mTreeCache
Definition Skeleton.hpp:1292
const std::string & addEntryToBodyNodeNameMgr(BodyNode *_newNode)
Add a BodyNode to the BodyNode NameManager.
Definition Skeleton.cpp:646
std::size_t mUnionSize
Definition Skeleton.hpp:1324
std::size_t mUnionIndex
Definition Skeleton.hpp:1327
const std::vector< BodyNode * > & getTreeBodyNodes(std::size_t _treeIdx)
Get the BodyNodes belonging to a tree in this Skeleton.
Definition Skeleton.cpp:1009
void registerBodyNode(BodyNode *_newBodyNode)
Register a BodyNode with the Skeleton. Internal use only.
Definition Skeleton.cpp:2113
void addEntryToSoftBodyNodeNameMgr(SoftBodyNode *_newNode)
Add a SoftBodyNode to the SoftBodyNode NameManager.
Definition Skeleton.cpp:668
void clearInternalForces() override
Clear the internal forces of the BodyNodes in this MetaSkeleton.
Definition Skeleton.cpp:3641
std::weak_ptr< Skeleton > mUnionRootSkeleton
Definition Skeleton.hpp:1321
Skeleton & operator=(const Skeleton &_other)=delete
Remove copy operator.
const Eigen::VectorXd & getCoriolisAndGravityForces() const override
Get combined vector of Coriolis force and gravity force of the MetaSkeleton.
Definition Skeleton.cpp:2028
bool isImpulseApplied() const
Get whether this skeleton is constrained.
Definition Skeleton.cpp:3853
std::size_t getSupportVersion() const
The version number of a support polygon will be incremented each time the support polygon needs to be...
Definition Skeleton.cpp:3543
double computePotentialEnergy() const override
Get the potential energy of this MetaSkeleton.
Definition Skeleton.cpp:3897
void clearIK()
Wipe away the WholeBodyIK module for this Skeleton, leaving it as a nullptr.
Definition Skeleton.cpp:1477
dart::common::NameManager< Joint * > mNameMgrForJoints
NameManager for tracking Joints.
Definition Skeleton.hpp:1173
math::Jacobian getCOMJacobian(const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM Jacobian in terms of any Frame (default is World Frame)
Definition Skeleton.cpp:4043
Eigen::Vector3d getCOMLinearAcceleration(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM linear acceleration in terms of any Frame (default is World Frame)
Definition Skeleton.cpp:3997
const Eigen::VectorXd & computeConstraintForces(DataCache &cache) const
Compute the constraint force vector for a tree.
Definition Skeleton.cpp:3375
math::Jacobian getJacobianClassicDeriv(const JacobianNode *_node) const override
Get the spatial Jacobian (classical) time derivative targeting the origin of a BodyNode.
Definition Skeleton.cpp:1819
const std::vector< DegreeOfFreedom * > & getDofs() override
Get the vector of DegreesOfFreedom for this MetaSkeleton.
Definition Skeleton.cpp:1167
math::Jacobian getJacobian(const JacobianNode *_node) const override
Get the spatial Jacobian targeting the origin of a BodyNode.
Definition Skeleton.cpp:1629
DataCache mSkelCache
Definition Skeleton.hpp:1294
SoftBodyNode represent a soft body that has one deformable skin.
Definition SoftBodyNode.hpp:46
The WholeBodyIK class provides an interface for simultaneously solving all the IK constraints of all ...
Definition HierarchicalIK.hpp:357
Definition Random-impl.hpp:92
Matrix< double, 6, 1 > Vector6d
Definition MathTypes.hpp:49
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition Memory.hpp:66
std::shared_ptr< const Skeleton > ConstSkeletonPtr
Definition SmartPointer.hpp:60
std::shared_ptr< MetaSkeleton > MetaSkeletonPtr
Definition SmartPointer.hpp:68
std::shared_ptr< Skeleton > SkeletonPtr
Definition SmartPointer.hpp:60
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition MathTypes.hpp:108
Eigen::Matrix< double, 3, Eigen::Dynamic > AngularJacobian
Definition MathTypes.hpp:107
Eigen::Matrix< double, 3, Eigen::Dynamic > LinearJacobian
Definition MathTypes.hpp:106
std::vector< Eigen::Vector3d > SupportGeometry
Definition Geometry.hpp:496
common::aligned_vector< Eigen::Vector2d > SupportPolygon
Definition Geometry.hpp:498
Definition BulletCollisionDetector.cpp:63
Definition SharedLibraryManager.hpp:43
The Configuration struct represents the joint configuration of a Skeleton.
Definition Skeleton.hpp:95
Eigen::VectorXd mPositions
Joint positions.
Definition Skeleton.hpp:116
bool operator!=(const Configuration &other) const
Inequality comparison operator.
Definition Skeleton.cpp:335
std::vector< std::size_t > mIndices
A list of degree of freedom indices that each entry in the Eigen::VectorXd members correspond to.
Definition Skeleton.hpp:113
Eigen::VectorXd mVelocities
Joint velocities.
Definition Skeleton.hpp:119
Eigen::VectorXd mAccelerations
Joint accelerations.
Definition Skeleton.hpp:122
bool operator==(const Configuration &other) const
Equality comparison operator.
Definition Skeleton.cpp:320
Eigen::VectorXd mCommands
Joint commands.
Definition Skeleton.hpp:128
Eigen::VectorXd mForces
Joint forces.
Definition Skeleton.hpp:125
Definition Skeleton.hpp:1228
std::vector< std::size_t > mSupportIndices
A map of which EndEffectors correspond to the individual points in the support polygon.
Definition Skeleton.hpp:1276
Eigen::VectorXd mFext
External force vector for the skeleton.
Definition Skeleton.hpp:1266
std::vector< const DegreeOfFreedom * > mConstDofs
Cache for const Degrees of Freedom, for the sake of the API.
Definition Skeleton.hpp:1241
Eigen::VectorXd mCvec
Coriolis vector for the skeleton which is C(q,dq)*dq.
Definition Skeleton.hpp:1256
Eigen::MatrixXd mInvM
Inverse of mass matrix for the skeleton.
Definition Skeleton.hpp:1250
Eigen::MatrixXd mAugM
Mass matrix for the skeleton.
Definition Skeleton.hpp:1247
Eigen::MatrixXd mInvAugM
Inverse of augmented mass matrix for the skeleton.
Definition Skeleton.hpp:1253
Eigen::VectorXd mG
Gravity vector for the skeleton; computed in nonrecursive dynamics only.
Definition Skeleton.hpp:1260
math::SupportPolygon mSupportPolygon
Support polygon.
Definition Skeleton.hpp:1272
DirtyFlags mDirty
Definition Skeleton.hpp:1229
std::vector< const BodyNode * > mConstBodyNodes
Cache for const BodyNodes, for the sake of the API.
Definition Skeleton.hpp:1235
std::vector< DegreeOfFreedom * > mDofs
Degrees of Freedom belonging to this tree.
Definition Skeleton.hpp:1238
math::SupportGeometry mSupportGeometry
Support geometry – only used for temporary storage purposes.
Definition Skeleton.hpp:1283
Eigen::Vector2d mSupportCentroid
Centroid of the support polygon.
Definition Skeleton.hpp:1286
std::pair< Eigen::Vector3d, Eigen::Vector3d > mSupportAxes
A pair of vectors which map the 2D coordinates of the support polygon into 3D space.
Definition Skeleton.hpp:1280
std::vector< BodyNode * > mBodyNodes
BodyNodes belonging to this tree.
Definition Skeleton.hpp:1232
Eigen::VectorXd mFc
Constraint force vector.
Definition Skeleton.hpp:1269
Eigen::MatrixXd mM
Mass matrix cache.
Definition Skeleton.hpp:1244
Eigen::VectorXd mCg
Combined coriolis and gravity vector which is C(q, dq)*dq + g(q).
Definition Skeleton.hpp:1263
Definition Skeleton.hpp:1185
bool mCoriolisForces
Dirty flag for the Coriolis force vector.
Definition Skeleton.hpp:1208
bool mExternalForces
Dirty flag for the external force vector.
Definition Skeleton.hpp:1214
bool mArticulatedInertia
Dirty flag for articulated body inertia.
Definition Skeleton.hpp:1190
bool mInvAugMassMatrix
Dirty flag for the inverse of augmented mass matrix.
Definition Skeleton.hpp:1202
bool mMassMatrix
Dirty flag for the mass matrix.
Definition Skeleton.hpp:1193
bool mAugMassMatrix
Dirty flag for the mass matrix.
Definition Skeleton.hpp:1196
std::size_t mSupportVersion
Increments each time a new support polygon is computed to help keep track of changes in the support p...
Definition Skeleton.hpp:1224
bool mSupport
Dirty flag for the support polygon.
Definition Skeleton.hpp:1220
bool mGravityForces
Dirty flag for the gravity force vector.
Definition Skeleton.hpp:1205
bool mDampingForces
Dirty flag for the damping force vector.
Definition Skeleton.hpp:1217
bool mCoriolisAndGravityForces
Dirty flag for the combined vector of Coriolis and gravity.
Definition Skeleton.hpp:1211
bool mInvMassMatrix
Dirty flag for the inverse of mass matrix.
Definition Skeleton.hpp:1199
The Properties of this Skeleton which are independent of the components within the Skeleton,...
Definition SkeletonAspect.hpp:54