DART 6.7.3
Loading...
Searching...
No Matches
BodyNode.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_BODYNODE_HPP_
34#define DART_DYNAMICS_BODYNODE_HPP_
35
36#include <string>
37#include <vector>
38
39#include <Eigen/Dense>
40#include <Eigen/StdVector>
41
42#include "dart/config.hpp"
54
55namespace dart {
56namespace dynamics {
57
58class GenCoord;
59class Skeleton;
60class Joint;
61class DegreeOfFreedom;
62class Shape;
63class EndEffector;
64class Marker;
65
73class BodyNode :
75 public virtual BodyNodeSpecializedFor<ShapeNode, EndEffector, Marker>,
77 public TemplatedJacobianNode<BodyNode>
78{
79public:
80
82 = common::Signal<void(const BodyNode*, ConstShapePtr _newColShape)>;
83
85
87 = common::Signal<void(const BodyNode*)>;
89
92
95
98
99 BodyNode(const BodyNode&) = delete;
100
102 virtual ~BodyNode();
103
106 virtual SoftBodyNode* asSoftBodyNode();
107
110 virtual const SoftBodyNode* asSoftBodyNode() const;
111
113 void setAllNodeStates(const AllNodeStates& states);
114
117
120
123
125 void setProperties(const CompositeProperties& _properties);
126
128 void setProperties(const AspectProperties& _properties);
129
131 void setAspectState(const AspectState& state);
132
135
138
140 void copy(const BodyNode& otherBodyNode);
141
143 void copy(const BodyNode* otherBodyNode);
144
146 BodyNode& operator=(const BodyNode& _otherBodyNode);
147
149 void duplicateNodes(const BodyNode* otherBodyNode);
150
153 void matchNodes(const BodyNode* otherBodyNode);
154
157 const std::string& setName(const std::string& _name) override;
158
159 // Documentation inherited
160 const std::string& getName() const override;
161
164 void setGravityMode(bool _gravityMode);
165
167 bool getGravityMode() const;
168
170 bool isCollidable() const;
171
174 void setCollidable(bool _isCollidable);
175
177 void setMass(double mass);
178
180 double getMass() const;
181
187 double _Ixx, double _Iyy, double _Izz,
188 double _Ixy = 0.0, double _Ixz = 0.0, double _Iyz = 0.0);
189
192 double& _Ixx, double& _Iyy, double& _Izz,
193 double& _Ixy, double& _Ixz, double& _Iyz) const;
194
196 const Eigen::Matrix6d& getSpatialInertia() const;
197
199 void setInertia(const Inertia& inertia);
200
202 const Inertia& getInertia() const;
203
206
210
212 void setLocalCOM(const Eigen::Vector3d& _com);
213
215 const Eigen::Vector3d& getLocalCOM() const;
216
218 Eigen::Vector3d getCOM(const Frame* _withRespectTo = Frame::World()) const;
219
222 Eigen::Vector3d getCOMLinearVelocity(
223 const Frame* _relativeTo = Frame::World(),
224 const Frame* _inCoordinatesOf = Frame::World()) const;
225
229
233 const Frame* _inCoordinatesOf) const;
234
237 Eigen::Vector3d getCOMLinearAcceleration(
238 const Frame* _relativeTo = Frame::World(),
239 const Frame* _inCoordinatesOf = Frame::World()) const;
240
244
248 const Frame* _inCoordinatesOf) const;
249
251 void setFrictionCoeff(double _coeff);
252
254 double getFrictionCoeff() const;
255
257 void setRestitutionCoeff(double _coeff);
258
260 double getRestitutionCoeff() const;
261
262 //--------------------------------------------------------------------------
263 // Structural Properties
264 //--------------------------------------------------------------------------
265
267 std::size_t getIndexInSkeleton() const;
268
270 std::size_t getIndexInTree() const;
271
273 std::size_t getTreeIndex() const;
274
284 SkeletonPtr remove(const std::string& _name = "temporary");
285
295 bool moveTo(BodyNode* _newParent);
296
302 bool moveTo(const SkeletonPtr& _newSkeleton, BodyNode* _newParent);
303
304#ifdef _WIN32
305 template <typename JointType>
306 static typename JointType::Properties createJointProperties()
307 {
308 return typename JointType::Properties();
309 }
310
311 template <typename NodeType>
312 static typename NodeType::Properties createBodyNodeProperties()
313 {
314 return typename NodeType::Properties();
315 }
316#endif
317 // TODO: Workaround for MSVC bug on template function specialization with
318 // default argument. Please see #487 for detail
319
328 template <class JointType>
329 JointType* moveTo(BodyNode* _newParent,
330#ifdef _WIN32
331 const typename JointType::Properties& _joint
332 = BodyNode::createJointProperties<JointType>());
333#else
334 const typename JointType::Properties& _joint
335 = typename JointType::Properties());
336#endif
337 // TODO: Workaround for MSVC bug on template function specialization with
338 // default argument. Please see #487 for detail
339
343 template <class JointType>
344 JointType* moveTo(const SkeletonPtr& _newSkeleton, BodyNode* _newParent,
345#ifdef _WIN32
346 const typename JointType::Properties& _joint
347 = BodyNode::createJointProperties<JointType>());
348#else
349 const typename JointType::Properties& _joint
350 = typename JointType::Properties());
351#endif
352 // TODO: Workaround for MSVC bug on template function specialization with
353 // default argument. Please see #487 for detail
354
365 SkeletonPtr split(const std::string& _skeletonName);
366
369 template <class JointType>
370 SkeletonPtr split(const std::string& _skeletonName,
371#ifdef _WIN32
372 const typename JointType::Properties& _joint
373 = BodyNode::createJointProperties<JointType>());
374#else
375 const typename JointType::Properties& _joint
376 = typename JointType::Properties());
377#endif
378 // TODO: Workaround for MSVC bug on template function specialization with
379 // default argument. Please see #487 for detail
380
385 template <class JointType>
386 JointType* changeParentJointType(
387#ifdef _WIN32
388 const typename JointType::Properties& _joint
389 = BodyNode::createJointProperties<JointType>());
390#else
391 const typename JointType::Properties& _joint
392 = typename JointType::Properties());
393#endif
394 // TODO: Workaround for MSVC bug on template function specialization with
395 // default argument. Please see #487 for detail
396
405 std::pair<Joint*, BodyNode*> copyTo(BodyNode* _newParent,
406 bool _recursive=true);
407
418 std::pair<Joint*, BodyNode*> copyTo(const SkeletonPtr& _newSkeleton,
419 BodyNode* _newParent,
420 bool _recursive=true) const;
421
424 template <class JointType>
425 std::pair<JointType*, BodyNode*> copyTo(
426 BodyNode* _newParent,
427#ifdef _WIN32
428 const typename JointType::Properties& _joint
429 = BodyNode::createJointProperties<JointType>(),
430#else
431 const typename JointType::Properties& _joint
432 = typename JointType::Properties(),
433#endif
434 bool _recursive = true);
435 // TODO: Workaround for MSVC bug on template function specialization with
436 // default argument. Please see #487 for detail
437
440 template <class JointType>
441 std::pair<JointType*, BodyNode*> copyTo(
442 const SkeletonPtr& _newSkeleton, BodyNode* _newParent,
443#ifdef _WIN32
444 const typename JointType::Properties& _joint
445 = BodyNode::createJointProperties<JointType>(),
446#else
447 const typename JointType::Properties& _joint
448 = typename JointType::Properties(),
449#endif
450 bool _recursive = true) const;
451 // TODO: Workaround for MSVC bug on template function specialization with
452 // default argument. Please see #487 for detail
453
458 SkeletonPtr copyAs(const std::string& _skeletonName,
459 bool _recursive=true) const;
460
463 template <class JointType>
465 const std::string& _skeletonName,
466#ifdef _WIN32
467 const typename JointType::Properties& _joint
468 = BodyNode::createJointProperties<JointType>(),
469#else
470 const typename JointType::Properties& _joint
471 = typename JointType::Properties(),
472#endif
473 bool _recursive=true) const;
474 // TODO: Workaround for MSVC bug on template function specialization with
475 // default argument. Please see #487 for detail
476
477 // Documentation inherited
478 SkeletonPtr getSkeleton() override;
479
480 // Documentation inherited
481 ConstSkeletonPtr getSkeleton() const override;
482
485
487 const Joint* getParentJoint() const;
488
490 BodyNode* getParentBodyNode();
491
493 const BodyNode* getParentBodyNode() const;
494
496 template <class JointType, class NodeType = BodyNode>
497 std::pair<JointType*, NodeType*> createChildJointAndBodyNodePair(
498#ifdef _WIN32
499 const typename JointType::Properties& _jointProperties
500 = BodyNode::createJointProperties<JointType>(),
501 const typename NodeType::Properties& _bodyProperties
502 = BodyNode::createBodyNodeProperties<NodeType>());
503#else
504 const typename JointType::Properties& _jointProperties
505 = typename JointType::Properties(),
506 const typename NodeType::Properties& _bodyProperties
507 = typename NodeType::Properties());
508#endif
509 // TODO: Workaround for MSVC bug on template function specialization with
510 // default argument. Please see #487 for detail
511
513 std::size_t getNumChildBodyNodes() const;
514
516 BodyNode* getChildBodyNode(std::size_t _index);
517
519 const BodyNode* getChildBodyNode(std::size_t _index) const;
520
522 std::size_t getNumChildJoints() const;
523
525 Joint* getChildJoint(std::size_t _index);
526
528 const Joint* getChildJoint(std::size_t _index) const;
529
531 template <class NodeType, typename ...Args>
532 NodeType* createNode(Args&&... args);
533
535
536
541 template <class ShapeNodeProperties>
542 ShapeNode* createShapeNode(ShapeNodeProperties properties,
543 bool automaticName = true);
544
547 template <class ShapeType>
548 ShapeNode* createShapeNode(const std::shared_ptr<ShapeType>& shape);
549
551 template <class ShapeType, class StringType>
552 ShapeNode* createShapeNode(
553 const std::shared_ptr<ShapeType>& shape,
554 StringType&& name);
555
557 const std::vector<ShapeNode*> getShapeNodes();
558
560 const std::vector<const ShapeNode*> getShapeNodes() const;
561
563 void removeAllShapeNodes();
564
567 template <class... Aspects>
568 ShapeNode* createShapeNodeWith(const ShapePtr& shape);
569
571 template <class... Aspects>
572 ShapeNode* createShapeNodeWith(const ShapePtr& shape,
573 const std::string& name);
574
576 template <class Aspect>
577 std::size_t getNumShapeNodesWith() const;
578
580 template <class Aspect>
581 const std::vector<ShapeNode*> getShapeNodesWith();
582
584 template <class Aspect>
585 const std::vector<const ShapeNode*> getShapeNodesWith() const;
586
588 template <class Aspect>
590
592
593
596 const EndEffector::BasicProperties& _properties);
597
599 EndEffector* createEndEffector(const std::string& _name = "EndEffector");
600
602 EndEffector* createEndEffector(const char* _name);
603
605
606
607 Marker* createMarker(
608 const std::string& name = "marker",
609 const Eigen::Vector3d& position = Eigen::Vector3d::Zero(),
610 const Eigen::Vector4d& color = Eigen::Vector4d::Constant(1.0));
611
614
615 // Documentation inherited
616 bool dependsOn(std::size_t _genCoordIndex) const override;
617
618 // Documentation inherited
619 std::size_t getNumDependentGenCoords() const override;
620
621 // Documentation inherited
622 std::size_t getDependentGenCoordIndex(std::size_t _arrayIndex) const override;
623
624 // Documentation inherited
625 const std::vector<std::size_t>& getDependentGenCoordIndices() const override;
626
627 // Documentation inherited
628 std::size_t getNumDependentDofs() const override;
629
630 // Documentation inherited
631 DegreeOfFreedom* getDependentDof(std::size_t _index) override;
632
633 // Documentation inherited
634 const DegreeOfFreedom* getDependentDof(std::size_t _index) const override;
635
636 // Documentation inherited
637 const std::vector<DegreeOfFreedom*>& getDependentDofs() override;
638
639 // Documentation inherited
640 const std::vector<const DegreeOfFreedom*>& getDependentDofs() const override;
641
642 // Documentation inherited
643 const std::vector<const DegreeOfFreedom*> getChainDofs() const override;
644
645 //--------------------------------------------------------------------------
646 // Properties updated by dynamics (kinematics)
647 //--------------------------------------------------------------------------
648
651 const Eigen::Isometry3d& getRelativeTransform() const override;
652
653 // Documentation inherited
654 const Eigen::Vector6d& getRelativeSpatialVelocity() const override;
655
656 // Documentation inherited
657 const Eigen::Vector6d& getRelativeSpatialAcceleration() const override;
658
659 // Documentation inherited
660 const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override;
661
663 const Eigen::Vector6d& getPartialAcceleration() const override;
664
667 const math::Jacobian& getJacobian() const override final;
668
669 // Prevent the inherited getJacobian functions from being shadowed
670 using TemplatedJacobianNode<BodyNode>::getJacobian;
671
674 const math::Jacobian& getWorldJacobian() const override final;
675
676 // Prevent the inherited getWorldJacobian functions from being shadowed
677 using TemplatedJacobianNode<BodyNode>::getWorldJacobian;
678
687 const math::Jacobian& getJacobianSpatialDeriv() const override final;
688
689 // Prevent the inherited getJacobianSpatialDeriv functions from being shadowed
690 using TemplatedJacobianNode<BodyNode>::getJacobianSpatialDeriv;
691
699 const math::Jacobian& getJacobianClassicDeriv() const override final;
700
701 // Prevent the inherited getJacobianClassicDeriv functions from being shadowed
702 using TemplatedJacobianNode<BodyNode>::getJacobianClassicDeriv;
703
705 const Eigen::Vector6d& getBodyVelocityChange() const;
706
711 DART_DEPRECATED(6.0)
712 void setColliding(bool _isColliding);
713
716 DART_DEPRECATED(6.0)
717 bool isColliding();
718
726 void addExtForce(const Eigen::Vector3d& _force,
727 const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero(),
728 bool _isForceLocal = false,
729 bool _isOffsetLocal = true);
730
732 void setExtForce(const Eigen::Vector3d& _force,
733 const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero(),
734 bool _isForceLocal = false,
735 bool _isOffsetLocal = true);
736
740 void addExtTorque(const Eigen::Vector3d& _torque, bool _isLocal = false);
741
745 void setExtTorque(const Eigen::Vector3d& _torque, bool _isLocal = false);
746
751 virtual void clearExternalForces();
752
756 virtual void clearInternalForces();
757
759 const Eigen::Vector6d& getExternalForceLocal() const;
760
762 Eigen::Vector6d getExternalForceGlobal() const;
763
769 const Eigen::Vector6d& getBodyForce() const;
770
771 //----------------------------------------------------------------------------
772 // Constraints
773 // - Following functions are managed by constraint solver.
774 //----------------------------------------------------------------------------
775
780 bool isReactive() const;
781
784 void setConstraintImpulse(const Eigen::Vector6d& _constImp);
785
788 void addConstraintImpulse(const Eigen::Vector6d& _constImp);
789
791 void addConstraintImpulse(const Eigen::Vector3d& _constImp,
792 const Eigen::Vector3d& _offset,
793 bool _isImpulseLocal = false,
794 bool _isOffsetLocal = true);
795
798 virtual void clearConstraintImpulse();
799
801 const Eigen::Vector6d& getConstraintImpulse() const;
802
803 //----------------------------------------------------------------------------
804 // Energies
805 //----------------------------------------------------------------------------
806
808 double computeLagrangian(const Eigen::Vector3d& gravity) const;
809
811 DART_DEPRECATED(6.1)
812 virtual double getKineticEnergy() const;
813
815 double computeKineticEnergy() const;
816
818 DART_DEPRECATED(6.1)
819 virtual double getPotentialEnergy(const Eigen::Vector3d& _gravity) const;
820
822 double computePotentialEnergy(const Eigen::Vector3d& gravity) const;
823
825 Eigen::Vector3d getLinearMomentum() const;
826
828 Eigen::Vector3d getAngularMomentum(
829 const Eigen::Vector3d& _pivot = Eigen::Vector3d::Zero());
830
831 //----------------------------------------------------------------------------
832 // Notifications
833 //----------------------------------------------------------------------------
834
835 // Documentation inherited
836 void dirtyTransform() override;
837
838 // Documentation inherited
839 void dirtyVelocity() override;
840
841 // Documentation inherited
842 void dirtyAcceleration() override;
843
846 DART_DEPRECATED(6.2)
848
852
854 DART_DEPRECATED(6.2)
856
858 void dirtyExternalForces();
859
861 DART_DEPRECATED(6.2)
863
865 void dirtyCoriolisForces();
866
867 //----------------------------------------------------------------------------
868 // Friendship
869 //----------------------------------------------------------------------------
870
871 friend class Skeleton;
872 friend class Joint;
873 friend class EndEffector;
874 friend class SoftBodyNode;
875 friend class PointMass;
876 friend class Node;
877
878protected:
879
881 BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint,
882 const Properties& _properties);
883
885 BodyNode(const std::tuple<BodyNode*, Joint*, Properties>& args);
886
889 virtual BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint,
890 bool cloneNodes) const;
891
893 Node* cloneNode(BodyNode* bn) const override final;
894
896 virtual void init(const SkeletonPtr& _skeleton);
897
899 void addChildBodyNode(BodyNode* _body);
900
901 //----------------------------------------------------------------------------
903 //----------------------------------------------------------------------------
904
907 void processNewEntity(Entity* _newChildEntity) override;
908
910 void processRemovedEntity(Entity* _oldChildEntity) override;
911
913 virtual void updateTransform();
914
916 virtual void updateVelocity();
917
919 virtual void updatePartialAcceleration() const;
920
923 virtual void updateArtInertia(double _timeStep) const;
924
929 virtual void updateBiasForce(const Eigen::Vector3d& _gravity,
930 double _timeStep);
931
934 virtual void updateBiasImpulse();
935
938 virtual void updateAccelerationID();
939
941 virtual void updateAccelerationFD();
942
944 virtual void updateVelocityChangeFD();
945
951 virtual void updateTransmittedForceID(const Eigen::Vector3d& _gravity,
952 bool _withExternalForces = false);
953
959 virtual void updateTransmittedForceFD();
960
966 virtual void updateTransmittedImpulse();
967 // TODO: Rename to updateTransmittedImpulseFD if impulse-based inverse
968 // dynamics is implemented.
969
971 virtual void updateJointForceID(double _timeStep,
972 bool _withDampingForces,
973 bool _withSpringForces);
974
976 virtual void updateJointForceFD(double _timeStep,
977 bool _withDampingForces,
978 bool _withSpringForces);
979
981 virtual void updateJointImpulseFD();
982
985 virtual void updateConstrainedTerms(double _timeStep);
986
988
989 //----------------------------------------------------------------------------
991 //----------------------------------------------------------------------------
992
994 virtual void updateMassMatrix();
995 virtual void aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col);
996 virtual void aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col,
997 double _timeStep);
998
1000 virtual void updateInvMassMatrix();
1001 virtual void updateInvAugMassMatrix();
1002 virtual void aggregateInvMassMatrix(Eigen::MatrixXd& _InvMCol, std::size_t _col);
1003 virtual void aggregateInvAugMassMatrix(Eigen::MatrixXd& _InvMCol, std::size_t _col,
1004 double _timeStep);
1005
1007 virtual void aggregateCoriolisForceVector(Eigen::VectorXd& _C);
1008
1010 virtual void aggregateGravityForceVector(Eigen::VectorXd& _g,
1011 const Eigen::Vector3d& _gravity);
1012
1014 virtual void updateCombinedVector();
1015 virtual void aggregateCombinedVector(Eigen::VectorXd& _Cg,
1016 const Eigen::Vector3d& _gravity);
1017
1020 virtual void aggregateExternalForces(Eigen::VectorXd& _Fext);
1021
1023 virtual void aggregateSpatialToGeneralized(Eigen::VectorXd& _generalized,
1024 const Eigen::Vector6d& _spatial);
1025
1028 void updateBodyJacobian() const;
1029
1032 void updateWorldJacobian() const;
1033
1037 void updateBodyJacobianSpatialDeriv() const;
1038
1043
1045
1046protected:
1047
1048 //--------------------------------------------------------------------------
1049 // General properties
1050 //--------------------------------------------------------------------------
1051
1053 int mID;
1054
1056 static std::size_t msBodyNodeCount;
1057
1061
1062 //--------------------------------------------------------------------------
1063 // Structural Properties
1064 //--------------------------------------------------------------------------
1065
1068
1071
1074
1077
1080
1083
1087
1090
1094
1097
1098 //--------------------------------------------------------------------------
1099 // Dynamical Properties
1100 //--------------------------------------------------------------------------
1101
1105 mutable math::Jacobian mBodyJacobian;
1106
1110 mutable math::Jacobian mWorldJacobian;
1111
1115 mutable math::Jacobian mBodyJacobianSpatialDeriv;
1116
1120 mutable math::Jacobian mWorldJacobianClassicDeriv;
1121
1125 mutable Eigen::Vector6d mPartialAcceleration;
1126 // TODO(JS): Rename with more informative name
1127
1130
1133 Eigen::Vector6d mF;
1134
1136 Eigen::Vector6d mFgravity;
1137
1141 mutable math::Inertia mArtInertia;
1142
1147
1150
1152 Eigen::Vector6d mCg_dV;
1153 Eigen::Vector6d mCg_F;
1154
1156 Eigen::Vector6d mG_F;
1157
1159 Eigen::Vector6d mFext_F;
1160
1162 Eigen::Vector6d mM_dV;
1163 Eigen::Vector6d mM_F;
1164
1166 Eigen::Vector6d mInvM_c;
1167 Eigen::Vector6d mInvM_U;
1168
1171
1172 //------------------------- Impulse-based Dyanmics ---------------------------
1175 Eigen::Vector6d mDelV;
1176
1180
1183
1184 // TODO(JS): rename with more informative one
1186 Eigen::Vector6d mImpF;
1187
1190
1193
1196
1197public:
1198 // To get byte-aligned Eigen vectors
1199 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1200
1201 //----------------------------------------------------------------------------
1203 //----------------------------------------------------------------------------
1204
1207
1210
1213 mutable common::SlotRegister<StructuralChangeSignal> onStructuralChange;
1214
1216
1217private:
1218
1222
1223};
1224
1225} // namespace dynamics
1226} // namespace dart
1227
1228#include "dart/dynamics/detail/BodyNode.hpp"
1229
1230#endif // DART_DYNAMICS_BODYNODE_HPP_
#define DART_BAKE_SPECIALIZED_NODE_DECLARATIONS(AspectName)
Definition BasicNodeManager.hpp:306
#define DART_DEPRECATED(version)
Definition Deprecated.hpp:51
BodyPropPtr properties
Definition SdfParser.cpp:80
std::string * name
Definition SkelParser.cpp:1642
Eigen::VectorXd position
Definition SkelParser.cpp:107
MapHolder is a templated wrapper class that is used to allow maps of Aspect::State and Aspect::Proper...
Definition Cloneable.hpp:224
detail::CompositeProperties Properties
Definition Composite.hpp:56
This is an alternative to EmbedStateAndProperties which allows your class to also inherit other Compo...
Definition EmbeddedAspect.hpp:438
typename Impl::AspectState AspectState
Definition EmbeddedAspect.hpp:444
Definition CompositeData.hpp:180
Definition CompositeData.hpp:104
Declaration of the variadic template.
Definition SpecializedNodeManager.hpp:49
BodyNode class represents a single node of the skeleton.
Definition BodyNode.hpp:78
void updateBodyJacobian() const
Update body Jacobian.
Definition BodyNode.cpp:2304
void addConstraintImpulse(const Eigen::Vector6d &_constImp)
Add constraint impulse.
Definition BodyNode.cpp:1898
BodyNode * getChildBodyNode(std::size_t _index)
Return the _index-th child BodyNode of this BodyNode.
Definition BodyNode.cpp:859
std::set< Entity * > mNonBodyNodeEntities
Array of child Entities that are not BodyNodes.
Definition BodyNode.hpp:1086
Node * cloneNode(BodyNode *bn) const override final
This is needed in order to inherit the Node class, but it does nothing.
Definition BodyNode.cpp:1290
void dirtyCoriolisForces()
Tell the Skeleton that the coriolis forces need to be update.
Definition BodyNode.cpp:1540
const std::string & getName() const override
Return the name of this Entity.
Definition BodyNode.cpp:427
Eigen::Vector3d getCOMLinearVelocity(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const
Return the linear velocity of the center of mass, expressed in terms of arbitrary Frames.
Definition BodyNode.cpp:591
detail::NodeStateMap NodeStateMap
Definition BodyNode.hpp:91
BodyNode(const BodyNode &)=delete
void removeAllShapeNodesWith()
Remove all ShapeNodes containing given Aspect from this BodyNode.
Definition BodyNode.hpp:255
void addChildBodyNode(BodyNode *_body)
Add a child bodynode into the bodynode.
Definition BodyNode.cpp:834
virtual ~BodyNode()
Destructor.
Definition BodyNode.cpp:235
bool mIsColliding
Whether the node is currently in collision with another node.
Definition BodyNode.hpp:1060
common::SlotRegister< ColShapeRemovedSignal > onColShapeRemoved
Slot register for collision shape removed signal.
Definition BodyNode.hpp:1209
void notifyArticulatedInertiaUpdate()
Notify the Skeleton that the tree of this BodyNode needs an articulated inertia update.
Definition BodyNode.cpp:1508
const math::Jacobian & getJacobianSpatialDeriv() const override final
Return the spatial time derivative of the generalized Jacobian targeting the origin of this BodyNode.
Definition BodyNode.cpp:1111
virtual void updateInvAugMassMatrix()
Definition BodyNode.cpp:2226
virtual void aggregateInvMassMatrix(Eigen::MatrixXd &_InvMCol, std::size_t _col)
Definition BodyNode.cpp:2247
const std::string & setName(const std::string &_name) override
Set name.
Definition BodyNode.cpp:390
Marker * createMarker(const std::string &name="marker", const Eigen::Vector3d &position=Eigen::Vector3d::Zero(), const Eigen::Vector4d &color=Eigen::Vector4d::Constant(1.0))
Create a Marker with the given fields.
Definition BodyNode.cpp:959
math::Jacobian mBodyJacobianSpatialDeriv
Spatial time derivative of body Jacobian.
Definition BodyNode.hpp:1115
Joint * getChildJoint(std::size_t _index)
Return the _index-th child Joint of this BodyNode.
Definition BodyNode.cpp:877
Eigen::Vector6d mPartialAcceleration
Partial spatial body acceleration due to parent joint's velocity.
Definition BodyNode.hpp:1125
Joint * getParentJoint()
Return the parent Joint of this BodyNode.
Definition BodyNode.cpp:810
const Eigen::Vector3d & getLocalCOM() const
Return center of mass expressed in body frame.
Definition BodyNode.cpp:579
double getFrictionCoeff() const
Return frictional coefficient.
Definition BodyNode.cpp:644
virtual void updateConstrainedTerms(double _timeStep)
Update constrained terms due to the constraint impulses for foward dynamics.
Definition BodyNode.cpp:1810
Eigen::Vector6d mF
Transmitted wrench from parent to the bodynode expressed in body-fixed frame.
Definition BodyNode.hpp:1133
void setExtTorque(const Eigen::Vector3d &_torque, bool _isLocal=false)
Set applying Cartesian torque to the node.
Definition BodyNode.cpp:1207
void setProperties(const CompositeProperties &_properties)
Same as setCompositeProperties()
Definition BodyNode.cpp:291
void notifyCoriolisUpdate()
Tell the Skeleton that the coriolis forces need to be update.
Definition BodyNode.cpp:1534
void dirtyAcceleration() override
Notify the acceleration updates of this Frame and all its children are needed.
Definition BodyNode.cpp:1492
virtual void updateBiasForce(const Eigen::Vector3d &_gravity, double _timeStep)
Update bias force associated with the articulated body inertia for forward dynamics.
Definition BodyNode.cpp:1653
Properties getBodyNodeProperties() const
Get the Properties of this BodyNode.
Definition BodyNode.cpp:323
Eigen::Vector6d mM_F
Definition BodyNode.hpp:1163
bool isReactive() const
Return true if the body can react to force or constraint impulse.
Definition BodyNode.cpp:1960
bool isColliding()
Return whether this body node is set to be colliding with other objects.
Definition BodyNode.cpp:1141
const Eigen::Vector6d & getBodyForce() const
Get spatial body force transmitted from the parent joint.
Definition BodyNode.cpp:1885
BodyNode * mParentBodyNode
Parent body node.
Definition BodyNode.hpp:1079
ColShapeRemovedSignal mColShapeRemovedSignal
Collision shape removed signal.
Definition BodyNode.hpp:1192
friend class Joint
Definition BodyNode.hpp:872
virtual double getPotentialEnergy(const Eigen::Vector3d &_gravity) const
Return potential energy.
Definition BodyNode.cpp:1932
virtual void clearExternalForces()
Clean up structures that store external forces: mContacts, mFext, mExtForceBody and mExtTorqueBody.
Definition BodyNode.cpp:1822
virtual void updateAccelerationFD()
Update spatial body acceleration for forward dynamics.
Definition BodyNode.cpp:1733
void matchNodes(const BodyNode *otherBodyNode)
Make the Nodes of this BodyNode match the Nodes of otherBodyNode.
Definition BodyNode.cpp:373
virtual void clearConstraintImpulse()
Clear constraint impulses and cache data used for impulse-based forward dynamics algorithm.
Definition BodyNode.cpp:1872
ShapeNode * createShapeNode(ShapeNodeProperties properties, bool automaticName=true)
Create an ShapeNode attached to this BodyNode.
Definition BodyNode.hpp:144
SkeletonPtr getSkeleton() override
Return the Skeleton that this Node is attached to.
Definition BodyNode.cpp:798
SkeletonPtr remove(const std::string &_name="temporary")
Remove this BodyNode and all of its children (recursively) from their Skeleton.
Definition BodyNode.cpp:720
Joint * mParentJoint
Parent joint.
Definition BodyNode.hpp:1076
Eigen::Vector6d mInvM_U
Definition BodyNode.hpp:1167
virtual void clearInternalForces()
Clear out the generalized forces of the parent Joint and any other forces related to this BodyNode th...
Definition BodyNode.cpp:1829
virtual void updateVelocityChangeFD()
Update spatical body velocity change for impluse-based forward dynamics.
Definition BodyNode.cpp:1753
void updateBodyJacobianSpatialDeriv() const
Update spatial time derivative of body Jacobian.
Definition BodyNode.cpp:2353
const Inertia & getInertia() const
Get the inertia data for this BodyNode.
Definition BodyNode.cpp:545
std::vector< DegreeOfFreedom * > mDependentDofs
A version of mDependentGenCoordIndices that holds DegreeOfFreedom pointers instead of indices.
Definition BodyNode.hpp:1093
Eigen::Vector6d mCg_dV
Cache data for combined vector of the system.
Definition BodyNode.hpp:1152
virtual void updateTransmittedForceID(const Eigen::Vector3d &_gravity, bool _withExternalForces=false)
Update spatial body force for inverse dynamics.
Definition BodyNode.cpp:1581
double getMass() const
Return the mass of the bodynode.
Definition BodyNode.cpp:490
virtual void aggregateSpatialToGeneralized(Eigen::VectorXd &_generalized, const Eigen::Vector6d &_spatial)
Definition BodyNode.cpp:2087
void addExtForce(const Eigen::Vector3d &_force, const Eigen::Vector3d &_offset=Eigen::Vector3d::Zero(), bool _isForceLocal=false, bool _isOffsetLocal=true)
Add applying linear Cartesian forces to this node.
Definition BodyNode.cpp:1147
virtual void updateCombinedVector()
Definition BodyNode.cpp:2019
NodeType * createNode(Args &&... args)
Create some Node type and attach it to this BodyNode.
Definition BodyNode.hpp:134
std::size_t getIndexInTree() const
Return the index of this BodyNode within its tree.
Definition BodyNode.cpp:675
const std::vector< ShapeNode * > getShapeNodesWith()
Return the list of ShapeNodes containing given Aspect.
Definition BodyNode.hpp:217
bool isCollidable() const
Return true if this body can collide with others bodies.
Definition BodyNode.cpp:453
common::SlotRegister< ColShapeAddedSignal > onColShapeAdded
Slot register for collision shape added signal.
Definition BodyNode.hpp:1206
DegreeOfFreedom * getDependentDof(std::size_t _index) override
Get a pointer to the _indexth dependent DegreeOfFreedom for this BodyNode.
Definition BodyNode.cpp:1012
JointType * changeParentJointType(const typename JointType::Properties &_joint=typename JointType::Properties())
Change the Joint type of this BodyNode's parent Joint.
Definition BodyNode.hpp:79
std::vector< BodyNode * > mChildBodyNodes
Array of child body nodes.
Definition BodyNode.hpp:1082
const Eigen::Vector6d & getPartialAcceleration() const override
Return the partial acceleration of this body.
Definition BodyNode.cpp:1084
static std::size_t msBodyNodeCount
Counts the number of nodes globally.
Definition BodyNode.hpp:1056
const std::vector< std::size_t > & getDependentGenCoordIndices() const override
Indices of the generalized coordinates which affect this JacobianNode.
Definition BodyNode.cpp:1000
Eigen::Vector6d mConstraintImpulse
Constraint impulse: contact impulse, dynamic joint impulse.
Definition BodyNode.hpp:1182
double getRestitutionCoeff() const
Return coefficient of restitution.
Definition BodyNode.cpp:663
std::pair< JointType *, NodeType * > createChildJointAndBodyNodePair(const typename JointType::Properties &_jointProperties=typename JointType::Properties(), const typename NodeType::Properties &_bodyProperties=typename NodeType::Properties())
Create a Joint and BodyNode pair as a child of this BodyNode.
Definition BodyNode.hpp:124
virtual void updateTransform()
Update transformation.
Definition BodyNode.cpp:1547
double computeKineticEnergy() const
Return kinetic energy.
Definition BodyNode.cpp:1923
void copy(const BodyNode &otherBodyNode)
Copy the Properties of another BodyNode.
Definition BodyNode.cpp:329
virtual void updateBiasImpulse()
Update bias impulse associated with the articulated body inertia for impulse-based forward dynamics.
Definition BodyNode.cpp:1692
virtual void updateTransmittedImpulse()
Update spatial body force for impulse-based forward dynamics.
Definition BodyNode.cpp:1724
const Eigen::Isometry3d & getRelativeTransform() const override
Get the transform of this BodyNode with respect to its parent BodyNode, which is also its parent Fram...
Definition BodyNode.cpp:1060
void setAspectState(const AspectState &state)
Set the AspectState of this BodyNode.
Definition BodyNode.cpp:303
Eigen::Vector6d getExternalForceGlobal() const
Definition BodyNode.cpp:1841
virtual void updatePartialAcceleration() const
Update partial spatial body acceleration due to parent joint's velocity.
Definition BodyNode.cpp:1563
math::Inertia mArtInertia
Articulated body inertia.
Definition BodyNode.hpp:1141
friend class EndEffector
Definition BodyNode.hpp:873
StructuralChangeSignal mStructuralChangeSignal
Structural change signal.
Definition BodyNode.hpp:1195
Eigen::Vector6d mG_F
Cache data for gravity force vector of the system.
Definition BodyNode.hpp:1156
const Eigen::Vector6d & getExternalForceLocal() const
Definition BodyNode.cpp:1835
double computeLagrangian(const Eigen::Vector3d &gravity) const
Return Lagrangian of this body.
Definition BodyNode.cpp:1911
SkeletonPtr split(const std::string &_skeletonName)
Remove this BodyNode and all of its children (recursively) from their current Skeleton and move them ...
Definition BodyNode.cpp:750
void setAspectProperties(const AspectProperties &properties)
Set the AspectProperties of this BodyNode.
Definition BodyNode.cpp:313
std::size_t mTreeIndex
Index of this BodyNode's tree.
Definition BodyNode.hpp:1073
void setLocalCOM(const Eigen::Vector3d &_com)
Set center of mass expressed in body frame.
Definition BodyNode.cpp:571
virtual void updateJointImpulseFD()
Update the joint impulse for forward dynamics.
Definition BodyNode.cpp:1803
void processNewEntity(Entity *_newChildEntity) override
Separate generic child Entities from child BodyNodes for more efficient update notices.
Definition BodyNode.cpp:1371
virtual BodyNode * clone(BodyNode *_parentBodyNode, Joint *_parentJoint, bool cloneNodes) const
Create a clone of this BodyNode.
Definition BodyNode.cpp:1275
void setGravityMode(bool _gravityMode)
Set whether gravity affects this body.
Definition BodyNode.cpp:433
AllNodeStates getAllNodeStates() const
Get the Node::State of all Nodes attached to this BodyNode.
Definition BodyNode.cpp:264
SkeletonPtr copyAs(const std::string &_skeletonName, bool _recursive=true) const
Create clones of this BodyNode and all of its children (recursively) and create a new Skeleton with t...
Definition BodyNode.cpp:787
bool moveTo(BodyNode *_newParent)
Remove this BodyNode and all of its children (recursively) from their current parent BodyNode,...
Definition BodyNode.cpp:726
void setFrictionCoeff(double _coeff)
Set coefficient of friction in range of [0, ~].
Definition BodyNode.cpp:631
void setCollidable(bool _isCollidable)
Set whether this body node will collide with others in the world.
Definition BodyNode.cpp:459
void getMomentOfInertia(double &_Ixx, double &_Iyy, double &_Izz, double &_Ixy, double &_Ixz, double &_Iyz) const
Return moment of inertia defined around the center of mass.
Definition BodyNode.cpp:507
bool getGravityMode() const
Return true if gravity mode is enabled.
Definition BodyNode.cpp:447
void setColliding(bool _isColliding)
Set whether this body node is colliding with other objects.
Definition BodyNode.cpp:1135
void setRestitutionCoeff(double _coeff)
Set coefficient of restitution in range of [0, 1].
Definition BodyNode.cpp:650
std::vector< std::size_t > mDependentGenCoordIndices
A increasingly sorted list of dependent dof indices.
Definition BodyNode.hpp:1089
const math::Inertia & getArticulatedInertia() const
Return the articulated body inertia.
Definition BodyNode.cpp:551
const std::vector< const DegreeOfFreedom * > getChainDofs() const override
Returns a DegreeOfFreedom vector containing the dofs that form a Chain leading up to this JacobianNod...
Definition BodyNode.cpp:1038
common::SlotRegister< StructuralChangeSignal > onStructuralChange
Raised when (1) parent BodyNode is changed, (2) moved between Skeletons, (3) parent Joint is changed.
Definition BodyNode.hpp:1213
void setMomentOfInertia(double _Ixx, double _Iyy, double _Izz, double _Ixy=0.0, double _Ixz=0.0, double _Iyz=0.0)
Set moment of inertia defined around the center of mass.
Definition BodyNode.cpp:496
Eigen::Vector3d getLinearMomentum() const
Return linear momentum.
Definition BodyNode.cpp:1944
const math::Jacobian & getJacobianClassicDeriv() const override final
Return the classical time derivative of the generalized Jacobian targeting the origin of this BodyNod...
Definition BodyNode.cpp:1120
ShapeNode * createShapeNodeWith(const ShapePtr &shape)
Create a ShapeNode with the specified Aspects and an automatically assigned name: <BodyNodeName>Shape...
Definition BodyNode.hpp:181
virtual SoftBodyNode * asSoftBodyNode()
Convert 'this' into a SoftBodyNode pointer if this BodyNode is a SoftBodyNode, otherwise return nullp...
Definition BodyNode.cpp:245
Eigen::Vector3d getCOMLinearAcceleration(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const
Return the linear acceleration of the center of mass, expressed in terms of arbitary Frames.
Definition BodyNode.cpp:611
virtual void aggregateCoriolisForceVector(Eigen::VectorXd &_C)
Definition BodyNode.cpp:1987
std::vector< const DegreeOfFreedom * > mConstDependentDofs
Same as mDependentDofs, but holds const pointers.
Definition BodyNode.hpp:1096
virtual void updateInvMassMatrix()
Definition BodyNode.cpp:2205
virtual void updateArtInertia(double _timeStep) const
Update articulated body inertia for forward dynamics.
Definition BodyNode.cpp:1623
const Eigen::Vector6d & getPrimaryRelativeAcceleration() const override
The Featherstone ABI algorithm exploits a component of the spatial acceleration which we refer to as ...
Definition BodyNode.cpp:1078
const std::vector< ShapeNode * > getShapeNodes()
Return the list of ShapeNodes.
Definition BodyNode.cpp:897
const Eigen::Vector6d & getRelativeSpatialAcceleration() const override
Get the spatial acceleration of this Frame relative to its parent Frame, in the coordinates of this F...
Definition BodyNode.cpp:1072
BodyNode * getParentBodyNode()
Return the parent BodyNdoe of this BodyNode.
Definition BodyNode.cpp:822
Eigen::Vector6d mInvM_c
Cache data for inverse mass matrix of the system.
Definition BodyNode.hpp:1166
math::Jacobian mWorldJacobian
Cached World Jacobian.
Definition BodyNode.hpp:1110
virtual void updateJointForceFD(double _timeStep, bool _withDampingForces, bool _withSpringForces)
Update the joint force for forward dynamics.
Definition BodyNode.cpp:1793
void removeAllShapeNodes()
Remove all ShapeNodes from this BodyNode.
Definition BodyNode.cpp:923
Eigen::Vector6d mM_dV
Cache data for mass matrix of the system.
Definition BodyNode.hpp:1162
detail::NodePropertiesMap NodePropertiesMap
Definition BodyNode.hpp:94
virtual void updateJointForceID(double _timeStep, bool _withDampingForces, bool _withSpringForces)
Update the joint force for inverse dynamics.
Definition BodyNode.cpp:1783
std::size_t mIndexInSkeleton
Index of this BodyNode in its Skeleton.
Definition BodyNode.hpp:1067
virtual void aggregateMassMatrix(Eigen::MatrixXd &_MCol, std::size_t _col)
Definition BodyNode.cpp:2129
Eigen::Vector6d mBiasImpulse
Impulsive bias force due to external impulsive force exerted on bodies of the parent skeleton.
Definition BodyNode.hpp:1179
virtual void updateVelocity()
Update spatial body velocity.
Definition BodyNode.cpp:1555
void dirtyVelocity() override
Notify the velocity updates of this Frame and all its children are needed.
Definition BodyNode.cpp:1465
virtual void aggregateGravityForceVector(Eigen::VectorXd &_g, const Eigen::Vector3d &_gravity)
Definition BodyNode.cpp:1993
EndEffector * createEndEffector(const EndEffector::BasicProperties &_properties)
Create an EndEffector attached to this BodyNode.
Definition BodyNode.cpp:934
std::shared_ptr< NodeDestructor > mSelfDestructor
Hold onto a reference to this BodyNode's own Destructor to make sure that it never gets destroyed.
Definition BodyNode.hpp:1221
Eigen::Vector6d mDelV
Velocity change due to to external impulsive force exerted on bodies of the parent skeleton.
Definition BodyNode.hpp:1175
void updateWorldJacobianClassicDeriv() const
Update classic time derivative of body Jacobian.
Definition BodyNode.cpp:2398
void notifyExternalForcesUpdate()
Tell the Skeleton that the external forces need to be updated.
Definition BodyNode.cpp:1522
void dirtyTransform() override
Notify the transformation updates of this Frame and all its children are needed.
Definition BodyNode.cpp:1433
const Eigen::Vector6d & getBodyVelocityChange() const
Return the velocity change due to the constraint impulse.
Definition BodyNode.cpp:1129
Eigen::Vector6d getCOMSpatialAcceleration() const
Return the acceleration of the center of mass expressed in coordinates of this BodyNode Frame and rel...
Definition BodyNode.cpp:618
void setAllNodeStates(const AllNodeStates &states)
Set the Node::State of all Nodes attached to this BodyNode.
Definition BodyNode.cpp:257
common::Signal< void(const BodyNode *, ConstShapePtr _newColShape)> ColShapeAddedSignal
Definition BodyNode.hpp:82
void setAllNodeProperties(const AllNodeProperties &properties)
Set the Node::Properties of all Nodes attached to this BodyNode.
Definition BodyNode.cpp:273
ColShapeAddedSignal mColShapeAddedSignal
Collision shape added signal.
Definition BodyNode.hpp:1189
Eigen::Vector3d getAngularMomentum(const Eigen::Vector3d &_pivot=Eigen::Vector3d::Zero())
Return angular momentum.
Definition BodyNode.cpp:1951
std::size_t mIndexInTree
Index of this BodyNode in its Tree.
Definition BodyNode.hpp:1070
Eigen::Vector6d mCg_F
Definition BodyNode.hpp:1153
virtual void updateMassMatrix()
Definition BodyNode.cpp:2112
const Eigen::Matrix6d & getSpatialInertia() const
Return spatial inertia.
Definition BodyNode.cpp:521
virtual void aggregateAugMassMatrix(Eigen::MatrixXd &_MCol, std::size_t _col, double _timeStep)
Definition BodyNode.cpp:2160
virtual void updateAccelerationID()
Update spatial body acceleration with the partial spatial body acceleration for inverse dynamics.
Definition BodyNode.cpp:1572
int mID
A unique ID of this node globally.
Definition BodyNode.hpp:1053
void addExtTorque(const Eigen::Vector3d &_torque, bool _isLocal=false)
Add applying Cartesian torque to the node.
Definition BodyNode.cpp:1196
const math::Jacobian & getWorldJacobian() const override final
Return the generalized Jacobian targeting the origin of this BodyNode.
Definition BodyNode.cpp:1102
double computePotentialEnergy(const Eigen::Vector3d &gravity) const
Return potential energy.
Definition BodyNode.cpp:1938
virtual void updateTransmittedForceFD()
Update spatial body force for forward dynamics.
Definition BodyNode.cpp:1715
Eigen::Vector6d mBiasForce
Bias force.
Definition BodyNode.hpp:1149
virtual void aggregateInvAugMassMatrix(Eigen::MatrixXd &_InvMCol, std::size_t _col, double _timeStep)
Definition BodyNode.cpp:2274
bool dependsOn(std::size_t _genCoordIndex) const override
Return true if _genCoordIndex-th generalized coordinate.
Definition BodyNode.cpp:978
void duplicateNodes(const BodyNode *otherBodyNode)
Give this BodyNode a copy of each Node from otherBodyNode.
Definition BodyNode.cpp:354
Eigen::Vector6d mImpF
Generalized impulsive body force w.r.t. body frame.
Definition BodyNode.hpp:1186
virtual double getKineticEnergy() const
Return kinetic energy.
Definition BodyNode.cpp:1917
bool mIsPartialAccelerationDirty
Is the partial acceleration vector dirty.
Definition BodyNode.hpp:1129
virtual void aggregateCombinedVector(Eigen::VectorXd &_Cg, const Eigen::Vector3d &_gravity)
Definition BodyNode.cpp:2033
std::size_t getNumChildBodyNodes() const
Return the number of child BodyNodes.
Definition BodyNode.cpp:853
std::size_t getNumDependentDofs() const override
Same as getNumDependentGenCoords()
Definition BodyNode.cpp:1006
std::size_t getIndexInSkeleton() const
Return the index of this BodyNode within its Skeleton.
Definition BodyNode.cpp:669
void updateWorldJacobian() const
Update the World Jacobian.
Definition BodyNode.cpp:2345
AllNodeProperties getAllNodeProperties() const
Get the Node::Properties of all Nodes attached to this BodyNode.
Definition BodyNode.cpp:280
Eigen::Vector3d getCOM(const Frame *_withRespectTo=Frame::World()) const
Return the center of mass with respect to an arbitrary Frame.
Definition BodyNode.cpp:585
Eigen::Vector6d mArbitrarySpatial
Cache data for arbitrary spatial value.
Definition BodyNode.hpp:1170
const math::Inertia & getArticulatedInertiaImplicit() const
Return the articulated body inertia for implicit joint damping and spring forces.
Definition BodyNode.cpp:561
std::size_t getNumShapeNodesWith() const
Return the number of ShapeNodes containing given Aspect in this BodyNode.
Definition BodyNode.hpp:201
std::size_t getTreeIndex() const
Return the index of the tree that this BodyNode belongs to.
Definition BodyNode.cpp:681
void setExtForce(const Eigen::Vector3d &_force, const Eigen::Vector3d &_offset=Eigen::Vector3d::Zero(), bool _isForceLocal=false, bool _isOffsetLocal=true)
Set Applying linear Cartesian forces to this node.
Definition BodyNode.cpp:1172
Eigen::Vector6d mFgravity
Spatial gravity force.
Definition BodyNode.hpp:1136
void dirtyArticulatedInertia()
Notify the Skeleton that the tree of this BodyNode needs an articulated inertia update.
Definition BodyNode.cpp:1514
math::Jacobian mBodyJacobian
Body Jacobian.
Definition BodyNode.hpp:1105
math::Jacobian mWorldJacobianClassicDeriv
Classic time derivative of Body Jacobian.
Definition BodyNode.hpp:1120
Eigen::Vector6d getCOMSpatialVelocity() const
Return the spatial velocity of the center of mass, expressed in coordinates of this Frame and relativ...
Definition BodyNode.cpp:598
BodyNode & operator=(const BodyNode &_otherBodyNode)
Same as copy(const BodyNode&)
Definition BodyNode.cpp:347
virtual void init(const SkeletonPtr &_skeleton)
Initialize the vector members with proper sizes.
Definition BodyNode.cpp:1299
const math::Jacobian & getJacobian() const override final
Return the generalized Jacobian targeting the origin of this BodyNode.
Definition BodyNode.cpp:1093
void dirtyExternalForces()
Tell the Skeleton that the external forces need to be updated.
Definition BodyNode.cpp:1528
Eigen::Vector6d mFext_F
Cache data for external force vector of the system.
Definition BodyNode.hpp:1159
const std::vector< DegreeOfFreedom * > & getDependentDofs() override
Return a std::vector of DegreeOfFreedom pointers that this Node depends on.
Definition BodyNode.cpp:1026
std::size_t getNumDependentGenCoords() const override
The number of the generalized coordinates which affect this JacobianNode.
Definition BodyNode.cpp:986
void processRemovedEntity(Entity *_oldChildEntity) override
Remove this Entity from mChildBodyNodes or mNonBodyNodeEntities.
Definition BodyNode.cpp:1416
void setConstraintImpulse(const Eigen::Vector6d &_constImp)
Set constraint impulse.
Definition BodyNode.cpp:1891
const Eigen::Vector6d & getConstraintImpulse() const
Return constraint impulse.
Definition BodyNode.cpp:1905
std::pair< Joint *, BodyNode * > copyTo(BodyNode *_newParent, bool _recursive=true)
Create clones of this BodyNode and all of its children recursively (unless _recursive is set to false...
Definition BodyNode.cpp:760
math::Inertia mArtInertiaImplicit
Articulated body inertia for implicit joint damping and spring forces.
Definition BodyNode.hpp:1146
const Eigen::Vector6d & getRelativeSpatialVelocity() const override
Get the spatial velocity of this Frame relative to its parent Frame, in its own coordinates.
Definition BodyNode.cpp:1066
std::size_t getDependentGenCoordIndex(std::size_t _arrayIndex) const override
Return a generalized coordinate index from the array index (< getNumDependentDofs)
Definition BodyNode.cpp:992
void setInertia(const Inertia &inertia)
Set the inertia data for this BodyNode.
Definition BodyNode.cpp:527
void setMass(double mass)
Set the mass of the bodynode.
Definition BodyNode.cpp:477
std::size_t getNumChildJoints() const
Return the number of child Joints.
Definition BodyNode.cpp:871
virtual void aggregateExternalForces(Eigen::VectorXd &_Fext)
Aggregate the external forces mFext in the generalized coordinates recursively.
Definition BodyNode.cpp:2066
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition DegreeOfFreedom.hpp:53
Definition EndEffector.hpp:84
common::Composite::MakeProperties< NameAspect, FixedFrame, EndEffector > BasicProperties
Definition EndEffector.hpp:95
Entity class is a base class for any objects that exist in the kinematic tree structure of DART.
Definition Entity.hpp:60
The Frame class serves as the backbone of DART's kinematic tree structure.
Definition Frame.hpp:57
static Frame * World()
Definition Frame.cpp:72
Definition Inertia.hpp:44
class Joint
Definition Joint.hpp:59
common::Composite::MakeProperties< NameAspect, FixedFrame, Marker > BasicProperties
Definition Marker.hpp:61
Definition Node.hpp:53
The Node class is a base class for BodyNode and any object that attaches to a BodyNode.
Definition Node.hpp:84
Definition PointMass.hpp:51
Definition BodyNodePtr.hpp:54
class Skeleton
Definition Skeleton.hpp:59
SoftBodyNode represent a soft body that has one deformable skin.
Definition SoftBodyNode.hpp:46
TemplatedJacobianNode provides a curiously recurring template pattern implementation of the various J...
Definition TemplatedJacobianNode.hpp:51
Definition Random-impl.hpp:92
Matrix< double, 6, 1 > Vector6d
Definition MathTypes.hpp:49
Matrix< double, 6, 6 > Matrix6d
Definition MathTypes.hpp:50
std::map< std::type_index, std::unique_ptr< NodeTypeStateVector > > NodeStateMap
Definition BodyNodeAspect.hpp:101
std::map< std::type_index, std::unique_ptr< NodeTypePropertiesVector > > NodePropertiesMap
Definition BodyNodeAspect.hpp:106
common::CloneableMap< NodePropertiesMap > AllNodeProperties
Definition BodyNodeAspect.hpp:107
common::CloneableMap< NodeStateMap > AllNodeStates
Definition BodyNodeAspect.hpp:102
std::shared_ptr< const Skeleton > ConstSkeletonPtr
Definition SmartPointer.hpp:60
std::shared_ptr< Shape > ShapePtr
Definition SmartPointer.hpp:81
std::shared_ptr< const Shape > ConstShapePtr
Definition SmartPointer.hpp:81
std::shared_ptr< Skeleton > SkeletonPtr
Definition SmartPointer.hpp:60
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition MathTypes.hpp:108
Eigen::Matrix6d Inertia
Definition MathTypes.hpp:105
Definition BulletCollisionDetector.cpp:63
Definition SharedLibraryManager.hpp:43
Definition BodyNodeAspect.hpp:68