33 #ifndef DART_DYNAMICS_REFERENTIALSKELETON_HPP_
34 #define DART_DYNAMICS_REFERENTIALSKELETON_HPP_
36 #include <unordered_map>
37 #include <unordered_set>
66 const std::string&
setName(
const std::string& _name)
override;
69 const std::string&
getName()
const override;
113 const std::vector<const BodyNode*>&
getBodyNodes()
const override;
126 const std::string&
name)
const override;
133 const BodyNode* _bn,
bool _warning =
true)
const override;
161 std::vector<Joint*>
getJoints()
override;
164 std::vector<const Joint*>
getJoints()
const override;
170 std::vector<Joint*>
getJoints(
const std::string&
name)
override;
176 std::vector<const Joint*>
getJoints(
const std::string&
name)
const override;
183 const Joint* _joint,
bool _warning =
true)
const override;
195 const std::vector<DegreeOfFreedom*>&
getDofs()
override;
198 std::vector<const DegreeOfFreedom*>
getDofs()
const override;
220 const Eigen::Vector3d& _localOffset)
const override;
225 const Eigen::Vector3d& _localOffset,
226 const Frame* _inCoordinatesOf)
const override;
234 const Eigen::Vector3d& _localOffset)
const override;
244 const Eigen::Vector3d& _localOffset,
263 const Eigen::Vector3d& _localOffset)
const override;
268 const Eigen::Vector3d& _localOffset,
269 const Frame* _inCoordinatesOf)
const override;
282 const Eigen::Vector3d& _localOffset,
293 const Eigen::Vector3d& _localOffset,
311 double getMass()
const override;
364 const
Frame* _withRespectTo =
Frame::World()) const override;
369 const
Frame* _inCoordinatesOf =
Frame::World()) const override;
374 const
Frame* _inCoordinatesOf =
Frame::World()) const override;
379 const
Frame* _inCoordinatesOf =
Frame::World()) const override;
384 const
Frame* _inCoordinatesOf =
Frame::World()) const override;
388 const
Frame* _inCoordinatesOf =
Frame::World()) const override;
392 const
Frame* _inCoordinatesOf =
Frame::World()) const override;
396 const
Frame* _inCoordinatesOf =
Frame::World()) const override;
400 const
Frame* _inCoordinatesOf =
Frame::World()) const override;
467 bool isExpired()
const;
495 std::vector<DegreeOfFreedomPtr>
mDofs;
508 std::unordered_map<const BodyNode*, IndexMap>
mIndexMap;
511 mutable Eigen::MatrixXd
mM;
526 mutable Eigen::VectorXd
mG;
529 mutable Eigen::VectorXd
mCg;
535 mutable Eigen::VectorXd
mFc;
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
std::string * name
Definition: SkelParser.cpp:1697
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:79
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:55
The Frame class serves as the backbone of DART's kinematic tree structure.
Definition: Frame.hpp:58
static Frame * World()
Definition: Frame.cpp:73
The JacobianNode class serves as a common interface for BodyNodes and EndEffectors to both be used as...
Definition: JacobianNode.hpp:55
class Joint
Definition: Joint.hpp:60
ReferentialSkeleton is a base class used to implement Linkage, Group, and other classes that are used...
Definition: ReferentialSkeleton.hpp:48
const std::string & setName(const std::string &_name) override
Set the name of this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:53
math::LinearJacobian getCOMLinearJacobian(const Frame *_inCoordinatesOf=Frame::World()) const override
Get the MetaSkeleton's COM Linear Jacobian in terms of any Frame (default is World Frame)
Definition: ReferentialSkeleton.cpp:1128
std::size_t getNumSkeletons() const
Returns number of skeletons associated with this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:71
const Eigen::VectorXd & getConstraintForces() const override
Get constraint force vector.
Definition: ReferentialSkeleton.cpp:934
math::AngularJacobian getAngularJacobianDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
Get the angular Jacobian time derivative of a BodyNode.
Definition: ReferentialSkeleton.cpp:796
Eigen::MatrixXd mAugM
Cache for Augmented Mass Matrix.
Definition: ReferentialSkeleton.hpp:514
std::vector< Joint * > getJoints() override
Returns all the joints that are held by this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:281
void clearExternalForces() override
Clear the external forces of the BodyNodes in this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:941
void updateCaches()
Update the caches to match any changes to the structure of this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1440
bool hasBodyNode(const BodyNode *bodyNode) const override
Returns whether this Skeleton contains bodyNode.
Definition: ReferentialSkeleton.cpp:190
Eigen::VectorXd mCg
Cache for combined Coriolis and gravity vector.
Definition: ReferentialSkeleton.hpp:529
std::string mName
Name of this ReferentialSkeleton.
Definition: ReferentialSkeleton.hpp:471
Eigen::Vector6d getCOMSpatialAcceleration(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM spatial acceleration in terms of any Frame (default is World Frame)
Definition: ReferentialSkeleton.cpp:1061
bool hasSkeleton(const Skeleton *skel) const
Returns whether this ReferentialSkeleton contains any BodyNode or Joint from skel.
Definition: ReferentialSkeleton.cpp:77
Joint * getJoint(std::size_t _idx) override
Get Joint whose index is _idx.
Definition: ReferentialSkeleton.cpp:236
const std::vector< DegreeOfFreedom * > & getDofs() override
Get the vector of DegreesOfFreedom for this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:389
std::set< std::mutex * > mSkeletonMutexes
Mutexes of the skeletons.
Definition: ReferentialSkeleton.hpp:479
const Eigen::VectorXd & getExternalForces() const override
Get external force vector of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:927
math::Jacobian getWorldJacobian(const JacobianNode *_node) const override
Get the spatial Jacobian targeting the origin of a BodyNode.
Definition: ReferentialSkeleton.cpp:567
std::vector< const BodyNode * > mRawConstBodyNodes
Raw const BodyNode pointers. This vector is used for the MetaSkeleton API.
Definition: ReferentialSkeleton.hpp:489
void registerDegreeOfFreedom(DegreeOfFreedom *_dof)
Add a DegreeOfFreedom to this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1239
virtual ~ReferentialSkeleton()=default
Default destructor.
const std::string & getName() const override
Get the name of this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:65
Eigen::VectorXd mCvec
Cache for Coriolis vector.
Definition: ReferentialSkeleton.hpp:523
double getMass() const override
Get the total mass of all BodyNodes in this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:803
math::Jacobian getCOMJacobian(const Frame *_inCoordinatesOf=Frame::World()) const override
Get the MetaSkeleton's COM Jacobian in terms of any Frame (default is World Frame)
Definition: ReferentialSkeleton.cpp:1119
std::vector< DegreeOfFreedomPtr > mDofs
DegreesOfFreedom that this ReferentialSkeleton references.
Definition: ReferentialSkeleton.hpp:495
void unregisterBodyNode(BodyNode *_bn, bool _unregisterDofs)
Remove a BodyNode from this ReferentialSkeleton, ignoring its parent DegreesOfFreedom.
Definition: ReferentialSkeleton.cpp:1285
std::vector< BodyNodePtr > mBodyNodes
BodyNodes that this ReferentialSkeleton references.
Definition: ReferentialSkeleton.hpp:483
const Eigen::MatrixXd & getInvMassMatrix() const override
Get inverse of Mass Matrix of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:868
Eigen::VectorXd mFext
Cache for external force vector.
Definition: ReferentialSkeleton.hpp:532
std::size_t getNumJoints() const override
Get number of Joints.
Definition: ReferentialSkeleton.cpp:230
const Eigen::MatrixXd & getAugMassMatrix() const override
Get augmented mass matrix of the skeleton.
Definition: ReferentialSkeleton.cpp:861
const Eigen::VectorXd & getCoriolisForces() const override
Get Coriolis force vector of the MetaSkeleton's BodyNodes.
Definition: ReferentialSkeleton.cpp:906
std::size_t getNumBodyNodes() const override
Get number of body nodes.
Definition: ReferentialSkeleton.cpp:83
math::Jacobian getJacobian(const JacobianNode *_node) const override
Get the spatial Jacobian targeting the origin of a BodyNode.
Definition: ReferentialSkeleton.cpp:519
Eigen::Vector3d getCOMLinearVelocity(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM linear velocity in terms of any Frame (default is World Frame)
Definition: ReferentialSkeleton.cpp:1052
void unregisterSkeleton(const Skeleton *skel)
Removes a Skeleton from this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1496
void registerComponent(BodyNode *_bn)
Add a BodyNode, along with its parent Joint and parent DegreesOfFreedom to this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1158
double computeKineticEnergy() const override
Get the kinetic energy of this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:955
Eigen::MatrixXd mM
Cache for Mass Matrix.
Definition: ReferentialSkeleton.hpp:511
std::unordered_set< const Skeleton * > mSkeletons
Skeletons that this ReferentialSkeleton contains any BodyNode or Joint from the Skeletons.
Definition: ReferentialSkeleton.hpp:475
Eigen::MatrixXd mInvAugM
Cache for inverse Augmented Mass Matrix.
Definition: ReferentialSkeleton.hpp:520
void unregisterDegreeOfFreedom(BodyNode *_bn, std::size_t _localIndex)
Remove a DegreeOfFreedom from this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1393
math::AngularJacobian getAngularJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
Get the angular Jacobian of a BodyNode.
Definition: ReferentialSkeleton.cpp:637
math::Jacobian getJacobianSpatialDeriv(const JacobianNode *_node) const override
Get the spatial Jacobian time derivative targeting the origin of a BodyNode.
Definition: ReferentialSkeleton.cpp:663
std::vector< DegreeOfFreedom * > mRawDofs
Raw DegreeOfFreedom vector. This vector is used for the MetaSkeleton API.
Definition: ReferentialSkeleton.hpp:498
void clearInternalForces() override
Clear the internal forces of the BodyNodes in this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:948
Eigen::VectorXd mFc
Cache for constraint force vector.
Definition: ReferentialSkeleton.hpp:535
ReferentialSkeleton & operator=(const ReferentialSkeleton &_other)=delete
Remove copy operator TODO(MXG): Consider allowing this.
BodyNode * getBodyNode(std::size_t _idx) override
Get BodyNode whose index is _idx.
Definition: ReferentialSkeleton.cpp:89
const Eigen::MatrixXd & getMassMatrix() const override
Get the Mass Matrix of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:855
double computePotentialEnergy() const override
Get the potential energy of this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:967
void clearCollidingBodies() override
Clear collision flags of the BodyNodes in this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:981
std::vector< const DegreeOfFreedom * > mRawConstDofs
Raw const DegreeOfFreedom vector.
Definition: ReferentialSkeleton.hpp:502
std::weak_ptr< MetaSkeleton > mPtr
Weak pointer to this Skeleton.
Definition: ReferentialSkeleton.hpp:446
void unregisterComponent(BodyNode *_bn)
Completely remove a BodyNode and its parent DegreesOfFreedom from this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1277
std::vector< BodyNode * > mRawBodyNodes
Raw BodyNode pointers. This vector is used for the MetaSkeleton API.
Definition: ReferentialSkeleton.hpp:486
std::size_t getNumDofs() const override
Return the number of degrees of freedom in this skeleton.
Definition: ReferentialSkeleton.cpp:371
void unregisterJoint(BodyNode *_child)
Remove a Joint from this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1340
bool hasJoint(const Joint *joint) const override
Returns whether this Skeleton contains join.
Definition: ReferentialSkeleton.cpp:332
Eigen::MatrixXd mInvM
Cache for inverse Mass Matrix.
Definition: ReferentialSkeleton.hpp:517
std::unordered_map< const BodyNode *, IndexMap > mIndexMap
Raw const DegreeOfFreedom. This vector is used for the MetaSkeleton API.
Definition: ReferentialSkeleton.hpp:508
math::LinearJacobian getLinearJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
Get the linear Jacobian targeting the origin of a BodyNode.
Definition: ReferentialSkeleton.cpp:601
DegreeOfFreedom * getDof(std::size_t _idx) override
Get degree of freedom (aka generalized coordinate) whose index is _idx.
Definition: ReferentialSkeleton.cpp:377
const std::vector< BodyNode * > & getBodyNodes() override
Get all the BodyNodes that are held by this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:145
const Eigen::MatrixXd & getInvAugMassMatrix() const override
Get inverse of augmented Mass Matrix of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:875
const Eigen::VectorXd & getGravityForces() const override
Get gravity force vector of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:913
void registerBodyNode(BodyNode *_bn)
Add a BodyNode to this ReferentialSkeleton, ignoring its Joint and DegreesOfFreedom.
Definition: ReferentialSkeleton.cpp:1169
math::LinearJacobian getLinearJacobianDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
of a BodyNode.
Definition: ReferentialSkeleton.cpp:758
const Eigen::VectorXd & getCoriolisAndGravityForces() const override
Get combined vector of Coriolis force and gravity force of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:920
Eigen::VectorXd mG
Cache for gravity vector.
Definition: ReferentialSkeleton.hpp:526
Eigen::Vector3d getCOMLinearAcceleration(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override
Get the MetaSkeleton's COM linear acceleration in terms of any Frame (default is World Frame)
Definition: ReferentialSkeleton.cpp:1071
void registerJoint(Joint *_joint)
Add a Joint to this Referential Skeleton, ignoring its DegreesOfFreedom.
Definition: ReferentialSkeleton.cpp:1201
std::vector< JointPtr > mJoints
Joints that this ReferentialSkeleton references.
Definition: ReferentialSkeleton.hpp:492
Eigen::Vector6d getCOMSpatialVelocity(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM spatial velocity in terms of any Frame (default is World Frame)
Definition: ReferentialSkeleton.cpp:1043
math::Jacobian getCOMJacobianSpatialDeriv(const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM Jacobian spatial time derivative in terms of any Frame (default is World Frame...
Definition: ReferentialSkeleton.cpp:1138
math::LinearJacobian getCOMLinearJacobianDeriv(const Frame *_inCoordinatesOf=Frame::World()) const override
Get the Skeleton's COM Linear Jacobian time derivative in terms of any Frame (default is World Frame)...
Definition: ReferentialSkeleton.cpp:1148
Eigen::Vector3d getCOM(const Frame *_withRespectTo=Frame::World()) const override
Get the MetaSkeleton's COM with respect to any Frame (default is World Frame)
Definition: ReferentialSkeleton.cpp:1002
void registerSkeleton(const Skeleton *skel)
Add a Skeleton to this ReferentialSkeleton, ignoring its Joint and DegreesOfFreedom.
Definition: ReferentialSkeleton.cpp:1482
std::size_t getIndexOf(const BodyNode *_bn, bool _warning=true) const override
Get the index of a specific BodyNode within this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:197
std::unique_ptr< common::LockableReference > getLockableReference() const override
Returns mutex.
Definition: ReferentialSkeleton.cpp:46
math::Jacobian getJacobianClassicDeriv(const JacobianNode *_node) const override
Get the spatial Jacobian (classical) time derivative targeting the origin of a BodyNode.
Definition: ReferentialSkeleton.cpp:713
class Skeleton
Definition: Skeleton.hpp:59
Definition: Random-impl.hpp:92
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:114
Eigen::Matrix< double, 3, Eigen::Dynamic > AngularJacobian
Definition: MathTypes.hpp:113
Eigen::Matrix< double, 3, Eigen::Dynamic > LinearJacobian
Definition: MathTypes.hpp:112
Definition: BulletCollisionDetector.cpp:65
Definition: SharedLibraryManager.hpp:46
A simple struct that contains the indexing of a BodyNode and its parent DegreesOfFreedom.
Definition: ReferentialSkeleton.hpp:451
std::size_t mBodyNodeIndex
Index of the BodyNode.
Definition: ReferentialSkeleton.hpp:453
std::size_t mJointIndex
Index of the parent Joint.
Definition: ReferentialSkeleton.hpp:456
std::vector< std::size_t > mDofIndices
Indices of the parent DegreesOfFreedom.
Definition: ReferentialSkeleton.hpp:459