DART  6.10.1
ReferentialSkeleton.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_REFERENTIALSKELETON_HPP_
34 #define DART_DYNAMICS_REFERENTIALSKELETON_HPP_
35 
36 #include <unordered_map>
37 #include <unordered_set>
38 
41 
42 namespace dart {
43 namespace dynamics {
44 
48 {
49 public:
53 
55  virtual ~ReferentialSkeleton() = default;
56 
57  // Documentation inherited
58  std::unique_ptr<common::LockableReference> getLockableReference()
59  const override;
60 
61  //----------------------------------------------------------------------------
63  //----------------------------------------------------------------------------
64 
65  // Documentation inherited
66  const std::string& setName(const std::string& _name) override;
67 
68  // Documentation inherited
69  const std::string& getName() const override;
70 
72 
73  //----------------------------------------------------------------------------
75  //----------------------------------------------------------------------------
76 
78  std::size_t getNumSkeletons() const;
79 
82  bool hasSkeleton(const Skeleton* skel) const;
83 
84  // Documentation inherited
85  std::size_t getNumBodyNodes() const override;
86 
87  // Documentation inherited
88  BodyNode* getBodyNode(std::size_t _idx) override;
89 
90  // Documentation inherited
91  const BodyNode* getBodyNode(std::size_t _idx) const override;
92 
99  BodyNode* getBodyNode(const std::string& name) override;
100 
107  const BodyNode* getBodyNode(const std::string& name) const override;
108 
109  // Documentation inherited
110  const std::vector<BodyNode*>& getBodyNodes() override;
111 
112  // Documentation inherited
113  const std::vector<const BodyNode*>& getBodyNodes() const override;
114 
119  std::vector<BodyNode*> getBodyNodes(const std::string& name) override;
120 
125  std::vector<const BodyNode*> getBodyNodes(
126  const std::string& name) const override;
127 
128  // Documentation inherited
129  bool hasBodyNode(const BodyNode* bodyNode) const override;
130 
131  // Documentation inherited
132  std::size_t getIndexOf(
133  const BodyNode* _bn, bool _warning = true) const override;
134 
135  // Documentation inherited
136  std::size_t getNumJoints() const override;
137 
138  // Documentation inherited
139  Joint* getJoint(std::size_t _idx) override;
140 
141  // Documentation inherited
142  const Joint* getJoint(std::size_t _idx) const override;
143 
150  Joint* getJoint(const std::string& name) override;
151 
158  const Joint* getJoint(const std::string& name) const override;
159 
160  // Documentation inherited
161  std::vector<Joint*> getJoints() override;
162 
163  // Documentation inherited
164  std::vector<const Joint*> getJoints() const override;
165 
170  std::vector<Joint*> getJoints(const std::string& name) override;
171 
176  std::vector<const Joint*> getJoints(const std::string& name) const override;
177 
178  // Documentation inherited
179  bool hasJoint(const Joint* joint) const override;
180 
181  // Documentation inherited
182  std::size_t getIndexOf(
183  const Joint* _joint, bool _warning = true) const override;
184 
185  // Documentation inherited
186  std::size_t getNumDofs() const override;
187 
188  // Documentation inherited
189  DegreeOfFreedom* getDof(std::size_t _idx) override;
190 
191  // Documentation inherited
192  const DegreeOfFreedom* getDof(std::size_t _idx) const override;
193 
194  // Documentation inherited
195  const std::vector<DegreeOfFreedom*>& getDofs() override;
196 
197  // Documentation inherited
198  std::vector<const DegreeOfFreedom*> getDofs() const override;
199 
200  // Documentation inherited
201  std::size_t getIndexOf(
202  const DegreeOfFreedom* _dof, bool _warning = true) const override;
203 
205 
206  //----------------------------------------------------------------------------
208  //----------------------------------------------------------------------------
209 
210  // Documentation inherited
211  math::Jacobian getJacobian(const JacobianNode* _node) const override;
212 
213  // Documentation inherited
215  const JacobianNode* _node, const Frame* _inCoordinatesOf) const override;
216 
217  // Documentation inherited
219  const JacobianNode* _node,
220  const Eigen::Vector3d& _localOffset) const override;
221 
222  // Documentation inherited
224  const JacobianNode* _node,
225  const Eigen::Vector3d& _localOffset,
226  const Frame* _inCoordinatesOf) const override;
227 
228  // Documentation inherited
229  math::Jacobian getWorldJacobian(const JacobianNode* _node) const override;
230 
231  // Documentation inherited
233  const JacobianNode* _node,
234  const Eigen::Vector3d& _localOffset) const override;
235 
236  // Documentation inherited
238  const JacobianNode* _node,
239  const Frame* _inCoordinatesOf = Frame::World()) const override;
240 
241  // Documentation inherited
243  const JacobianNode* _node,
244  const Eigen::Vector3d& _localOffset,
245  const Frame* _inCoordinatesOf = Frame::World()) const override;
246 
247  // Documentation inherited
249  const JacobianNode* _node,
250  const Frame* _inCoordinatesOf = Frame::World()) const override;
251 
252  // Documentation inherited
254  const JacobianNode* _node) const override;
255 
256  // Documentation inherited
258  const JacobianNode* _node, const Frame* _inCoordinatesOf) const override;
259 
260  // Documentation inherited
262  const JacobianNode* _node,
263  const Eigen::Vector3d& _localOffset) const override;
264 
265  // Documentation inherited
267  const JacobianNode* _node,
268  const Eigen::Vector3d& _localOffset,
269  const Frame* _inCoordinatesOf) const override;
270 
271  // Documentation inherited
273  const JacobianNode* _node) const override;
274 
275  // Documentation inherited
277  const JacobianNode* _node, const Frame* _inCoordinatesOf) const override;
278 
279  // Documentation inherited
281  const JacobianNode* _node,
282  const Eigen::Vector3d& _localOffset,
283  const Frame* _inCoordinatesOf = Frame::World()) const override;
284 
285  // Documentation inherited
287  const JacobianNode* _node,
288  const Frame* _inCoordinatesOf = Frame::World()) const override;
289 
290  // Documentation inherited
292  const JacobianNode* _node,
293  const Eigen::Vector3d& _localOffset,
294  const Frame* _inCoordinatesOf = Frame::World()) const override;
295 
296  // Documentation inherited
298  const JacobianNode* _node,
299  const Frame* _inCoordinatesOf = Frame::World()) const override;
300 
302 
303  //----------------------------------------------------------------------------
305  //----------------------------------------------------------------------------
306 
311  double getMass() const override;
312 
313  // Documentation inherited
314  const Eigen::MatrixXd& getMassMatrix() const override;
315 
316  // Documentation inherited
317  const Eigen::MatrixXd& getAugMassMatrix() const override;
318 
319  // Documentation inherited
320  const Eigen::MatrixXd& getInvMassMatrix() const override;
321 
322  // Documentation inherited
323  const Eigen::MatrixXd& getInvAugMassMatrix() const override;
324 
325  // Documentation inherited
326  const Eigen::VectorXd& getCoriolisForces() const override;
327 
328  // Documentation inherited
329  const Eigen::VectorXd& getGravityForces() const override;
330 
331  // Documentation inherited
332  const Eigen::VectorXd& getCoriolisAndGravityForces() const override;
333 
334  // Documentation inherited
335  const Eigen::VectorXd& getExternalForces() const override;
336 
337  // Documentation inherited
338  const Eigen::VectorXd& getConstraintForces() const override;
339 
340  // Documentation inherited
341  void clearExternalForces() override;
342 
343  // Documentation inherited
344  void clearInternalForces() override;
345 
346  // Documentation inherited
347  double computeKineticEnergy() const override;
348 
349  // Documentation inherited
350  double computePotentialEnergy() const override;
351 
352  // Documentation inherited
353  DART_DEPRECATED(6.0)
354  void clearCollidingBodies() override;
355 
357 
358  //----------------------------------------------------------------------------
360  //----------------------------------------------------------------------------
361 
362  // Documentation inherited
363  Eigen::Vector3d getCOM(
364  const Frame* _withRespectTo = Frame::World()) const override;
365 
366  // Documentation inherited
368  const Frame* _relativeTo = Frame::World(),
369  const Frame* _inCoordinatesOf = Frame::World()) const override;
370 
371  // Documentation inherited
372  Eigen::Vector3d getCOMLinearVelocity(
373  const Frame* _relativeTo = Frame::World(),
374  const Frame* _inCoordinatesOf = Frame::World()) const override;
375 
376  // Documentation inherited
378  const Frame* _relativeTo = Frame::World(),
379  const Frame* _inCoordinatesOf = Frame::World()) const override;
380 
381  // Documentation inherited
383  const Frame* _relativeTo = Frame::World(),
384  const Frame* _inCoordinatesOf = Frame::World()) const override;
385 
386  // Documentation inherited
387  math::Jacobian getCOMJacobian(
388  const Frame* _inCoordinatesOf = Frame::World()) const override;
389 
390  // Documentation inherited
392  const Frame* _inCoordinatesOf = Frame::World()) const override;
393 
394  // Documentation inherited
396  const Frame* _inCoordinatesOf = Frame::World()) const override;
397 
398  // Documentation inherited
400  const Frame* _inCoordinatesOf = Frame::World()) const override;
401 
403 
404 protected:
407  ReferentialSkeleton() = default;
408 
411  void registerComponent(BodyNode* _bn);
412 
415  void registerBodyNode(BodyNode* _bn);
416 
419  void registerJoint(Joint* _joint);
420 
424 
427  void unregisterComponent(BodyNode* _bn);
428 
431  void unregisterBodyNode(BodyNode* _bn, bool _unregisterDofs);
432 
435  void unregisterJoint(BodyNode* _child);
436 
439  void unregisterDegreeOfFreedom(BodyNode* _bn, std::size_t _localIndex);
440 
443  void updateCaches();
444 
446  std::weak_ptr<MetaSkeleton> mPtr;
447 
450  struct IndexMap
451  {
453  std::size_t mBodyNodeIndex;
454 
456  std::size_t mJointIndex;
457 
459  std::vector<std::size_t> mDofIndices;
460 
463  IndexMap();
464 
467  bool isExpired() const;
468  };
469 
471  std::string mName;
472 
475  std::unordered_set<const Skeleton*> mSkeletons;
476 
479  std::set<std::mutex*> mSkeletonMutexes;
480 
483  std::vector<BodyNodePtr> mBodyNodes;
484 
486  mutable std::vector<BodyNode*> mRawBodyNodes;
487 
489  mutable std::vector<const BodyNode*> mRawConstBodyNodes;
490 
492  std::vector<JointPtr> mJoints;
493 
495  std::vector<DegreeOfFreedomPtr> mDofs;
496 
498  mutable std::vector<DegreeOfFreedom*> mRawDofs;
499 
502  mutable std::vector<const DegreeOfFreedom*> mRawConstDofs;
503 
505 
508  std::unordered_map<const BodyNode*, IndexMap> mIndexMap;
509 
511  mutable Eigen::MatrixXd mM;
512 
514  mutable Eigen::MatrixXd mAugM;
515 
517  mutable Eigen::MatrixXd mInvM;
518 
520  mutable Eigen::MatrixXd mInvAugM;
521 
523  mutable Eigen::VectorXd mCvec;
524 
526  mutable Eigen::VectorXd mG;
527 
529  mutable Eigen::VectorXd mCg;
530 
532  mutable Eigen::VectorXd mFext;
533 
535  mutable Eigen::VectorXd mFc;
536 
537 private:
540  void registerSkeleton(const Skeleton* skel);
541 
544  void unregisterSkeleton(const Skeleton* skel);
545 };
546 
547 } // namespace dynamics
548 } // namespace dart
549 
550 #endif // DART_DYNAMICS_REFERENTIALSKELETON_HPP_
#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
MetaSkeleton is a pure abstract base class that provides a common interface for obtaining data (such ...
Definition: MetaSkeleton.hpp:62
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