DART  6.10.1
BodyNode.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_DYNAMICS_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 
44 #include "dart/common/Signal.hpp"
45 #include "dart/config.hpp"
46 #include "dart/dynamics/Frame.hpp"
47 #include "dart/dynamics/Node.hpp"
53 #include "dart/math/Geometry.hpp"
54 
55 namespace dart {
56 namespace dynamics {
57 
58 class GenCoord;
59 class Skeleton;
60 class Joint;
61 class DegreeOfFreedom;
62 class Shape;
63 class EndEffector;
64 class Marker;
65 
74 class BodyNode
76  public virtual BodyNodeSpecializedFor<ShapeNode, EndEffector, Marker>,
78  public TemplatedJacobianNode<BodyNode>
79 {
80 public:
82  = common::Signal<void(const BodyNode*, ConstShapePtr _newColShape)>;
83 
85 
88 
91 
94 
97 
98  BodyNode(const BodyNode&) = delete;
99 
101  virtual ~BodyNode();
102 
105  virtual SoftBodyNode* asSoftBodyNode();
106 
109  virtual const SoftBodyNode* asSoftBodyNode() const;
110 
112  void setAllNodeStates(const AllNodeStates& states);
113 
116 
119 
122 
124  void setProperties(const CompositeProperties& _properties);
125 
127  void setProperties(const AspectProperties& _properties);
128 
130  void setAspectState(const AspectState& state);
131 
134 
137 
139  void copy(const BodyNode& otherBodyNode);
140 
142  void copy(const BodyNode* otherBodyNode);
143 
145  BodyNode& operator=(const BodyNode& _otherBodyNode);
146 
148  void duplicateNodes(const BodyNode* otherBodyNode);
149 
152  void matchNodes(const BodyNode* otherBodyNode);
153 
156  const std::string& setName(const std::string& _name) override;
157 
158  // Documentation inherited
159  const std::string& getName() const override;
160 
163  void setGravityMode(bool _gravityMode);
164 
166  bool getGravityMode() const;
167 
169  bool isCollidable() const;
170 
173  void setCollidable(bool _isCollidable);
174 
176  void setMass(double mass);
177 
179  double getMass() const;
180 
185  void setMomentOfInertia(
186  double _Ixx,
187  double _Iyy,
188  double _Izz,
189  double _Ixy = 0.0,
190  double _Ixz = 0.0,
191  double _Iyz = 0.0);
192 
194  void getMomentOfInertia(
195  double& _Ixx,
196  double& _Iyy,
197  double& _Izz,
198  double& _Ixy,
199  double& _Ixz,
200  double& _Iyz) const;
201 
203  const Eigen::Matrix6d& getSpatialInertia() const;
204 
206  void setInertia(const Inertia& inertia);
207 
209  const Inertia& getInertia() const;
210 
212  const math::Inertia& getArticulatedInertia() const;
213 
217 
219  void setLocalCOM(const Eigen::Vector3d& _com);
220 
222  const Eigen::Vector3d& getLocalCOM() const;
223 
225  Eigen::Vector3d getCOM(const Frame* _withRespectTo = Frame::World()) const;
226 
229  Eigen::Vector3d getCOMLinearVelocity(
230  const Frame* _relativeTo = Frame::World(),
231  const Frame* _inCoordinatesOf = Frame::World()) const;
232 
236 
240  const Frame* _relativeTo, const Frame* _inCoordinatesOf) const;
241 
244  Eigen::Vector3d getCOMLinearAcceleration(
245  const Frame* _relativeTo = Frame::World(),
246  const Frame* _inCoordinatesOf = Frame::World()) const;
247 
251 
255  const Frame* _relativeTo, const Frame* _inCoordinatesOf) const;
256 
261  DART_DEPRECATED(6.10)
262  void setFrictionCoeff(double _coeff);
263 
268  DART_DEPRECATED(6.10)
269  double getFrictionCoeff() const;
270 
275  DART_DEPRECATED(6.10)
276  void setRestitutionCoeff(double _coeff);
277 
282  DART_DEPRECATED(6.10)
283  double getRestitutionCoeff() const;
284 
285  //--------------------------------------------------------------------------
286  // Structural Properties
287  //--------------------------------------------------------------------------
288 
290  std::size_t getIndexInSkeleton() const;
291 
293  std::size_t getIndexInTree() const;
294 
296  std::size_t getTreeIndex() const;
297 
307  SkeletonPtr remove(const std::string& _name = "temporary");
308 
318  bool moveTo(BodyNode* _newParent);
319 
325  bool moveTo(const SkeletonPtr& _newSkeleton, BodyNode* _newParent);
326 
335  template <class JointType>
336  JointType* moveTo(
337  BodyNode* _newParent,
338  const typename JointType::Properties& _joint
339  = typename JointType::Properties());
340 
344  template <class JointType>
345  JointType* moveTo(
346  const SkeletonPtr& _newSkeleton,
347  BodyNode* _newParent,
348  const typename JointType::Properties& _joint
349  = typename JointType::Properties());
350 
361  SkeletonPtr split(const std::string& _skeletonName);
362 
365  template <class JointType>
367  const std::string& _skeletonName,
368  const typename JointType::Properties& _joint
369  = typename JointType::Properties());
370 
375  template <class JointType>
377  const typename JointType::Properties& _joint
378  = typename JointType::Properties());
379 
388  std::pair<Joint*, BodyNode*> copyTo(
389  BodyNode* _newParent, bool _recursive = true);
390 
401  std::pair<Joint*, BodyNode*> copyTo(
402  const SkeletonPtr& _newSkeleton,
403  BodyNode* _newParent,
404  bool _recursive = true) const;
405 
408  template <class JointType>
409  std::pair<JointType*, BodyNode*> copyTo(
410  BodyNode* _newParent,
411  const typename JointType::Properties& _joint
412  = typename JointType::Properties(),
413  bool _recursive = true);
414 
417  template <class JointType>
418  std::pair<JointType*, BodyNode*> copyTo(
419  const SkeletonPtr& _newSkeleton,
420  BodyNode* _newParent,
421  const typename JointType::Properties& _joint
422  = typename JointType::Properties(),
423  bool _recursive = true) const;
424 
430  const std::string& _skeletonName, bool _recursive = true) const;
431 
434  template <class JointType>
436  const std::string& _skeletonName,
437  const typename JointType::Properties& _joint
438  = typename JointType::Properties(),
439  bool _recursive = true) const;
440 
441  // Documentation inherited
442  SkeletonPtr getSkeleton() override;
443 
444  // Documentation inherited
445  ConstSkeletonPtr getSkeleton() const override;
446 
449 
451  const Joint* getParentJoint() const;
452 
455 
457  const BodyNode* getParentBodyNode() const;
458 
460  template <class JointType, class NodeType = BodyNode>
461  std::pair<JointType*, NodeType*> createChildJointAndBodyNodePair(
462  const typename JointType::Properties& _jointProperties
463  = typename JointType::Properties(),
464  const typename NodeType::Properties& _bodyProperties
465  = typename NodeType::Properties());
466 
468  std::size_t getNumChildBodyNodes() const;
469 
471  BodyNode* getChildBodyNode(std::size_t _index);
472 
474  const BodyNode* getChildBodyNode(std::size_t _index) const;
475 
477  std::size_t getNumChildJoints() const;
478 
480  Joint* getChildJoint(std::size_t _index);
481 
483  const Joint* getChildJoint(std::size_t _index) const;
484 
486  template <class NodeType, typename... Args>
487  NodeType* createNode(Args&&... args);
488 
490 
496  template <class ShapeNodeProperties>
498  ShapeNodeProperties properties, bool automaticName = true);
499 
502  template <class ShapeType>
503  ShapeNode* createShapeNode(const std::shared_ptr<ShapeType>& shape);
504 
506  template <class ShapeType, class StringType>
508  const std::shared_ptr<ShapeType>& shape, StringType&& name);
509 
511  const std::vector<ShapeNode*> getShapeNodes();
512 
514  const std::vector<const ShapeNode*> getShapeNodes() const;
515 
517  void removeAllShapeNodes();
518 
521  template <class... Aspects>
522  ShapeNode* createShapeNodeWith(const ShapePtr& shape);
523 
525  template <class... Aspects>
527  const ShapePtr& shape, const std::string& name);
528 
530  template <class Aspect>
531  std::size_t getNumShapeNodesWith() const;
532 
534  template <class Aspect>
535  const std::vector<ShapeNode*> getShapeNodesWith();
536 
538  template <class Aspect>
539  const std::vector<const ShapeNode*> getShapeNodesWith() const;
540 
542  template <class Aspect>
544 
546 
550  const EndEffector::BasicProperties& _properties);
551 
553  EndEffector* createEndEffector(const std::string& _name = "EndEffector");
554 
556  EndEffector* createEndEffector(const char* _name);
557 
559 
562  const std::string& name = "marker",
563  const Eigen::Vector3d& position = Eigen::Vector3d::Zero(),
564  const Eigen::Vector4d& color = Eigen::Vector4d::Constant(1.0));
565 
567  Marker* createMarker(const Marker::BasicProperties& properties);
568 
569  // Documentation inherited
570  bool dependsOn(std::size_t _genCoordIndex) const override;
571 
572  // Documentation inherited
573  std::size_t getNumDependentGenCoords() const override;
574 
575  // Documentation inherited
576  std::size_t getDependentGenCoordIndex(std::size_t _arrayIndex) const override;
577 
578  // Documentation inherited
579  const std::vector<std::size_t>& getDependentGenCoordIndices() const override;
580 
581  // Documentation inherited
582  std::size_t getNumDependentDofs() const override;
583 
584  // Documentation inherited
585  DegreeOfFreedom* getDependentDof(std::size_t _index) override;
586 
587  // Documentation inherited
588  const DegreeOfFreedom* getDependentDof(std::size_t _index) const override;
589 
590  // Documentation inherited
591  const std::vector<DegreeOfFreedom*>& getDependentDofs() override;
592 
593  // Documentation inherited
594  const std::vector<const DegreeOfFreedom*>& getDependentDofs() const override;
595 
596  // Documentation inherited
597  const std::vector<const DegreeOfFreedom*> getChainDofs() const override;
598 
599  //--------------------------------------------------------------------------
600  // Properties updated by dynamics (kinematics)
601  //--------------------------------------------------------------------------
602 
605  const Eigen::Isometry3d& getRelativeTransform() const override;
606 
607  // Documentation inherited
608  const Eigen::Vector6d& getRelativeSpatialVelocity() const override;
609 
610  // Documentation inherited
611  const Eigen::Vector6d& getRelativeSpatialAcceleration() const override;
612 
613  // Documentation inherited
614  const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override;
615 
617  const Eigen::Vector6d& getPartialAcceleration() const override;
618 
621  const math::Jacobian& getJacobian() const override final;
622 
623  // Prevent the inherited getJacobian functions from being shadowed
625 
628  const math::Jacobian& getWorldJacobian() const override final;
629 
630  // Prevent the inherited getWorldJacobian functions from being shadowed
632 
641  const math::Jacobian& getJacobianSpatialDeriv() const override final;
642 
643  // Prevent the inherited getJacobianSpatialDeriv functions from being shadowed
645 
653  const math::Jacobian& getJacobianClassicDeriv() const override final;
654 
655  // Prevent the inherited getJacobianClassicDeriv functions from being shadowed
657 
659  const Eigen::Vector6d& getBodyVelocityChange() const;
660 
665  DART_DEPRECATED(6.0)
666  void setColliding(bool _isColliding);
667 
670  DART_DEPRECATED(6.0)
671  bool isColliding();
672 
680  void addExtForce(
681  const Eigen::Vector3d& _force,
682  const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero(),
683  bool _isForceLocal = false,
684  bool _isOffsetLocal = true);
685 
687  void setExtForce(
688  const Eigen::Vector3d& _force,
689  const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero(),
690  bool _isForceLocal = false,
691  bool _isOffsetLocal = true);
692 
696  void addExtTorque(const Eigen::Vector3d& _torque, bool _isLocal = false);
697 
701  void setExtTorque(const Eigen::Vector3d& _torque, bool _isLocal = false);
702 
707  virtual void clearExternalForces();
708 
712  virtual void clearInternalForces();
713 
715  const Eigen::Vector6d& getExternalForceLocal() const;
716 
719 
725  const Eigen::Vector6d& getBodyForce() const;
726 
727  //----------------------------------------------------------------------------
728  // Constraints
729  // - Following functions are managed by constraint solver.
730  //----------------------------------------------------------------------------
731 
736  bool isReactive() const;
737 
740  void setConstraintImpulse(const Eigen::Vector6d& _constImp);
741 
744  void addConstraintImpulse(const Eigen::Vector6d& _constImp);
745 
748  const Eigen::Vector3d& _constImp,
749  const Eigen::Vector3d& _offset,
750  bool _isImpulseLocal = false,
751  bool _isOffsetLocal = true);
752 
755  virtual void clearConstraintImpulse();
756 
758  const Eigen::Vector6d& getConstraintImpulse() const;
759 
760  //----------------------------------------------------------------------------
761  // Energies
762  //----------------------------------------------------------------------------
763 
765  double computeLagrangian(const Eigen::Vector3d& gravity) const;
766 
768  DART_DEPRECATED(6.1)
769  virtual double getKineticEnergy() const;
770 
772  double computeKineticEnergy() const;
773 
775  DART_DEPRECATED(6.1)
776  virtual double getPotentialEnergy(const Eigen::Vector3d& _gravity) const;
777 
779  double computePotentialEnergy(const Eigen::Vector3d& gravity) const;
780 
782  Eigen::Vector3d getLinearMomentum() const;
783 
785  Eigen::Vector3d getAngularMomentum(
786  const Eigen::Vector3d& _pivot = Eigen::Vector3d::Zero());
787 
788  //----------------------------------------------------------------------------
789  // Notifications
790  //----------------------------------------------------------------------------
791 
792  // Documentation inherited
793  void dirtyTransform() override;
794 
795  // Documentation inherited
796  void dirtyVelocity() override;
797 
798  // Documentation inherited
799  void dirtyAcceleration() override;
800 
803  DART_DEPRECATED(6.2)
805 
809 
811  DART_DEPRECATED(6.2)
813 
815  void dirtyExternalForces();
816 
818  DART_DEPRECATED(6.2)
819  void notifyCoriolisUpdate();
820 
822  void dirtyCoriolisForces();
823 
824  //----------------------------------------------------------------------------
825  // Friendship
826  //----------------------------------------------------------------------------
827 
828  friend class Skeleton;
829  friend class Joint;
830  friend class EndEffector;
831  friend class SoftBodyNode;
832  friend class PointMass;
833  friend class Node;
834 
835 protected:
837  BodyNode(
838  BodyNode* _parentBodyNode,
839  Joint* _parentJoint,
840  const Properties& _properties);
841 
843  BodyNode(const std::tuple<BodyNode*, Joint*, Properties>& args);
844 
847  virtual BodyNode* clone(
848  BodyNode* _parentBodyNode, Joint* _parentJoint, bool cloneNodes) const;
849 
851  Node* cloneNode(BodyNode* bn) const override final;
852 
854  virtual void init(const SkeletonPtr& _skeleton);
855 
857  void addChildBodyNode(BodyNode* _body);
858 
859  //----------------------------------------------------------------------------
861  //----------------------------------------------------------------------------
862 
865  void processNewEntity(Entity* _newChildEntity) override;
866 
868  void processRemovedEntity(Entity* _oldChildEntity) override;
869 
871  virtual void updateTransform();
872 
874  virtual void updateVelocity();
875 
877  virtual void updatePartialAcceleration() const;
878 
881  virtual void updateArtInertia(double _timeStep) const;
882 
887  virtual void updateBiasForce(
888  const Eigen::Vector3d& _gravity, double _timeStep);
889 
892  virtual void updateBiasImpulse();
893 
896  virtual void updateAccelerationID();
897 
899  virtual void updateAccelerationFD();
900 
902  virtual void updateVelocityChangeFD();
903 
909  virtual void updateTransmittedForceID(
910  const Eigen::Vector3d& _gravity, bool _withExternalForces = false);
911 
917  virtual void updateTransmittedForceFD();
918 
924  virtual void updateTransmittedImpulse();
925  // TODO: Rename to updateTransmittedImpulseFD if impulse-based inverse
926  // dynamics is implemented.
927 
929  virtual void updateJointForceID(
930  double _timeStep, bool _withDampingForces, bool _withSpringForces);
931 
933  virtual void updateJointForceFD(
934  double _timeStep, bool _withDampingForces, bool _withSpringForces);
935 
937  virtual void updateJointImpulseFD();
938 
941  virtual void updateConstrainedTerms(double _timeStep);
942 
944 
945  //----------------------------------------------------------------------------
947  //----------------------------------------------------------------------------
948 
950  virtual void updateMassMatrix();
951  virtual void aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col);
952  virtual void aggregateAugMassMatrix(
953  Eigen::MatrixXd& _MCol, std::size_t _col, double _timeStep);
954 
956  virtual void updateInvMassMatrix();
957  virtual void updateInvAugMassMatrix();
958  virtual void aggregateInvMassMatrix(
959  Eigen::MatrixXd& _InvMCol, std::size_t _col);
960  virtual void aggregateInvAugMassMatrix(
961  Eigen::MatrixXd& _InvMCol, std::size_t _col, double _timeStep);
962 
964  virtual void aggregateCoriolisForceVector(Eigen::VectorXd& _C);
965 
967  virtual void aggregateGravityForceVector(
968  Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity);
969 
971  virtual void updateCombinedVector();
972  virtual void aggregateCombinedVector(
973  Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity);
974 
977  virtual void aggregateExternalForces(Eigen::VectorXd& _Fext);
978 
980  virtual void aggregateSpatialToGeneralized(
981  Eigen::VectorXd& _generalized, const Eigen::Vector6d& _spatial);
982 
985  void updateBodyJacobian() const;
986 
989  void updateWorldJacobian() const;
990 
994  void updateBodyJacobianSpatialDeriv() const;
995 
999  void updateWorldJacobianClassicDeriv() const;
1000 
1002 
1003 protected:
1004  //--------------------------------------------------------------------------
1005  // General properties
1006  //--------------------------------------------------------------------------
1007 
1009  int mID;
1010 
1012  static std::size_t msBodyNodeCount;
1013 
1017 
1018  //--------------------------------------------------------------------------
1019  // Structural Properties
1020  //--------------------------------------------------------------------------
1021 
1024 
1027 
1029  std::size_t mTreeIndex;
1030 
1033 
1036 
1039 
1043 
1046 
1050 
1053 
1054  //--------------------------------------------------------------------------
1055  // Dynamical Properties
1056  //--------------------------------------------------------------------------
1057 
1061  mutable math::Jacobian mBodyJacobian;
1062 
1066  mutable math::Jacobian mWorldJacobian;
1067 
1072 
1077 
1082  // TODO(JS): Rename with more informative name
1083 
1086 
1090 
1093 
1097  mutable math::Inertia mArtInertia;
1098 
1103 
1106 
1110 
1113 
1116 
1120 
1124 
1127 
1128  //------------------------- Impulse-based Dyanmics ---------------------------
1132 
1136 
1139 
1140  // TODO(JS): rename with more informative one
1143 
1146 
1149 
1152 
1153 public:
1154  // To get byte-aligned Eigen vectors
1155  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1156 
1157  //----------------------------------------------------------------------------
1159  //----------------------------------------------------------------------------
1160 
1162  common::SlotRegister<ColShapeAddedSignal> onColShapeAdded;
1163 
1166 
1169  mutable common::SlotRegister<StructuralChangeSignal> onStructuralChange;
1170 
1172 
1173 private:
1177 };
1179 
1180 } // namespace dynamics
1181 } // namespace dart
1182 
1183 #include "dart/dynamics/detail/BodyNode.hpp"
1184 
1185 #endif // DART_DYNAMICS_BODYNODE_HPP_
#define DART_BAKE_SPECIALIZED_NODE_DECLARATIONS(AspectName)
Definition: BasicNodeManager.hpp:331
#define DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END
Definition: ClassWithVirtualBase.hpp:44
#define DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
Definition: ClassWithVirtualBase.hpp:43
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
BodyPropPtr properties
Definition: SdfParser.cpp:80
std::string * name
Definition: SkelParser.cpp:1697
Eigen::VectorXd position
Definition: SkelParser.cpp:108
MapHolder is a templated wrapper class that is used to allow maps of Aspect::State and Aspect::Proper...
Definition: Cloneable.hpp:221
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:435
typename Impl::AspectState AspectState
Definition: EmbeddedAspect.hpp:440
typename Impl::Aspect Aspect
Definition: EmbeddedAspect.hpp:443
typename Impl::AspectProperties AspectProperties
Definition: EmbeddedAspect.hpp:442
Definition: CompositeData.hpp:186
Definition: CompositeData.hpp:107
Declaration of the variadic template.
Definition: SpecializedNodeManager.hpp:50
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:79
void updateBodyJacobian() const
Update body Jacobian.
Definition: BodyNode.cpp:2397
void addConstraintImpulse(const Eigen::Vector6d &_constImp)
Add constraint impulse.
Definition: BodyNode.cpp:1975
BodyNode * getChildBodyNode(std::size_t _index)
Return the _index-th child BodyNode of this BodyNode.
Definition: BodyNode.cpp:919
std::set< Entity * > mNonBodyNodeEntities
Array of child Entities that are not BodyNodes.
Definition: BodyNode.hpp:1042
Node * cloneNode(BodyNode *bn) const override final
This is needed in order to inherit the Node class, but it does nothing.
Definition: BodyNode.cpp:1361
void dirtyCoriolisForces()
Tell the Skeleton that the coriolis forces need to be update.
Definition: BodyNode.cpp:1613
const std::string & getName() const override
Get the name of this Node.
Definition: BodyNode.cpp:456
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:627
detail::NodeStateMap NodeStateMap
Definition: BodyNode.hpp:90
BodyNode(const BodyNode &)=delete
void removeAllShapeNodesWith()
Remove all ShapeNodes containing given Aspect from this BodyNode.
Definition: BodyNode.hpp:259
void addChildBodyNode(BodyNode *_body)
Add a child bodynode into the bodynode.
Definition: BodyNode.cpp:894
virtual ~BodyNode()
Destructor.
Definition: BodyNode.cpp:262
bool mIsColliding
Whether the node is currently in collision with another node.
Definition: BodyNode.hpp:1016
common::SlotRegister< ColShapeRemovedSignal > onColShapeRemoved
Slot register for collision shape removed signal.
Definition: BodyNode.hpp:1165
void notifyArticulatedInertiaUpdate()
Notify the Skeleton that the tree of this BodyNode needs an articulated inertia update.
Definition: BodyNode.cpp:1581
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:1174
virtual void updateInvAugMassMatrix()
Definition: BodyNode.cpp:2313
virtual void aggregateInvMassMatrix(Eigen::MatrixXd &_InvMCol, std::size_t _col)
Definition: BodyNode.cpp:2335
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:1019
math::Jacobian mBodyJacobianSpatialDeriv
Spatial time derivative of body Jacobian.
Definition: BodyNode.hpp:1071
Joint * getChildJoint(std::size_t _index)
Return the _index-th child Joint of this BodyNode.
Definition: BodyNode.cpp:937
Eigen::Vector6d mPartialAcceleration
Partial spatial body acceleration due to parent joint's velocity.
Definition: BodyNode.hpp:1081
Joint * getParentJoint()
Return the parent Joint of this BodyNode.
Definition: BodyNode.cpp:870
const Eigen::Vector3d & getLocalCOM() const
Return center of mass expressed in body frame.
Definition: BodyNode.cpp:615
double getFrictionCoeff() const
Return frictional coefficient.
Definition: BodyNode.cpp:690
virtual void updateConstrainedTerms(double _timeStep)
Update constrained terms due to the constraint impulses for foward dynamics.
Definition: BodyNode.cpp:1886
Eigen::Vector6d mF
Transmitted wrench from parent to the bodynode expressed in body-fixed frame.
Definition: BodyNode.hpp:1089
void setExtTorque(const Eigen::Vector3d &_torque, bool _isLocal=false)
Set applying Cartesian torque to the node.
Definition: BodyNode.cpp:1274
void setProperties(const CompositeProperties &_properties)
Same as setCompositeProperties()
Definition: BodyNode.cpp:318
void notifyCoriolisUpdate()
Tell the Skeleton that the coriolis forces need to be update.
Definition: BodyNode.cpp:1607
void dirtyAcceleration() override
Notify the acceleration updates of this Frame and all its children are needed.
Definition: BodyNode.cpp:1565
virtual void updateBiasForce(const Eigen::Vector3d &_gravity, double _timeStep)
Update bias force associated with the articulated body inertia for forward dynamics.
Definition: BodyNode.cpp:1727
Properties getBodyNodeProperties() const
Get the Properties of this BodyNode.
Definition: BodyNode.cpp:352
Eigen::Vector6d mM_F
Definition: BodyNode.hpp:1119
bool isReactive() const
Return true if the body can react to force or constraint impulse.
Definition: BodyNode.cpp:2037
bool isColliding()
Return whether this body node is set to be colliding with other objects.
Definition: BodyNode.cpp:1204
const Eigen::Vector6d & getBodyForce() const
Get spatial body force transmitted from the parent joint.
Definition: BodyNode.cpp:1962
BodyNode * mParentBodyNode
Parent body node.
Definition: BodyNode.hpp:1035
ColShapeRemovedSignal mColShapeRemovedSignal
Collision shape removed signal.
Definition: BodyNode.hpp:1148
virtual double getPotentialEnergy(const Eigen::Vector3d &_gravity) const
Return potential energy.
Definition: BodyNode.cpp:2009
virtual void clearExternalForces()
Clean up structures that store external forces: mContacts, mFext, mExtForceBody and mExtTorqueBody.
Definition: BodyNode.cpp:1898
virtual void updateAccelerationFD()
Update spatial body acceleration for forward dynamics.
Definition: BodyNode.cpp:1810
void matchNodes(const BodyNode *otherBodyNode)
Make the Nodes of this BodyNode match the Nodes of otherBodyNode.
Definition: BodyNode.cpp:402
virtual void clearConstraintImpulse()
Clear constraint impulses and cache data used for impulse-based forward dynamics algorithm.
Definition: BodyNode.cpp:1949
ShapeNode * createShapeNode(ShapeNodeProperties properties, bool automaticName=true)
Create an ShapeNode attached to this BodyNode.
Definition: BodyNode.hpp:149
SkeletonPtr getSkeleton() override
Return the Skeleton that this Node is attached to.
Definition: BodyNode.cpp:858
SkeletonPtr remove(const std::string &_name="temporary")
Remove this BodyNode and all of its children (recursively) from their Skeleton.
Definition: BodyNode.cpp:779
Joint * mParentJoint
Parent joint.
Definition: BodyNode.hpp:1032
Eigen::Vector6d mInvM_U
Definition: BodyNode.hpp:1123
virtual void clearInternalForces()
Clear out the generalized forces of the parent Joint and any other forces related to this BodyNode th...
Definition: BodyNode.cpp:1905
virtual void updateVelocityChangeFD()
Update spatical body velocity change for impluse-based forward dynamics.
Definition: BodyNode.cpp:1831
void updateBodyJacobianSpatialDeriv() const
Update spatial time derivative of body Jacobian.
Definition: BodyNode.cpp:2446
const Inertia & getInertia() const
Get the inertia data for this BodyNode.
Definition: BodyNode.cpp:581
std::vector< DegreeOfFreedom * > mDependentDofs
A version of mDependentGenCoordIndices that holds DegreeOfFreedom pointers instead of indices.
Definition: BodyNode.hpp:1049
Eigen::Vector6d mCg_dV
Cache data for combined vector of the system.
Definition: BodyNode.hpp:1108
virtual void updateTransmittedForceID(const Eigen::Vector3d &_gravity, bool _withExternalForces=false)
Update spatial body force for inverse dynamics.
Definition: BodyNode.cpp:1654
double getMass() const
Return the mass of the bodynode.
Definition: BodyNode.cpp:519
virtual void aggregateSpatialToGeneralized(Eigen::VectorXd &_generalized, const Eigen::Vector6d &_spatial)
Definition: BodyNode.cpp:2170
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:1210
virtual void updateCombinedVector()
Definition: BodyNode.cpp:2098
NodeType * createNode(Args &&... args)
Create some Node type and attach it to this BodyNode.
Definition: BodyNode.hpp:139
std::size_t getIndexInTree() const
Return the index of this BodyNode within its tree.
Definition: BodyNode.cpp:732
const std::vector< ShapeNode * > getShapeNodesWith()
Return the list of ShapeNodes containing given Aspect.
Definition: BodyNode.hpp:221
bool isCollidable() const
Return true if this body can collide with others bodies.
Definition: BodyNode.cpp:482
common::SlotRegister< ColShapeAddedSignal > onColShapeAdded
Slot register for collision shape added signal.
Definition: BodyNode.hpp:1162
DegreeOfFreedom * getDependentDof(std::size_t _index) override
Get a pointer to the _indexth dependent DegreeOfFreedom for this BodyNode.
Definition: BodyNode.cpp:1074
JointType * changeParentJointType(const typename JointType::Properties &_joint=typename JointType::Properties())
Change the Joint type of this BodyNode's parent Joint.
Definition: BodyNode.hpp:81
std::vector< BodyNode * > mChildBodyNodes
Array of child body nodes.
Definition: BodyNode.hpp:1038
const Eigen::Vector6d & getPartialAcceleration() const override
Return the partial acceleration of this body.
Definition: BodyNode.cpp:1147
static std::size_t msBodyNodeCount
Counts the number of nodes globally.
Definition: BodyNode.hpp:1012
const std::vector< std::size_t > & getDependentGenCoordIndices() const override
Indices of the generalized coordinates which affect this JacobianNode.
Definition: BodyNode.cpp:1062
Eigen::Vector6d mConstraintImpulse
Constraint impulse: contact impulse, dynamic joint impulse.
Definition: BodyNode.hpp:1138
double getRestitutionCoeff() const
Return coefficient of restitution.
Definition: BodyNode.cpp:720
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:129
virtual void updateTransform()
Update transformation.
Definition: BodyNode.cpp:1620
double computeKineticEnergy() const
Return kinetic energy.
Definition: BodyNode.cpp:2000
void copy(const BodyNode &otherBodyNode)
Copy the Properties of another BodyNode.
Definition: BodyNode.cpp:358
virtual void updateBiasImpulse()
Update bias impulse associated with the articulated body inertia for impulse-based forward dynamics.
Definition: BodyNode.cpp:1768
virtual void updateTransmittedImpulse()
Update spatial body force for impulse-based forward dynamics.
Definition: BodyNode.cpp:1801
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:1123
void setAspectState(const AspectState &state)
Set the AspectState of this BodyNode.
Definition: BodyNode.cpp:330
Eigen::Vector6d getExternalForceGlobal() const
Definition: BodyNode.cpp:1917
virtual void updatePartialAcceleration() const
Update partial spatial body acceleration due to parent joint's velocity.
Definition: BodyNode.cpp:1636
math::Inertia mArtInertia
Articulated body inertia.
Definition: BodyNode.hpp:1097
StructuralChangeSignal mStructuralChangeSignal
Structural change signal.
Definition: BodyNode.hpp:1151
Eigen::Vector6d mG_F
Cache data for gravity force vector of the system.
Definition: BodyNode.hpp:1112
const Eigen::Vector6d & getExternalForceLocal() const
Definition: BodyNode.cpp:1911
double computeLagrangian(const Eigen::Vector3d &gravity) const
Return Lagrangian of this body.
Definition: BodyNode.cpp:1988
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:809
void setAspectProperties(const AspectProperties &properties)
Set the AspectProperties of this BodyNode.
Definition: BodyNode.cpp:340
std::size_t mTreeIndex
Index of this BodyNode's tree.
Definition: BodyNode.hpp:1029
void setLocalCOM(const Eigen::Vector3d &_com)
Set center of mass expressed in body frame.
Definition: BodyNode.cpp:607
virtual void updateJointImpulseFD()
Update the joint impulse for forward dynamics.
Definition: BodyNode.cpp:1879
void processNewEntity(Entity *_newChildEntity) override
Separate generic child Entities from child BodyNodes for more efficient update notices.
Definition: BodyNode.cpp:1442
virtual BodyNode * clone(BodyNode *_parentBodyNode, Joint *_parentJoint, bool cloneNodes) const
Create a clone of this BodyNode.
Definition: BodyNode.cpp:1346
void setGravityMode(bool _gravityMode)
Set whether gravity affects this body.
Definition: BodyNode.cpp:462
AllNodeStates getAllNodeStates() const
Get the Node::State of all Nodes attached to this BodyNode.
Definition: BodyNode.cpp:291
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:847
bool moveTo(BodyNode *_newParent)
Remove this BodyNode and all of its children (recursively) from their current parent BodyNode,...
Definition: BodyNode.cpp:785
void setFrictionCoeff(double _coeff)
Set coefficient of friction in range of [0, ~].
Definition: BodyNode.cpp:667
void setCollidable(bool _isCollidable)
Set whether this body node will collide with others in the world.
Definition: BodyNode.cpp:488
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:539
bool getGravityMode() const
Return true if gravity mode is enabled.
Definition: BodyNode.cpp:476
void setColliding(bool _isColliding)
Set whether this body node is colliding with other objects.
Definition: BodyNode.cpp:1198
void setRestitutionCoeff(double _coeff)
Set coefficient of restitution in range of [0, 1].
Definition: BodyNode.cpp:696
std::vector< std::size_t > mDependentGenCoordIndices
A increasingly sorted list of dependent dof indices.
Definition: BodyNode.hpp:1045
const math::Inertia & getArticulatedInertia() const
Return the articulated body inertia.
Definition: BodyNode.cpp:587
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:1100
common::SlotRegister< StructuralChangeSignal > onStructuralChange
Raised when (1) parent BodyNode is changed, (2) moved between Skeletons, (3) parent Joint is changed.
Definition: BodyNode.hpp:1169
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:525
Eigen::Vector3d getLinearMomentum() const
Return linear momentum.
Definition: BodyNode.cpp:2021
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:1183
ShapeNode * createShapeNodeWith(const ShapePtr &shape)
Create a ShapeNode with the specified Aspects and an automatically assigned name: <BodyNodeName>Shape...
Definition: BodyNode.hpp:185
virtual SoftBodyNode * asSoftBodyNode()
Convert 'this' into a SoftBodyNode pointer if this BodyNode is a SoftBodyNode, otherwise return nullp...
Definition: BodyNode.cpp:272
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:647
virtual void aggregateCoriolisForceVector(Eigen::VectorXd &_C)
Definition: BodyNode.cpp:2064
std::vector< const DegreeOfFreedom * > mConstDependentDofs
Same as mDependentDofs, but holds const pointers.
Definition: BodyNode.hpp:1052
virtual void updateInvMassMatrix()
Definition: BodyNode.cpp:2291
virtual void updateArtInertia(double _timeStep) const
Update articulated body inertia for forward dynamics.
Definition: BodyNode.cpp:1697
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:1141
const std::vector< ShapeNode * > getShapeNodes()
Return the list of ShapeNodes.
Definition: BodyNode.cpp:957
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:1135
BodyNode * getParentBodyNode()
Return the parent BodyNdoe of this BodyNode.
Definition: BodyNode.cpp:882
Eigen::Vector6d mInvM_c
Cache data for inverse mass matrix of the system.
Definition: BodyNode.hpp:1122
math::Jacobian mWorldJacobian
Cached World Jacobian.
Definition: BodyNode.hpp:1066
virtual void updateJointForceFD(double _timeStep, bool _withDampingForces, bool _withSpringForces)
Update the joint force for forward dynamics.
Definition: BodyNode.cpp:1870
void removeAllShapeNodes()
Remove all ShapeNodes from this BodyNode.
Definition: BodyNode.cpp:983
Eigen::Vector6d mM_dV
Cache data for mass matrix of the system.
Definition: BodyNode.hpp:1118
detail::NodePropertiesMap NodePropertiesMap
Definition: BodyNode.hpp:93
virtual void updateJointForceID(double _timeStep, bool _withDampingForces, bool _withSpringForces)
Update the joint force for inverse dynamics.
Definition: BodyNode.cpp:1861
std::size_t mIndexInSkeleton
Index of this BodyNode in its Skeleton.
Definition: BodyNode.hpp:1023
virtual void aggregateMassMatrix(Eigen::MatrixXd &_MCol, std::size_t _col)
Definition: BodyNode.cpp:2213
Eigen::Vector6d mBiasImpulse
Impulsive bias force due to external impulsive force exerted on bodies of the parent skeleton.
Definition: BodyNode.hpp:1135
virtual void updateVelocity()
Update spatial body velocity.
Definition: BodyNode.cpp:1628
void dirtyVelocity() override
Notify the velocity updates of this Frame and all its children are needed.
Definition: BodyNode.cpp:1538
virtual void aggregateGravityForceVector(Eigen::VectorXd &_g, const Eigen::Vector3d &_gravity)
Definition: BodyNode.cpp:2070
EndEffector * createEndEffector(const EndEffector::BasicProperties &_properties)
Create an EndEffector attached to this BodyNode.
Definition: BodyNode.cpp:994
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:1176
Eigen::Vector6d mDelV
Velocity change due to to external impulsive force exerted on bodies of the parent skeleton.
Definition: BodyNode.hpp:1131
void updateWorldJacobianClassicDeriv() const
Update classic time derivative of body Jacobian.
Definition: BodyNode.cpp:2492
void notifyExternalForcesUpdate()
Tell the Skeleton that the external forces need to be updated.
Definition: BodyNode.cpp:1595
void dirtyTransform() override
Notify the transformation updates of this Frame and all its children are needed.
Definition: BodyNode.cpp:1506
const Eigen::Vector6d & getBodyVelocityChange() const
Return the velocity change due to the constraint impulse.
Definition: BodyNode.cpp:1192
Eigen::Vector6d getCOMSpatialAcceleration() const
Return the acceleration of the center of mass expressed in coordinates of this BodyNode Frame and rel...
Definition: BodyNode.cpp:654
void setAllNodeStates(const AllNodeStates &states)
Set the Node::State of all Nodes attached to this BodyNode.
Definition: BodyNode.cpp:284
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:300
ColShapeAddedSignal mColShapeAddedSignal
Collision shape added signal.
Definition: BodyNode.hpp:1145
Eigen::Vector3d getAngularMomentum(const Eigen::Vector3d &_pivot=Eigen::Vector3d::Zero())
Return angular momentum.
Definition: BodyNode.cpp:2028
std::size_t mIndexInTree
Index of this BodyNode in its Tree.
Definition: BodyNode.hpp:1026
Eigen::Vector6d mCg_F
Definition: BodyNode.hpp:1109
virtual void updateMassMatrix()
Definition: BodyNode.cpp:2196
const Eigen::Matrix6d & getSpatialInertia() const
Return spatial inertia.
Definition: BodyNode.cpp:557
virtual void aggregateAugMassMatrix(Eigen::MatrixXd &_MCol, std::size_t _col, double _timeStep)
Definition: BodyNode.cpp:2245
virtual void updateAccelerationID()
Update spatial body acceleration with the partial spatial body acceleration for inverse dynamics.
Definition: BodyNode.cpp:1645
int mID
A unique ID of this node globally.
Definition: BodyNode.hpp:1009
void addExtTorque(const Eigen::Vector3d &_torque, bool _isLocal=false)
Add applying Cartesian torque to the node.
Definition: BodyNode.cpp:1262
const math::Jacobian & getWorldJacobian() const override final
Return the generalized Jacobian targeting the origin of this BodyNode.
Definition: BodyNode.cpp:1165
double computePotentialEnergy(const Eigen::Vector3d &gravity) const
Return potential energy.
Definition: BodyNode.cpp:2015
virtual void updateTransmittedForceFD()
Update spatial body force for forward dynamics.
Definition: BodyNode.cpp:1792
Eigen::Vector6d mBiasForce
Bias force.
Definition: BodyNode.hpp:1105
virtual void aggregateInvAugMassMatrix(Eigen::MatrixXd &_InvMCol, std::size_t _col, double _timeStep)
Definition: BodyNode.cpp:2363
bool dependsOn(std::size_t _genCoordIndex) const override
Return true if _genCoordIndex-th generalized coordinate.
Definition: BodyNode.cpp:1039
void duplicateNodes(const BodyNode *otherBodyNode)
Give this BodyNode a copy of each Node from otherBodyNode.
Definition: BodyNode.cpp:383
Eigen::Vector6d mImpF
Generalized impulsive body force w.r.t. body frame.
Definition: BodyNode.hpp:1142
virtual double getKineticEnergy() const
Return kinetic energy.
Definition: BodyNode.cpp:1994
bool mIsPartialAccelerationDirty
Is the partial acceleration vector dirty.
Definition: BodyNode.hpp:1085
virtual void aggregateCombinedVector(Eigen::VectorXd &_Cg, const Eigen::Vector3d &_gravity)
Definition: BodyNode.cpp:2113
std::size_t getNumChildBodyNodes() const
Return the number of child BodyNodes.
Definition: BodyNode.cpp:913
std::size_t getNumDependentDofs() const override
Same as getNumDependentGenCoords()
Definition: BodyNode.cpp:1068
std::size_t getIndexInSkeleton() const
Return the index of this BodyNode within its Skeleton.
Definition: BodyNode.cpp:726
void updateWorldJacobian() const
Update the World Jacobian.
Definition: BodyNode.cpp:2438
AllNodeProperties getAllNodeProperties() const
Get the Node::Properties of all Nodes attached to this BodyNode.
Definition: BodyNode.cpp:307
Eigen::Vector3d getCOM(const Frame *_withRespectTo=Frame::World()) const
Return the center of mass with respect to an arbitrary Frame.
Definition: BodyNode.cpp:621
Eigen::Vector6d mArbitrarySpatial
Cache data for arbitrary spatial value.
Definition: BodyNode.hpp:1126
const math::Inertia & getArticulatedInertiaImplicit() const
Return the articulated body inertia for implicit joint damping and spring forces.
Definition: BodyNode.cpp:597
std::size_t getNumShapeNodesWith() const
Return the number of ShapeNodes containing given Aspect in this BodyNode.
Definition: BodyNode.hpp:205
std::size_t getTreeIndex() const
Return the index of the tree that this BodyNode belongs to.
Definition: BodyNode.cpp:738
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:1236
Eigen::Vector6d mFgravity
Spatial gravity force.
Definition: BodyNode.hpp:1092
void dirtyArticulatedInertia()
Notify the Skeleton that the tree of this BodyNode needs an articulated inertia update.
Definition: BodyNode.cpp:1587
math::Jacobian mBodyJacobian
Body Jacobian.
Definition: BodyNode.hpp:1061
math::Jacobian mWorldJacobianClassicDeriv
Classic time derivative of Body Jacobian.
Definition: BodyNode.hpp:1076
Eigen::Vector6d getCOMSpatialVelocity() const
Return the spatial velocity of the center of mass, expressed in coordinates of this Frame and relativ...
Definition: BodyNode.cpp:634
BodyNode & operator=(const BodyNode &_otherBodyNode)
Same as copy(const BodyNode&)
Definition: BodyNode.cpp:376
virtual void init(const SkeletonPtr &_skeleton)
Initialize the vector members with proper sizes.
Definition: BodyNode.cpp:1370
const math::Jacobian & getJacobian() const override final
Return the generalized Jacobian targeting the origin of this BodyNode.
Definition: BodyNode.cpp:1156
void dirtyExternalForces()
Tell the Skeleton that the external forces need to be updated.
Definition: BodyNode.cpp:1601
Eigen::Vector6d mFext_F
Cache data for external force vector of the system.
Definition: BodyNode.hpp:1115
const std::vector< DegreeOfFreedom * > & getDependentDofs() override
Return a std::vector of DegreeOfFreedom pointers that this Node depends on.
Definition: BodyNode.cpp:1088
std::size_t getNumDependentGenCoords() const override
The number of the generalized coordinates which affect this JacobianNode.
Definition: BodyNode.cpp:1048
void processRemovedEntity(Entity *_oldChildEntity) override
Remove this Entity from mChildBodyNodes or mNonBodyNodeEntities.
Definition: BodyNode.cpp:1487
void setConstraintImpulse(const Eigen::Vector6d &_constImp)
Set constraint impulse.
Definition: BodyNode.cpp:1968
const Eigen::Vector6d & getConstraintImpulse() const
Return constraint impulse.
Definition: BodyNode.cpp:1982
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:819
math::Inertia mArtInertiaImplicit
Articulated body inertia for implicit joint damping and spring forces.
Definition: BodyNode.hpp:1102
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:1129
std::size_t getDependentGenCoordIndex(std::size_t _arrayIndex) const override
Return a generalized coordinate index from the array index (< getNumDependentDofs)
Definition: BodyNode.cpp:1054
void setInertia(const Inertia &inertia)
Set the inertia data for this BodyNode.
Definition: BodyNode.cpp:563
void setMass(double mass)
Set the mass of the bodynode.
Definition: BodyNode.cpp:506
std::size_t getNumChildJoints() const
Return the number of child Joints.
Definition: BodyNode.cpp:931
virtual void aggregateExternalForces(Eigen::VectorXd &_Fext)
Aggregate the external forces mFext in the generalized coordinates recursively.
Definition: BodyNode.cpp:2147
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:55
Definition: EndEffector.hpp:81
Entity class is a base class for any objects that exist in the kinematic tree structure of DART.
Definition: Entity.hpp:61
The Frame class serves as the backbone of DART's kinematic tree structure.
Definition: Frame.hpp:58
static Frame * World()
Definition: Frame.cpp:73
Definition: Inertia.hpp:44
virtual const std::string & setName(const std::string &newName)=0
Set the name of this Node.
class Joint
Definition: Joint.hpp:60
Definition: Marker.hpp:50
Definition: Node.hpp:53
The Node class is a base class for BodyNode and any object that attaches to a BodyNode.
Definition: Node.hpp:81
Definition: PointMass.hpp:51
Definition: ShapeNode.hpp:49
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< NodeTypePropertiesVector > > NodePropertiesMap
Definition: BodyNodeAspect.hpp:127
common::CloneableMap< NodePropertiesMap > AllNodeProperties
Definition: BodyNodeAspect.hpp:128
common::CloneableMap< NodeStateMap > AllNodeStates
Definition: BodyNodeAspect.hpp:121
std::map< std::type_index, std::unique_ptr< NodeTypeStateVector > > NodeStateMap
Definition: BodyNodeAspect.hpp:120
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:114
Eigen::Matrix6d Inertia
Definition: MathTypes.hpp:111
JointType
Definition: Types.hpp:132
Definition: BulletCollisionDetector.cpp:65
Definition: SharedLibraryManager.hpp:46
Definition: BodyNodeAspect.hpp:68