DART  6.10.1
MetaSkeleton.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_METASKELETON_HPP_
34 #define DART_DYNAMICS_METASKELETON_HPP_
35 
36 #include <string>
37 #include <vector>
38 
39 #include <Eigen/Dense>
40 
42 #include "dart/common/Signal.hpp"
43 #include "dart/common/Subject.hpp"
44 #include "dart/dynamics/Frame.hpp"
46 #include "dart/math/Geometry.hpp"
47 
48 namespace dart {
49 namespace dynamics {
50 
51 class BodyNode;
52 class SoftBodyNode;
53 class PointMass;
54 class Joint;
55 class DegreeOfFreedom;
56 class Marker;
57 
62 {
63 public:
65  std::shared_ptr<const MetaSkeleton> _skeleton,
66  const std::string& _oldName,
67  const std::string& _newName)>;
68 
69  MetaSkeleton(const MetaSkeleton&) = delete;
70 
72  virtual ~MetaSkeleton() = default;
73 
76  const std::string& cloneName) const = 0;
77  // TODO: In DART7, rename this to clone() and change the current
78  // Skeleton::clone() to override it.
79 
82 
84  virtual std::unique_ptr<common::LockableReference> getLockableReference()
85  const = 0;
86  // TODO: In DART7, rename this to getMutex() and change the current
87  // Skeleton::getMutex() to override it.
88 
89  //----------------------------------------------------------------------------
91  //----------------------------------------------------------------------------
92 
94  virtual const std::string& setName(const std::string& _name) = 0;
95 
97  virtual const std::string& getName() const = 0;
98 
100 
101  //----------------------------------------------------------------------------
103  //----------------------------------------------------------------------------
104 
106  virtual std::size_t getNumBodyNodes() const = 0;
107 
109  virtual BodyNode* getBodyNode(std::size_t _idx) = 0;
110 
112  virtual const BodyNode* getBodyNode(std::size_t _idx) const = 0;
113 
118  virtual BodyNode* getBodyNode(const std::string& name) = 0;
119 
124  virtual const BodyNode* getBodyNode(const std::string& name) const = 0;
125 
127  virtual const std::vector<BodyNode*>& getBodyNodes() = 0;
128 
130  virtual const std::vector<const BodyNode*>& getBodyNodes() const = 0;
131 
135  virtual std::vector<BodyNode*> getBodyNodes(const std::string& name) = 0;
136 
140  virtual std::vector<const BodyNode*> getBodyNodes(
141  const std::string& name) const = 0;
142 
144  virtual bool hasBodyNode(const BodyNode* bodyNode) const = 0;
145 
150  virtual std::size_t getIndexOf(
151  const BodyNode* _bn, bool _warning = true) const = 0;
152 
154  virtual std::size_t getNumJoints() const = 0;
155 
157  virtual Joint* getJoint(std::size_t _idx) = 0;
158 
160  virtual const Joint* getJoint(std::size_t _idx) const = 0;
161 
165  virtual Joint* getJoint(const std::string& name) = 0;
166 
170  virtual const Joint* getJoint(const std::string& name) const = 0;
171 
173  virtual std::vector<Joint*> getJoints() = 0;
174 
176  virtual std::vector<const Joint*> getJoints() const = 0;
177 
185  virtual std::vector<Joint*> getJoints(const std::string& name) = 0;
186 
194  virtual std::vector<const Joint*> getJoints(
195  const std::string& name) const = 0;
196 
198  virtual bool hasJoint(const Joint* joint) const = 0;
199 
204  virtual std::size_t getIndexOf(
205  const Joint* _joint, bool _warning = true) const = 0;
206 
208  virtual std::size_t getNumDofs() const = 0;
209 
211  virtual DegreeOfFreedom* getDof(std::size_t _idx) = 0;
212 
214  virtual const DegreeOfFreedom* getDof(std::size_t _idx) const = 0;
215 
217  virtual const std::vector<DegreeOfFreedom*>& getDofs() = 0;
218 
220  virtual std::vector<const DegreeOfFreedom*> getDofs() const = 0;
221 
226  virtual std::size_t getIndexOf(
227  const DegreeOfFreedom* _dof, bool _warning = true) const = 0;
228 
230 
231  //----------------------------------------------------------------------------
233  //----------------------------------------------------------------------------
234 
236  void setCommand(std::size_t _index, double _command);
237 
239  double getCommand(std::size_t _index) const;
240 
242  void setCommands(const Eigen::VectorXd& _commands);
243 
245  void setCommands(
246  const std::vector<std::size_t>& _indices,
247  const Eigen::VectorXd& _commands);
248 
250  Eigen::VectorXd getCommands() const;
251 
253  Eigen::VectorXd getCommands(const std::vector<std::size_t>& _indices) const;
254 
256  void resetCommands();
257 
259 
260  //----------------------------------------------------------------------------
262  //----------------------------------------------------------------------------
263 
265  void setPosition(std::size_t index, double _position);
266 
268  double getPosition(std::size_t _index) const;
269 
271  void setPositions(const Eigen::VectorXd& _positions);
272 
274  void setPositions(
275  const std::vector<std::size_t>& _indices,
276  const Eigen::VectorXd& _positions);
277 
279  Eigen::VectorXd getPositions() const;
280 
282  Eigen::VectorXd getPositions(const std::vector<std::size_t>& _indices) const;
283 
285  void resetPositions();
286 
288  void setPositionLowerLimit(std::size_t _index, double _position);
289 
291  void setPositionLowerLimits(const Eigen::VectorXd& positions);
292 
295  const std::vector<std::size_t>& indices,
296  const Eigen::VectorXd& positions);
297 
299  double getPositionLowerLimit(std::size_t _index) const;
300 
302  Eigen::VectorXd getPositionLowerLimits() const;
303 
305  Eigen::VectorXd getPositionLowerLimits(
306  const std::vector<std::size_t>& indices) const;
307 
309  void setPositionUpperLimit(std::size_t _index, double _position);
310 
312  void setPositionUpperLimits(const Eigen::VectorXd& positions);
313 
316  const std::vector<std::size_t>& indices,
317  const Eigen::VectorXd& positions);
318 
320  double getPositionUpperLimit(std::size_t _index) const;
321 
323  Eigen::VectorXd getPositionUpperLimits() const;
324 
326  Eigen::VectorXd getPositionUpperLimits(
327  const std::vector<std::size_t>& indices) const;
328 
330 
331  //----------------------------------------------------------------------------
333  //----------------------------------------------------------------------------
334 
336  void setVelocity(std::size_t _index, double _velocity);
337 
339  double getVelocity(std::size_t _index) const;
340 
342  void setVelocities(const Eigen::VectorXd& _velocities);
343 
345  void setVelocities(
346  const std::vector<std::size_t>& _indices,
347  const Eigen::VectorXd& _velocities);
348 
350  Eigen::VectorXd getVelocities() const;
351 
353  Eigen::VectorXd getVelocities(const std::vector<std::size_t>& _indices) const;
354 
356  void resetVelocities();
357 
359  void setVelocityLowerLimit(std::size_t _index, double _velocity);
360 
362  void setVelocityLowerLimits(const Eigen::VectorXd& velocities);
363 
367  const std::vector<std::size_t>& indices,
368  const Eigen::VectorXd& velocities);
369 
371  double getVelocityLowerLimit(std::size_t _index);
372 
374  Eigen::VectorXd getVelocityLowerLimits() const;
375 
378  Eigen::VectorXd getVelocityLowerLimits(
379  const std::vector<std::size_t>& indices) const;
380 
382  void setVelocityUpperLimit(std::size_t _index, double _velocity);
383 
385  void setVelocityUpperLimits(const Eigen::VectorXd& velocities);
386 
390  const std::vector<std::size_t>& indices,
391  const Eigen::VectorXd& velocities);
392 
394  double getVelocityUpperLimit(std::size_t _index);
395 
397  Eigen::VectorXd getVelocityUpperLimits() const;
398 
401  Eigen::VectorXd getVelocityUpperLimits(
402  const std::vector<std::size_t>& indices) const;
403 
405 
406  //----------------------------------------------------------------------------
408  //----------------------------------------------------------------------------
409 
411  void setAcceleration(std::size_t _index, double _acceleration);
412 
414  double getAcceleration(std::size_t _index) const;
415 
417  void setAccelerations(const Eigen::VectorXd& _accelerations);
418 
420  void setAccelerations(
421  const std::vector<std::size_t>& _indices,
422  const Eigen::VectorXd& _accelerations);
423 
425  Eigen::VectorXd getAccelerations() const;
426 
428  Eigen::VectorXd getAccelerations(
429  const std::vector<std::size_t>& _indices) const;
430 
432  void resetAccelerations();
433 
435  void setAccelerationLowerLimit(std::size_t _index, double _acceleration);
436 
438  void setAccelerationLowerLimits(const Eigen::VectorXd& accelerations);
439 
443  const std::vector<std::size_t>& indices,
444  const Eigen::VectorXd& accelerations);
445 
447  double getAccelerationLowerLimit(std::size_t _index) const;
448 
450  Eigen::VectorXd getAccelerationLowerLimits() const;
451 
454  Eigen::VectorXd getAccelerationLowerLimits(
455  const std::vector<std::size_t>& indices) const;
456 
458  void setAccelerationUpperLimit(std::size_t _index, double _acceleration);
459 
461  void setAccelerationUpperLimits(const Eigen::VectorXd& accelerations);
462 
466  const std::vector<std::size_t>& indices,
467  const Eigen::VectorXd& accelerations);
468 
470  double getAccelerationUpperLimit(std::size_t _index) const;
471 
473  Eigen::VectorXd getAccelerationUpperLimits() const;
474 
477  Eigen::VectorXd getAccelerationUpperLimits(
478  const std::vector<std::size_t>& indices) const;
479 
481 
482  //----------------------------------------------------------------------------
484  //----------------------------------------------------------------------------
485 
487  void setForce(std::size_t _index, double _force);
488 
490  double getForce(std::size_t _index) const;
491 
493  void setForces(const Eigen::VectorXd& _forces);
494 
496  void setForces(
497  const std::vector<std::size_t>& _index, const Eigen::VectorXd& _forces);
498 
500  Eigen::VectorXd getForces() const;
501 
503  Eigen::VectorXd getForces(const std::vector<std::size_t>& _indices) const;
504 
506  void resetGeneralizedForces();
507 
509  void setForceLowerLimit(std::size_t _index, double _force);
510 
512  void setForceLowerLimits(const Eigen::VectorXd& forces);
513 
515  void setForceLowerLimits(
516  const std::vector<std::size_t>& indices, const Eigen::VectorXd& forces);
517 
519  double getForceLowerLimit(std::size_t _index) const;
520 
522  Eigen::VectorXd getForceLowerLimits() const;
523 
525  Eigen::VectorXd getForceLowerLimits(
526  const std::vector<std::size_t>& indices) const;
527 
529  void setForceUpperLimit(std::size_t _index, double _force);
530 
532  void setForceUpperLimits(const Eigen::VectorXd& forces);
533 
535  void setForceUpperLimits(
536  const std::vector<std::size_t>& indices, const Eigen::VectorXd& forces);
537 
539  double getForceUpperLimit(std::size_t _index) const;
540 
542  Eigen::VectorXd getForceUpperLimits() const;
543 
545  Eigen::VectorXd getForceUpperLimits(
546  const std::vector<std::size_t>& indices) const;
547 
549 
550  //----------------------------------------------------------------------------
552  //----------------------------------------------------------------------------
553 
555  Eigen::VectorXd getVelocityChanges() const;
556 
557  //----------------------------------------------------------------------------
559  //----------------------------------------------------------------------------
560 
562  void setJointConstraintImpulses(const Eigen::VectorXd& _impulses);
563 
565  Eigen::VectorXd getJointConstraintImpulses() const;
566 
567  //----------------------------------------------------------------------------
569  //----------------------------------------------------------------------------
570 
573  virtual math::Jacobian getJacobian(const JacobianNode* _node) const = 0;
574 
578  const JacobianNode* _node, const Frame* _inCoordinatesOf) const = 0;
579 
584  const JacobianNode* _node,
585  const JacobianNode* _relativeTo,
586  const Frame* _inCoordinatesOf) const;
587 
592  const JacobianNode* _node, const Eigen::Vector3d& _localOffset) const = 0;
593 
598  const JacobianNode* _node,
599  const Eigen::Vector3d& _localOffset,
600  const Frame* _inCoordinatesOf) const = 0;
601 
607  const JacobianNode* _node,
608  const Eigen::Vector3d& _localOffset,
609  const JacobianNode* _relativeTo,
610  const Frame* _inCoordinatesOf) const;
611 
614  virtual math::Jacobian getWorldJacobian(const JacobianNode* _node) const = 0;
615 
620  const JacobianNode* _node, const Eigen::Vector3d& _localOffset) const = 0;
621 
625  const JacobianNode* _node,
626  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
627 
632  const JacobianNode* _node,
633  const Eigen::Vector3d& _localOffset,
634  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
635 
640  const JacobianNode* _node,
641  const JacobianNode* _relativeTo,
642  const Frame* _inCoordinatesOf = Frame::World()) const;
643 
649  const JacobianNode* _node,
650  const Eigen::Vector3d& _localOffset,
651  const JacobianNode* _relativeTo,
652  const Frame* _inCoordinatesOf = Frame::World()) const;
653 
655 
656  //----------------------------------------------------------------------------
658  //----------------------------------------------------------------------------
659 
663  const JacobianNode* _node,
664  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
665 
670  const JacobianNode* _node,
671  const JacobianNode* _relativeTo,
672  const Frame* _inCoordinatesOf = Frame::World()) const;
673 
677  const JacobianNode* _node) const = 0;
678 
682  const JacobianNode* _node, const Frame* _inCoordinatesOf) const = 0;
683 
688  const JacobianNode* _node, const Eigen::Vector3d& _localOffset) const = 0;
689 
694  const JacobianNode* _node,
695  const Eigen::Vector3d& _localOffset,
696  const Frame* _inCoordinatesOf) const = 0;
697 
702  const JacobianNode* _node,
703  const JacobianNode* _relativeTo,
704  const Frame* _inCoordinatesOf) const;
705 
711  const JacobianNode* _node,
712  const Eigen::Vector3d& _localOffset,
713  const JacobianNode* _relativeTo,
714  const Frame* _inCoordinatesOf) const;
715 
719  const JacobianNode* _node) const = 0;
720 
725  const JacobianNode* _node, const Frame* _inCoordinatesOf) const = 0;
726 
731  const JacobianNode* _node,
732  const Eigen::Vector3d& _localOffset,
733  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
734 
738  const JacobianNode* _node,
739  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
740 
745  const JacobianNode* _node,
746  const Eigen::Vector3d& _localOffset,
747  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
748 
752  const JacobianNode* _node,
753  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
754 
756 
757  //----------------------------------------------------------------------------
759  //----------------------------------------------------------------------------
760 
764  virtual double getMass() const = 0;
765 
767  virtual const Eigen::MatrixXd& getMassMatrix() const = 0;
768 
773  virtual const Eigen::MatrixXd& getAugMassMatrix() const = 0;
774 
776  virtual const Eigen::MatrixXd& getInvMassMatrix() const = 0;
777 
779  virtual const Eigen::MatrixXd& getInvAugMassMatrix() const = 0;
780 
782  virtual const Eigen::VectorXd& getCoriolisForces() const = 0;
783 
785  virtual const Eigen::VectorXd& getGravityForces() const = 0;
786 
789  virtual const Eigen::VectorXd& getCoriolisAndGravityForces() const = 0;
790 
792  virtual const Eigen::VectorXd& getExternalForces() const = 0;
793 
795  virtual const Eigen::VectorXd& getConstraintForces() const = 0;
796 
798  virtual void clearExternalForces() = 0;
799 
801  virtual void clearInternalForces() = 0;
802 
804  double computeLagrangian() const;
805 
807  DART_DEPRECATED(6.1)
808  double getKineticEnergy() const;
809 
811  virtual double computeKineticEnergy() const = 0;
812 
814  DART_DEPRECATED(6.1)
815  double getPotentialEnergy() const;
816 
818  virtual double computePotentialEnergy() const = 0;
819 
821  DART_DEPRECATED(6.0)
822  virtual void clearCollidingBodies() = 0;
823 
825 
826  //----------------------------------------------------------------------------
828  //----------------------------------------------------------------------------
829 
832  virtual Eigen::Vector3d getCOM(
833  const Frame* _withRespectTo = Frame::World()) const = 0;
834 
838  const Frame* _relativeTo = Frame::World(),
839  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
840 
843  virtual Eigen::Vector3d getCOMLinearVelocity(
844  const Frame* _relativeTo = Frame::World(),
845  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
846 
850  const Frame* _relativeTo = Frame::World(),
851  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
852 
855  virtual Eigen::Vector3d getCOMLinearAcceleration(
856  const Frame* _relativeTo = Frame::World(),
857  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
858 
861  virtual math::Jacobian getCOMJacobian(
862  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
863 
867  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
868 
876  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
877 
885  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
886 
888 
889 protected:
891  MetaSkeleton();
892 
893  //--------------------------------------------------------------------------
894  // Signals
895  //--------------------------------------------------------------------------
897 
898 public:
899  //--------------------------------------------------------------------------
900  // Slot registers
901  //--------------------------------------------------------------------------
902  common::SlotRegister<NameChangedSignal> onNameChanged;
903 };
904 
905 } // namespace dynamics
906 } // namespace dart
907 
908 #endif // DART_DYNAMICS_METASKELETON_HPP_
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
std::string * name
Definition: SkelParser.cpp:1697
std::size_t index
Definition: SkelParser.cpp:1672
The Subject class is a base class for any object that wants to report when it gets destroyed.
Definition: Subject.hpp:58
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
Eigen::VectorXd getForces() const
Get the forces for all generalized coordinates.
Definition: MetaSkeleton.cpp:843
void setAccelerationLowerLimits(const Eigen::VectorXd &accelerations)
Set the lower limits for all generalized coordinates's acceleration.
Definition: MetaSkeleton.cpp:720
virtual math::Jacobian getJacobian(const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf) const =0
Get the spatial Jacobian targeting an offset in a BodyNode.
virtual math::LinearJacobian getLinearJacobianDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const =0
of a BodyNode.
void resetCommands()
Set all commands to zero.
Definition: MetaSkeleton.cpp:378
double getPotentialEnergy() const
Get the potential energy of this MetaSkeleton.
Definition: MetaSkeleton.cpp:1110
Eigen::VectorXd getAccelerationUpperLimits() const
Get the upper limits for all generalized coordinates's acceleration.
Definition: MetaSkeleton.cpp:799
void setVelocityLowerLimits(const Eigen::VectorXd &velocities)
Set the lower limits for all generalized coordinates's velocity.
Definition: MetaSkeleton.cpp:580
double getPositionUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's position.
Definition: MetaSkeleton.cpp:500
void setVelocityUpperLimits(const Eigen::VectorXd &velocities)
Set the upper limits for all generalized coordinates's velocity.
Definition: MetaSkeleton.cpp:624
double getAccelerationUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's acceleration.
Definition: MetaSkeleton.cpp:792
virtual Eigen::Vector3d getCOM(const Frame *_withRespectTo=Frame::World()) const =0
Get the MetaSkeleton's COM with respect to any Frame (default is World Frame)
void setForceUpperLimits(const Eigen::VectorXd &forces)
Set the upperlimits for all generalized coordinates's force.
Definition: MetaSkeleton.cpp:916
double getKineticEnergy() const
Get the kinetic energy of this MetaSkeleton.
Definition: MetaSkeleton.cpp:1104
virtual ~MetaSkeleton()=default
Default destructor.
void setAcceleration(std::size_t _index, double _acceleration)
Set the acceleration of a single generalized coordinate.
Definition: MetaSkeleton.cpp:661
virtual const std::vector< DegreeOfFreedom * > & getDofs()=0
Get the vector of DegreesOfFreedom for this MetaSkeleton.
void setAccelerationUpperLimit(std::size_t _index, double _acceleration)
Set the upper limit of a generalized coordinate's acceleration.
Definition: MetaSkeleton.cpp:763
virtual Eigen::Vector3d getCOMLinearVelocity(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the Skeleton's COM linear velocity in terms of any Frame (default is World Frame)
virtual void clearCollidingBodies()=0
Clear collision flags of the BodyNodes in this MetaSkeleton.
Eigen::VectorXd getCommands() const
Get commands for all generalized coordinates.
Definition: MetaSkeleton.cpp:363
double getForce(std::size_t _index) const
Get the force of a single generalized coordinate.
Definition: MetaSkeleton.cpp:821
void setForce(std::size_t _index, double _force)
Set the force of a single generalized coordinate.
Definition: MetaSkeleton.cpp:814
virtual math::Jacobian getJacobianClassicDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf) const =0
Get the spatial Jacobian (classical) time derivative targeting the origin a BodyNode.
void resetAccelerations()
Set all accelerations to zero.
Definition: MetaSkeleton.cpp:706
NameChangedSignal mNameChangedSignal
Definition: MetaSkeleton.hpp:896
virtual math::LinearJacobian getLinearJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the linear Jacobian targeting the origin of a BodyNode.
virtual std::size_t getIndexOf(const DegreeOfFreedom *_dof, bool _warning=true) const =0
Get the index of a specific DegreeOfFreedom within this ReferentialSkeleton.
void resetPositions()
Set all positions to zero.
Definition: MetaSkeleton.cpp:428
virtual bool hasBodyNode(const BodyNode *bodyNode) const =0
Returns whether this Skeleton contains bodyNode.
virtual const std::vector< const BodyNode * > & getBodyNodes() const =0
Get all the BodyNodes that are held by this MetaSkeleton.
virtual void clearExternalForces()=0
Clear the external forces of the BodyNodes in this MetaSkeleton.
virtual std::size_t getNumJoints() const =0
Get number of Joints.
Eigen::VectorXd getPositionUpperLimits() const
Get the upper limits for all generalized coordinates.
Definition: MetaSkeleton.cpp:507
virtual math::AngularJacobian getAngularJacobianDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the angular Jacobian time derivative of a BodyNode.
void setPosition(std::size_t index, double _position)
Set the position of a single generalized coordinate.
Definition: MetaSkeleton.cpp:384
virtual double getMass() const =0
Get the total mass of all BodyNodes in this MetaSkeleton.
void setForceLowerLimits(const Eigen::VectorXd &forces)
Set the lower limits for all generalized coordinates's force.
Definition: MetaSkeleton.cpp:872
virtual Eigen::Vector3d getCOMLinearAcceleration(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the MetaSkeleton's COM linear acceleration in terms of any Frame (default is World Frame)
virtual const DegreeOfFreedom * getDof(std::size_t _idx) const =0
Get degree of freedom (aka generalized coordinate) whose index is _idx.
double getAccelerationLowerLimit(std::size_t _index) const
Get the lower limit of a generalized coordinate's acceleration.
Definition: MetaSkeleton.cpp:741
double getForceUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's force.
Definition: MetaSkeleton.cpp:931
Eigen::VectorXd getPositionLowerLimits() const
Get the lower limits for all generalized coordinates.
Definition: MetaSkeleton.cpp:463
void setAccelerationUpperLimits(const Eigen::VectorXd &accelerations)
Set the upper limits for all generalized coordinates's acceleration.
Definition: MetaSkeleton.cpp:771
virtual math::Jacobian getJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf) const =0
Get the spatial Jacobian targeting the origin of a BodyNode.
double getVelocity(std::size_t _index) const
Get the velocity of a single generalized coordinate.
Definition: MetaSkeleton.cpp:529
virtual const Eigen::VectorXd & getCoriolisForces() const =0
Get Coriolis force vector of the MetaSkeleton's BodyNodes.
void setPositionUpperLimits(const Eigen::VectorXd &positions)
Set the upper limits for all generalized coordinates.
Definition: MetaSkeleton.cpp:485
virtual math::LinearJacobian getLinearJacobian(const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the linear Jacobian targeting an offset in a BodyNode.
virtual math::Jacobian getCOMJacobian(const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the MetaSkeleton's COM Jacobian in terms of any Frame (default is World Frame)
void resetGeneralizedForces()
Set all forces of the generalized coordinates to zero.
Definition: MetaSkeleton.cpp:857
void setCommand(std::size_t _index, double _command)
Set a single command.
Definition: MetaSkeleton.cpp:334
void setVelocityUpperLimit(std::size_t _index, double _velocity)
Set the upper limit of a generalized coordinate's velocity.
Definition: MetaSkeleton.cpp:617
virtual std::size_t getNumBodyNodes() const =0
Get number of body nodes.
void setAccelerationLowerLimit(std::size_t _index, double _acceleration)
Set the lower limit of a generalized coordinate's acceleration.
Definition: MetaSkeleton.cpp:712
double getForceLowerLimit(std::size_t _index) const
Get the lower limit of a generalized coordinate's force.
Definition: MetaSkeleton.cpp:887
virtual std::vector< const DegreeOfFreedom * > getDofs() const =0
Get a vector of const DegreesOfFreedom for this MetaSkeleton.
virtual std::vector< const BodyNode * > getBodyNodes(const std::string &name) const =0
Returns all the BodyNodes of given name.
void resetVelocities()
Set all velocities to zero.
Definition: MetaSkeleton.cpp:567
virtual math::Jacobian getJacobianClassicDeriv(const JacobianNode *_node) const =0
Get the spatial Jacobian (classical) time derivative targeting the origin of a BodyNode.
Eigen::VectorXd getVelocityUpperLimits() const
Get the upper limits for all generalized coordinates's velocity.
Definition: MetaSkeleton.cpp:646
Eigen::VectorXd getVelocityLowerLimits() const
Get the lower limits for all generalized coordinates's velocity.
Definition: MetaSkeleton.cpp:602
virtual const BodyNode * getBodyNode(const std::string &name) const =0
Returns the BodyNode of given name.
virtual const std::string & setName(const std::string &_name)=0
Set the name of this MetaSkeleton.
virtual Joint * getJoint(const std::string &name)=0
Returns the Joint of given name.
void setForces(const Eigen::VectorXd &_forces)
Set the forces of all generalized coordinates.
Definition: MetaSkeleton.cpp:828
virtual std::vector< const Joint * > getJoints(const std::string &name) const =0
Returns all the Joint of given name.
void setVelocities(const Eigen::VectorXd &_velocities)
Set the velocities of all generalized coordinates.
Definition: MetaSkeleton.cpp:536
Eigen::VectorXd getVelocities() const
Get the velocities for all generalized coordinates.
Definition: MetaSkeleton.cpp:552
double getPositionLowerLimit(std::size_t _index) const
Get the lower limit of a generalized coordinate's position.
Definition: MetaSkeleton.cpp:456
void setPositionLowerLimit(std::size_t _index, double _position)
Set the lower limit of a generalized coordinate's position.
Definition: MetaSkeleton.cpp:434
virtual Joint * getJoint(std::size_t _idx)=0
Get Joint whose index is _idx.
void setPositions(const Eigen::VectorXd &_positions)
Set the positions for all generalized coordinates.
Definition: MetaSkeleton.cpp:398
double getPosition(std::size_t _index) const
Get the position of a single generalized coordinate.
Definition: MetaSkeleton.cpp:391
void setCommands(const Eigen::VectorXd &_commands)
Set commands for all generalized coordinates.
Definition: MetaSkeleton.cpp:348
virtual const BodyNode * getBodyNode(std::size_t _idx) const =0
Get const BodyNode whose index is _idx.
virtual math::Jacobian getJacobianSpatialDeriv(const JacobianNode *_node, const Frame *_inCoordinatesOf) const =0
Get the spatial Jacobian time derivative targeting the origin of a BodyNode.
virtual void clearInternalForces()=0
Clear the internal forces of the BodyNodes in this MetaSkeleton.
virtual bool hasJoint(const Joint *joint) const =0
Returns whether this Skeleton contains join.
virtual math::Jacobian getJacobianClassicDeriv(const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the spatial Jacobian (classical) time derivative targeting an offset in a BodyNode.
virtual Eigen::Vector6d getCOMSpatialVelocity(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the Skeleton's COM spatial velocity in terms of any Frame (default is World Frame)
virtual math::Jacobian getJacobian(const JacobianNode *_node, const Eigen::Vector3d &_localOffset) const =0
Get the spatial Jacobian targeting an offset in a BodyNode.
virtual math::Jacobian getJacobian(const JacobianNode *_node) const =0
Get the spatial Jacobian targeting the origin of a BodyNode.
Eigen::VectorXd getForceLowerLimits() const
Get the lower limits for all generalized coordinates's force.
Definition: MetaSkeleton.cpp:894
virtual math::Jacobian getCOMJacobianSpatialDeriv(const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the Skeleton's COM Jacobian spatial time derivative in terms of any Frame (default is World Frame...
Eigen::VectorXd getForceUpperLimits() const
Get the upper limits for all generalized coordinates's force.
Definition: MetaSkeleton.cpp:938
Eigen::VectorXd getAccelerationLowerLimits() const
Get the lower limits for all generalized coordinates's acceleration.
Definition: MetaSkeleton.cpp:748
virtual const Eigen::VectorXd & getGravityForces() const =0
Get gravity force vector of the MetaSkeleton.
Eigen::VectorXd getPositions() const
Get the positions for all generalized coordinates.
Definition: MetaSkeleton.cpp:413
MetaSkeletonPtr cloneMetaSkeleton() const
Creates an identical clone of this MetaSkeleton.
Definition: MetaSkeleton.cpp:328
double getAcceleration(std::size_t _index) const
Get the acceleration of a single generalized coordinate.
Definition: MetaSkeleton.cpp:668
void setPositionLowerLimits(const Eigen::VectorXd &positions)
Set the lower limits for all generalized coordinates.
Definition: MetaSkeleton.cpp:441
virtual const std::vector< BodyNode * > & getBodyNodes()=0
Get all the BodyNodes that are held by this MetaSkeleton.
virtual std::size_t getIndexOf(const BodyNode *_bn, bool _warning=true) const =0
Get the index of a specific BodyNode within this ReferentialSkeleton.
double getVelocityUpperLimit(std::size_t _index)
Get the upper limit of a generalized coordinate's velocity.
Definition: MetaSkeleton.cpp:639
void setForceUpperLimit(std::size_t _index, double _force)
Set the upper limit of a generalized coordinate's force.
Definition: MetaSkeleton.cpp:909
virtual const Eigen::VectorXd & getExternalForces() const =0
Get external force vector of the MetaSkeleton.
virtual const Joint * getJoint(std::size_t _idx) const =0
Get const Joint whose index is _idx.
void setForceLowerLimit(std::size_t _index, double _force)
Set the lower limit of a generalized coordinate's force.
Definition: MetaSkeleton.cpp:865
virtual const Eigen::MatrixXd & getInvMassMatrix() const =0
Get inverse of Mass Matrix of the MetaSkeleton.
void setAccelerations(const Eigen::VectorXd &_accelerations)
Set the accelerations of all generalized coordinates.
Definition: MetaSkeleton.cpp:675
virtual math::Jacobian getJacobianSpatialDeriv(const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf) const =0
Get the spatial Jacobian time derivative targeting an offset in a BodyNode.
Eigen::VectorXd getVelocityChanges() const
Get the velocity changes for all the generalized coordinates.
Definition: MetaSkeleton.cpp:953
virtual math::Jacobian getWorldJacobian(const JacobianNode *_node, const Eigen::Vector3d &_localOffset) const =0
Get the spatial Jacobian targeting an offset in a BodyNode.
virtual BodyNode * getBodyNode(const std::string &name)=0
Returns the BodyNode of given name.
void setPositionUpperLimit(std::size_t _index, double _position)
Set the upper limit of a generalized coordainte's position.
Definition: MetaSkeleton.cpp:478
virtual math::AngularJacobian getAngularJacobian(const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the angular Jacobian of a BodyNode.
MetaSkeleton(const MetaSkeleton &)=delete
virtual const Eigen::MatrixXd & getAugMassMatrix() const =0
Get augmented mass matrix of the skeleton.
virtual const Eigen::MatrixXd & getInvAugMassMatrix() const =0
Get inverse of augmented Mass Matrix of the MetaSkeleton.
virtual math::LinearJacobian getCOMLinearJacobianDeriv(const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the Skeleton's COM Linear Jacobian time derivative in terms of any Frame (default is World Frame)...
virtual std::vector< BodyNode * > getBodyNodes(const std::string &name)=0
Returns all the BodyNodes of given name.
virtual math::Jacobian getWorldJacobian(const JacobianNode *_node) const =0
Get the spatial Jacobian targeting the origin of a BodyNode.
common::SlotRegister< NameChangedSignal > onNameChanged
Definition: MetaSkeleton.hpp:902
virtual double computeKineticEnergy() const =0
Get the kinetic energy of this MetaSkeleton.
void setVelocityLowerLimit(std::size_t _index, double _velocity)
Set the lower limit of a generalized coordinate's velocity.
Definition: MetaSkeleton.cpp:573
virtual DegreeOfFreedom * getDof(std::size_t _idx)=0
Get degree of freedom (aka generalized coordinate) whose index is _idx.
virtual math::Jacobian getJacobianSpatialDeriv(const JacobianNode *_node) const =0
Get the spatial Jacobian time derivative targeting the origin of a BodyNode.
virtual const Eigen::VectorXd & getConstraintForces() const =0
Get constraint force vector.
double getVelocityLowerLimit(std::size_t _index)
Get the lower limit of a generalized coordinate's velocity.
Definition: MetaSkeleton.cpp:595
virtual const Eigen::VectorXd & getCoriolisAndGravityForces() const =0
Get combined vector of Coriolis force and gravity force of the MetaSkeleton.
virtual std::vector< Joint * > getJoints(const std::string &name)=0
Returns all the Joint of given name.
virtual double computePotentialEnergy() const =0
Get the potential energy of this MetaSkeleton.
double getCommand(std::size_t _index) const
Get a single command.
Definition: MetaSkeleton.cpp:341
virtual const std::string & getName() const =0
Get the name of this MetaSkeleton.
virtual std::unique_ptr< common::LockableReference > getLockableReference() const =0
Returns mutex.
virtual std::vector< Joint * > getJoints()=0
Returns all the joints that are held by this MetaSkeleton.
Eigen::VectorXd getAccelerations() const
Get the accelerations for all generalized coordinates.
Definition: MetaSkeleton.cpp:691
virtual Eigen::Vector6d getCOMSpatialAcceleration(const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the Skeleton's COM spatial acceleration in terms of any Frame (default is World Frame)
double computeLagrangian() const
Compute and return Lagrangian of this MetaSkeleton.
Definition: MetaSkeleton.cpp:1098
virtual math::LinearJacobian getCOMLinearJacobian(const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the MetaSkeleton's COM Linear Jacobian in terms of any Frame (default is World Frame)
virtual const Eigen::MatrixXd & getMassMatrix() const =0
Get the Mass Matrix of the MetaSkeleton.
virtual MetaSkeletonPtr cloneMetaSkeleton(const std::string &cloneName) const =0
Creates an identical clone of this MetaSkeleton.
virtual math::Jacobian getJacobianSpatialDeriv(const JacobianNode *_node, const Eigen::Vector3d &_localOffset) const =0
Get the spatial Jacobian time derivative targeting an offset in a BodyNode.
virtual math::LinearJacobian getLinearJacobianDeriv(const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf=Frame::World()) const =0
Get the linear Jacobian (classical) time derivative targeting an offset in a BodyNode.
virtual const Joint * getJoint(const std::string &name) const =0
Returns the joint of given name.
void setJointConstraintImpulses(const Eigen::VectorXd &_impulses)
Set the constraint impulses for the generalized coordinates.
Definition: MetaSkeleton.cpp:960
Eigen::VectorXd getJointConstraintImpulses() const
Get the constraint impulses for the generalized coordinates.
Definition: MetaSkeleton.cpp:967
virtual std::size_t getNumDofs() const =0
Return the number of degrees of freedom in this skeleton.
virtual BodyNode * getBodyNode(std::size_t _idx)=0
Get BodyNode whose index is _idx.
void setVelocity(std::size_t _index, double _velocity)
Set the velocity of a single generalized coordinate.
Definition: MetaSkeleton.cpp:522
virtual std::vector< const Joint * > getJoints() const =0
Returns all the joints that are held by this MetaSkeleton.
virtual std::size_t getIndexOf(const Joint *_joint, bool _warning=true) const =0
Get the index of a specific Joint within this ReferentialSkeleton.
Definition: Random-impl.hpp:92
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
std::shared_ptr< MetaSkeleton > MetaSkeletonPtr
Definition: SmartPointer.hpp:68
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