DART  6.7.3
ReferentialSkeleton.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_DYNAMICS_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:
50 
54 
56  virtual ~ReferentialSkeleton() = default;
57 
58  // Documentation inherited
59  std::unique_ptr<common::LockableReference> getLockableReference() 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(const BodyNode* _bn, bool _warning=true) const override;
133 
134  // Documentation inherited
135  std::size_t getNumJoints() const override;
136 
137  // Documentation inherited
138  Joint* getJoint(std::size_t _idx) override;
139 
140  // Documentation inherited
141  const Joint* getJoint(std::size_t _idx) const override;
142 
149  Joint* getJoint(const std::string& name) override;
150 
157  const Joint* getJoint(const std::string& name) const override;
158 
159  // Documentation inherited
160  std::vector<Joint*> getJoints() override;
161 
162  // Documentation inherited
163  std::vector<const Joint*> getJoints() const override;
164 
169  std::vector<Joint*> getJoints(const std::string& name) override;
170 
175  std::vector<const Joint*> getJoints(const std::string& name) const override;
176 
177  // Documentation inherited
178  bool hasJoint(const Joint* joint) const override;
179 
180  // Documentation inherited
181  std::size_t getIndexOf(const Joint* _joint, bool _warning=true) const override;
182 
183  // Documentation inherited
184  std::size_t getNumDofs() const override;
185 
186  // Documentation inherited
187  DegreeOfFreedom* getDof(std::size_t _idx) override;
188 
189  // Documentation inherited
190  const DegreeOfFreedom* getDof(std::size_t _idx) const override;
191 
192  // Documentation inherited
193  const std::vector<DegreeOfFreedom*>& getDofs() override;
194 
195  // Documentation inherited
196  std::vector<const DegreeOfFreedom*> getDofs() const override;
197 
198  // Documentation inherited
199  std::size_t getIndexOf(const DegreeOfFreedom* _dof,
200  bool _warning=true) const override;
201 
203 
204  //----------------------------------------------------------------------------
206  //----------------------------------------------------------------------------
207 
208  // Documentation inherited
209  math::Jacobian getJacobian(const JacobianNode* _node) const override;
210 
211  // Documentation inherited
213  const JacobianNode* _node,
214  const Frame* _inCoordinatesOf) const override;
215 
216  // Documentation inherited
218  const JacobianNode* _node,
219  const Eigen::Vector3d& _localOffset) const override;
220 
221  // Documentation inherited
223  const JacobianNode* _node,
224  const Eigen::Vector3d& _localOffset,
225  const Frame* _inCoordinatesOf) const override;
226 
227  // Documentation inherited
228  math::Jacobian getWorldJacobian(const JacobianNode* _node) const override;
229 
230  // Documentation inherited
232  const Eigen::Vector3d& _localOffset) const override;
233 
234  // Documentation inherited
236  const JacobianNode* _node,
237  const Frame* _inCoordinatesOf = Frame::World()) const override;
238 
239  // Documentation inherited
241  const JacobianNode* _node,
242  const Eigen::Vector3d& _localOffset,
243  const Frame* _inCoordinatesOf = Frame::World()) const override;
244 
245  // Documentation inherited
247  const JacobianNode* _node,
248  const Frame* _inCoordinatesOf = Frame::World()) const override;
249 
250  // Documentation inherited
252  const JacobianNode* _node) const override;
253 
254  // Documentation inherited
256  const JacobianNode* _node,
257  const Frame* _inCoordinatesOf) const override;
258 
259  // Documentation inherited
261  const JacobianNode* _node,
262  const Eigen::Vector3d& _localOffset) const override;
263 
264  // Documentation inherited
266  const JacobianNode* _node,
267  const Eigen::Vector3d& _localOffset,
268  const Frame* _inCoordinatesOf) const override;
269 
270  // Documentation inherited
272  const JacobianNode* _node) const override;
273 
274  // Documentation inherited
276  const JacobianNode* _node,
277  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:
405 
408  ReferentialSkeleton() = default;
409 
412  void registerComponent(BodyNode* _bn);
413 
416  void registerBodyNode(BodyNode* _bn);
417 
420  void registerJoint(Joint* _joint);
421 
425 
428  void unregisterComponent(BodyNode* _bn);
429 
432  void unregisterBodyNode(BodyNode* _bn, bool _unregisterDofs);
433 
436  void unregisterJoint(BodyNode* _child);
437 
440  void unregisterDegreeOfFreedom(BodyNode* _bn, std::size_t _localIndex);
441 
444  void updateCaches();
445 
447  std::weak_ptr<MetaSkeleton> mPtr;
448 
451  struct IndexMap
452  {
454  std::size_t mBodyNodeIndex;
455 
457  std::size_t mJointIndex;
458 
460  std::vector<std::size_t> mDofIndices;
461 
464  IndexMap();
465 
468  bool isExpired() const;
469  };
470 
472  std::string mName;
473 
476  std::unordered_set<const Skeleton*> mSkeletons;
477 
480  std::set<std::mutex*> mSkeletonMutexes;
481 
484  std::vector<BodyNodePtr> mBodyNodes;
485 
487  mutable std::vector<BodyNode*> mRawBodyNodes;
488 
490  mutable std::vector<const BodyNode*> mRawConstBodyNodes;
491 
493  std::vector<JointPtr> mJoints;
494 
496  std::vector<DegreeOfFreedomPtr> mDofs;
497 
499  mutable std::vector<DegreeOfFreedom*> mRawDofs;
500 
503  mutable std::vector<const DegreeOfFreedom*> mRawConstDofs;
504 
506 
509  std::unordered_map<const BodyNode*, IndexMap> mIndexMap;
510 
512  mutable Eigen::MatrixXd mM;
513 
515  mutable Eigen::MatrixXd mAugM;
516 
518  mutable Eigen::MatrixXd mInvM;
519 
521  mutable Eigen::MatrixXd mInvAugM;
522 
524  mutable Eigen::VectorXd mCvec;
525 
527  mutable Eigen::VectorXd mG;
528 
530  mutable Eigen::VectorXd mCg;
531 
533  mutable Eigen::VectorXd mFext;
534 
536  mutable Eigen::VectorXd mFc;
537 
538 private:
541  void registerSkeleton(const Skeleton* skel);
542 
545  void unregisterSkeleton(const Skeleton* skel);
546 };
547 
548 } // namespace dynamics
549 } // namespace dart
550 
551 #endif // DART_DYNAMICS_REFERENTIALSKELETON_HPP_
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
std::string * name
Definition: SkelParser.cpp:1642
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:78
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:53
The Frame class serves as the backbone of DART's kinematic tree structure.
Definition: Frame.hpp:57
static Frame * World()
Definition: Frame.cpp:72
The JacobianNode class serves as a common interface for BodyNodes and EndEffectors to both be used as...
Definition: JacobianNode.hpp:54
class Joint
Definition: Joint.hpp:59
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:54
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:1111
std::size_t getNumSkeletons() const
Returns number of skeletons associated with this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:72
const Eigen::VectorXd & getConstraintForces() const override
Get constraint force vector.
Definition: ReferentialSkeleton.cpp:922
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:784
Eigen::MatrixXd mAugM
Cache for Augmented Mass Matrix.
Definition: ReferentialSkeleton.hpp:515
std::vector< Joint * > getJoints() override
Returns all the joints that are held by this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:282
void clearExternalForces() override
Clear the external forces of the BodyNodes in this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:929
void updateCaches()
Update the caches to match any changes to the structure of this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1423
bool hasBodyNode(const BodyNode *bodyNode) const override
Returns whether this Skeleton contains bodyNode.
Definition: ReferentialSkeleton.cpp:192
Eigen::VectorXd mCg
Cache for combined Coriolis and gravity vector.
Definition: ReferentialSkeleton.hpp:530
std::string mName
Name of this ReferentialSkeleton.
Definition: ReferentialSkeleton.hpp:472
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:1046
bool hasSkeleton(const Skeleton *skel) const
Returns whether this ReferentialSkeleton contains any BodyNode or Joint from skel.
Definition: ReferentialSkeleton.cpp:78
Joint * getJoint(std::size_t _idx) override
Get Joint whose index is _idx.
Definition: ReferentialSkeleton.cpp:237
const std::vector< DegreeOfFreedom * > & getDofs() override
Get the vector of DegreesOfFreedom for this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:388
std::set< std::mutex * > mSkeletonMutexes
Mutexes of the skeletons.
Definition: ReferentialSkeleton.hpp:480
const Eigen::VectorXd & getExternalForces() const override
Get external force vector of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:915
math::Jacobian getWorldJacobian(const JacobianNode *_node) const override
Get the spatial Jacobian targeting the origin of a BodyNode.
Definition: ReferentialSkeleton.cpp:563
std::vector< const BodyNode * > mRawConstBodyNodes
Raw const BodyNode pointers. This vector is used for the MetaSkeleton API.
Definition: ReferentialSkeleton.hpp:490
void registerDegreeOfFreedom(DegreeOfFreedom *_dof)
Add a DegreeOfFreedom to this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1221
virtual ~ReferentialSkeleton()=default
Default destructor.
const std::string & getName() const override
Get the name of this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:66
Eigen::VectorXd mCvec
Cache for Coriolis vector.
Definition: ReferentialSkeleton.hpp:524
double getMass() const override
Get the total mass of all BodyNodes in this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:791
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:1102
std::vector< DegreeOfFreedomPtr > mDofs
DegreesOfFreedom that this ReferentialSkeleton references.
Definition: ReferentialSkeleton.hpp:496
void unregisterBodyNode(BodyNode *_bn, bool _unregisterDofs)
Remove a BodyNode from this ReferentialSkeleton, ignoring its parent DegreesOfFreedom.
Definition: ReferentialSkeleton.cpp:1267
std::vector< BodyNodePtr > mBodyNodes
BodyNodes that this ReferentialSkeleton references.
Definition: ReferentialSkeleton.hpp:484
const Eigen::MatrixXd & getInvMassMatrix() const override
Get inverse of Mass Matrix of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:856
Eigen::VectorXd mFext
Cache for external force vector.
Definition: ReferentialSkeleton.hpp:533
std::size_t getNumJoints() const override
Get number of Joints.
Definition: ReferentialSkeleton.cpp:231
const Eigen::MatrixXd & getAugMassMatrix() const override
Get augmented mass matrix of the skeleton.
Definition: ReferentialSkeleton.cpp:849
const Eigen::VectorXd & getCoriolisForces() const override
Get Coriolis force vector of the MetaSkeleton's BodyNodes.
Definition: ReferentialSkeleton.cpp:894
std::size_t getNumBodyNodes() const override
Get number of body nodes.
Definition: ReferentialSkeleton.cpp:84
math::Jacobian getJacobian(const JacobianNode *_node) const override
Get the spatial Jacobian targeting the origin of a BodyNode.
Definition: ReferentialSkeleton.cpp:517
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:1038
void unregisterSkeleton(const Skeleton *skel)
Removes a Skeleton from this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1479
void registerComponent(BodyNode *_bn)
Add a BodyNode, along with its parent Joint and parent DegreesOfFreedom to this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1140
double computeKineticEnergy() const override
Get the kinetic energy of this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:943
Eigen::MatrixXd mM
Cache for Mass Matrix.
Definition: ReferentialSkeleton.hpp:512
std::unordered_set< const Skeleton * > mSkeletons
Skeletons that this ReferentialSkeleton contains any BodyNode or Joint from the Skeletons.
Definition: ReferentialSkeleton.hpp:476
Eigen::MatrixXd mInvAugM
Cache for inverse Augmented Mass Matrix.
Definition: ReferentialSkeleton.hpp:521
void unregisterDegreeOfFreedom(BodyNode *_bn, std::size_t _localIndex)
Remove a DegreeOfFreedom from this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1375
math::AngularJacobian getAngularJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
Get the angular Jacobian of a BodyNode.
Definition: ReferentialSkeleton.cpp:632
math::Jacobian getJacobianSpatialDeriv(const JacobianNode *_node) const override
Get the spatial Jacobian time derivative targeting the origin of a BodyNode.
Definition: ReferentialSkeleton.cpp:657
std::vector< DegreeOfFreedom * > mRawDofs
Raw DegreeOfFreedom vector. This vector is used for the MetaSkeleton API.
Definition: ReferentialSkeleton.hpp:499
void clearInternalForces() override
Clear the internal forces of the BodyNodes in this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:936
Eigen::VectorXd mFc
Cache for constraint force vector.
Definition: ReferentialSkeleton.hpp:536
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:90
const Eigen::MatrixXd & getMassMatrix() const override
Get the Mass Matrix of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:843
double computePotentialEnergy() const override
Get the potential energy of this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:955
void clearCollidingBodies() override
Clear collision flags of the BodyNodes in this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:969
std::vector< const DegreeOfFreedom * > mRawConstDofs
Raw const DegreeOfFreedom vector.
Definition: ReferentialSkeleton.hpp:503
std::weak_ptr< MetaSkeleton > mPtr
Weak pointer to this Skeleton.
Definition: ReferentialSkeleton.hpp:447
void unregisterComponent(BodyNode *_bn)
Completely remove a BodyNode and its parent DegreesOfFreedom from this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1259
std::vector< BodyNode * > mRawBodyNodes
Raw BodyNode pointers. This vector is used for the MetaSkeleton API.
Definition: ReferentialSkeleton.hpp:487
std::size_t getNumDofs() const override
Return the number of degrees of freedom in this skeleton.
Definition: ReferentialSkeleton.cpp:370
void unregisterJoint(BodyNode *_child)
Remove a Joint from this ReferentialSkeleton.
Definition: ReferentialSkeleton.cpp:1322
bool hasJoint(const Joint *joint) const override
Returns whether this Skeleton contains join.
Definition: ReferentialSkeleton.cpp:333
Eigen::MatrixXd mInvM
Cache for inverse Mass Matrix.
Definition: ReferentialSkeleton.hpp:518
std::unordered_map< const BodyNode *, IndexMap > mIndexMap
Raw const DegreeOfFreedom. This vector is used for the MetaSkeleton API.
Definition: ReferentialSkeleton.hpp:509
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:596
DegreeOfFreedom * getDof(std::size_t _idx) override
Get degree of freedom (aka generalized coordinate) whose index is _idx.
Definition: ReferentialSkeleton.cpp:376
const std::vector< BodyNode * > & getBodyNodes() override
Get all the BodyNodes that are held by this MetaSkeleton.
Definition: ReferentialSkeleton.cpp:146
const Eigen::MatrixXd & getInvAugMassMatrix() const override
Get inverse of augmented Mass Matrix of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:863
const Eigen::VectorXd & getGravityForces() const override
Get gravity force vector of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:901
void registerBodyNode(BodyNode *_bn)
Add a BodyNode to this ReferentialSkeleton, ignoring its Joint and DegreesOfFreedom.
Definition: ReferentialSkeleton.cpp:1151
math::LinearJacobian getLinearJacobianDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override
of a BodyNode.
Definition: ReferentialSkeleton.cpp:748
const Eigen::VectorXd & getCoriolisAndGravityForces() const override
Get combined vector of Coriolis force and gravity force of the MetaSkeleton.
Definition: ReferentialSkeleton.cpp:908
Eigen::VectorXd mG
Cache for gravity vector.
Definition: ReferentialSkeleton.hpp:527
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:1055
void registerJoint(Joint *_joint)
Add a Joint to this Referential Skeleton, ignoring its DegreesOfFreedom.
Definition: ReferentialSkeleton.cpp:1183
std::vector< JointPtr > mJoints
Joints that this ReferentialSkeleton references.
Definition: ReferentialSkeleton.hpp:493
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:1030
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:1120
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:1130
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:990
void registerSkeleton(const Skeleton *skel)
Add a Skeleton to this ReferentialSkeleton, ignoring its Joint and DegreesOfFreedom.
Definition: ReferentialSkeleton.cpp:1465
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:199
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:705
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:108
Eigen::Matrix< double, 3, Eigen::Dynamic > AngularJacobian
Definition: MathTypes.hpp:107
Eigen::Matrix< double, 3, Eigen::Dynamic > LinearJacobian
Definition: MathTypes.hpp:106
Definition: BulletCollisionDetector.cpp:63
Definition: SharedLibraryManager.hpp:43
A simple struct that contains the indexing of a BodyNode and its parent DegreesOfFreedom.
Definition: ReferentialSkeleton.hpp:452
std::size_t mBodyNodeIndex
Index of the BodyNode.
Definition: ReferentialSkeleton.hpp:454
std::size_t mJointIndex
Index of the parent Joint.
Definition: ReferentialSkeleton.hpp:457
std::vector< std::size_t > mDofIndices
Indices of the parent DegreesOfFreedom.
Definition: ReferentialSkeleton.hpp:460