DART  6.6.2
BodyNode.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2018, 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"
44 #include "dart/common/Signal.hpp"
46 #include "dart/math/Geometry.hpp"
47 #include "dart/dynamics/Node.hpp"
48 #include "dart/dynamics/Frame.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 
73 class BodyNode :
75  public virtual BodyNodeSpecializedFor<ShapeNode, EndEffector, Marker>,
77  public TemplatedJacobianNode<BodyNode>
78 {
79 public:
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 
186  void setMomentOfInertia(
187  double _Ixx, double _Iyy, double _Izz,
188  double _Ixy = 0.0, double _Ixz = 0.0, double _Iyz = 0.0);
189 
191  void getMomentOfInertia(
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 
205  const math::Inertia& getArticulatedInertia() const;
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 
232  Eigen::Vector6d getCOMSpatialVelocity(const Frame* _relativeTo,
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 
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
671 
674  const math::Jacobian& getWorldJacobian() const override final;
675 
676  // Prevent the inherited getWorldJacobian functions from being shadowed
678 
687  const math::Jacobian& getJacobianSpatialDeriv() const override final;
688 
689  // Prevent the inherited getJacobianSpatialDeriv functions from being shadowed
691 
699  const math::Jacobian& getJacobianClassicDeriv() const override final;
700 
701  // Prevent the inherited getJacobianClassicDeriv functions from being shadowed
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 
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)
862  void notifyCoriolisUpdate();
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 
878 protected:
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 
1042  void updateWorldJacobianClassicDeriv() const;
1043 
1045 
1046 protected:
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 
1073  std::size_t mTreeIndex;
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 
1116 
1121 
1126  // TODO(JS): Rename with more informative name
1127 
1130 
1134 
1137 
1141  mutable math::Inertia mArtInertia;
1142 
1147 
1150 
1154 
1157 
1160 
1164 
1168 
1171 
1172  //------------------------- Impulse-based Dyanmics ---------------------------
1176 
1180 
1183 
1184  // TODO(JS): rename with more informative one
1187 
1190 
1193 
1196 
1197 public:
1198  // To get byte-aligned Eigen vectors
1199  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1200 
1201  //----------------------------------------------------------------------------
1203  //----------------------------------------------------------------------------
1204 
1206  common::SlotRegister<ColShapeAddedSignal> onColShapeAdded;
1207 
1210 
1213  mutable common::SlotRegister<StructuralChangeSignal> onStructuralChange;
1214 
1216 
1217 private:
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
typename Impl::AspectProperties AspectProperties
Definition: EmbeddedAspect.hpp:446
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
Get the name of this Node.
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
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
virtual const std::string & setName(const std::string &newName)=0
Set the name of this Node.
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
friend class BodyNode
Definition: Node.hpp:87
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
TemplatedJacobianNode(BodyNode *bn)
Constructor.
Definition: TemplatedJacobianNode.hpp:311
Definition: MathTypes.hpp:47
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