DART  6.7.3
MetaSkeleton.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_METASKELETON_HPP_
34 #define DART_DYNAMICS_METASKELETON_HPP_
35 
36 #include <vector>
37 #include <string>
38 
39 #include <Eigen/Dense>
40 
42 #include "dart/common/Signal.hpp"
43 #include "dart/common/Subject.hpp"
44 #include "dart/math/Geometry.hpp"
45 #include "dart/dynamics/Frame.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  = common::Signal<void(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(const BodyNode* _bn, bool _warning=true) const = 0;
151 
153  virtual std::size_t getNumJoints() const = 0;
154 
156  virtual Joint* getJoint(std::size_t _idx) = 0;
157 
159  virtual const Joint* getJoint(std::size_t _idx) const = 0;
160 
164  virtual Joint* getJoint(const std::string& name) = 0;
165 
169  virtual const Joint* getJoint(const std::string& name) const = 0;
170 
172  virtual std::vector<Joint*> getJoints() = 0;
173 
175  virtual std::vector<const Joint*> getJoints() const = 0;
176 
184  virtual std::vector<Joint*> getJoints(const std::string& name) = 0;
185 
193  virtual std::vector<const Joint*> getJoints(
194  const std::string& name) const = 0;
195 
197  virtual bool hasJoint(const Joint* joint) const = 0;
198 
203  virtual std::size_t getIndexOf(const Joint* _joint, bool _warning=true) const = 0;
204 
206  virtual std::size_t getNumDofs() const = 0;
207 
209  virtual DegreeOfFreedom* getDof(std::size_t _idx) = 0;
210 
212  virtual const DegreeOfFreedom* getDof(std::size_t _idx) const = 0;
213 
215  virtual const std::vector<DegreeOfFreedom*>& getDofs() = 0;
216 
218  virtual std::vector<const DegreeOfFreedom*> getDofs() const = 0;
219 
224  virtual std::size_t getIndexOf(const DegreeOfFreedom* _dof,
225  bool _warning=true) const = 0;
226 
228 
229  //----------------------------------------------------------------------------
231  //----------------------------------------------------------------------------
232 
234  void setCommand(std::size_t _index, double _command);
235 
237  double getCommand(std::size_t _index) const;
238 
240  void setCommands(const Eigen::VectorXd& _commands);
241 
243  void setCommands(const std::vector<std::size_t>& _indices,
244  const Eigen::VectorXd& _commands);
245 
247  Eigen::VectorXd getCommands() const;
248 
250  Eigen::VectorXd getCommands(const std::vector<std::size_t>& _indices) const;
251 
253  void resetCommands();
254 
256 
257  //----------------------------------------------------------------------------
259  //----------------------------------------------------------------------------
260 
262  void setPosition(std::size_t index, double _position);
263 
265  double getPosition(std::size_t _index) const;
266 
268  void setPositions(const Eigen::VectorXd& _positions);
269 
271  void setPositions(const std::vector<std::size_t>& _indices,
272  const Eigen::VectorXd& _positions);
273 
275  Eigen::VectorXd getPositions() const;
276 
278  Eigen::VectorXd getPositions(const std::vector<std::size_t>& _indices) const;
279 
281  void resetPositions();
282 
284  void setPositionLowerLimit(std::size_t _index, double _position);
285 
287  void setPositionLowerLimits(const Eigen::VectorXd& positions);
288 
290  void setPositionLowerLimits(const std::vector<std::size_t>& indices,
291  const Eigen::VectorXd& positions);
292 
294  double getPositionLowerLimit(std::size_t _index) const;
295 
297  Eigen::VectorXd getPositionLowerLimits() const;
298 
300  Eigen::VectorXd getPositionLowerLimits(
301  const std::vector<std::size_t>& indices) const;
302 
304  void setPositionUpperLimit(std::size_t _index, double _position);
305 
307  void setPositionUpperLimits(const Eigen::VectorXd& positions);
308 
310  void setPositionUpperLimits(const std::vector<std::size_t>& indices,
311  const Eigen::VectorXd& positions);
312 
314  double getPositionUpperLimit(std::size_t _index) const;
315 
317  Eigen::VectorXd getPositionUpperLimits() const;
318 
320  Eigen::VectorXd getPositionUpperLimits(
321  const std::vector<std::size_t>& indices) const;
322 
324 
325  //----------------------------------------------------------------------------
327  //----------------------------------------------------------------------------
328 
330  void setVelocity(std::size_t _index, double _velocity);
331 
333  double getVelocity(std::size_t _index) const;
334 
336  void setVelocities(const Eigen::VectorXd& _velocities);
337 
339  void setVelocities(const std::vector<std::size_t>& _indices,
340  const Eigen::VectorXd& _velocities);
341 
343  Eigen::VectorXd getVelocities() const;
344 
346  Eigen::VectorXd getVelocities(const std::vector<std::size_t>& _indices) const;
347 
349  void resetVelocities();
350 
352  void setVelocityLowerLimit(std::size_t _index, double _velocity);
353 
355  void setVelocityLowerLimits(const Eigen::VectorXd& velocities);
356 
359  void setVelocityLowerLimits(const std::vector<std::size_t>& indices,
360  const Eigen::VectorXd& velocities);
361 
363  double getVelocityLowerLimit(std::size_t _index);
364 
366  Eigen::VectorXd getVelocityLowerLimits() const;
367 
370  Eigen::VectorXd getVelocityLowerLimits(
371  const std::vector<std::size_t>& indices) const;
372 
374  void setVelocityUpperLimit(std::size_t _index, double _velocity);
375 
377  void setVelocityUpperLimits(const Eigen::VectorXd& velocities);
378 
381  void setVelocityUpperLimits(const std::vector<std::size_t>& indices,
382  const Eigen::VectorXd& velocities);
383 
385  double getVelocityUpperLimit(std::size_t _index);
386 
388  Eigen::VectorXd getVelocityUpperLimits() const;
389 
392  Eigen::VectorXd getVelocityUpperLimits(
393  const std::vector<std::size_t>& indices) const;
394 
396 
397  //----------------------------------------------------------------------------
399  //----------------------------------------------------------------------------
400 
402  void setAcceleration(std::size_t _index, double _acceleration);
403 
405  double getAcceleration(std::size_t _index) const;
406 
408  void setAccelerations(const Eigen::VectorXd& _accelerations);
409 
411  void setAccelerations(const std::vector<std::size_t>& _indices,
412  const Eigen::VectorXd& _accelerations);
413 
415  Eigen::VectorXd getAccelerations() const;
416 
418  Eigen::VectorXd getAccelerations(const std::vector<std::size_t>& _indices) const;
419 
421  void resetAccelerations();
422 
424  void setAccelerationLowerLimit(std::size_t _index, double _acceleration);
425 
427  void setAccelerationLowerLimits(const Eigen::VectorXd& accelerations);
428 
431  void setAccelerationLowerLimits(const std::vector<std::size_t>& indices,
432  const Eigen::VectorXd& accelerations);
433 
435  double getAccelerationLowerLimit(std::size_t _index) const;
436 
438  Eigen::VectorXd getAccelerationLowerLimits() const;
439 
442  Eigen::VectorXd getAccelerationLowerLimits(
443  const std::vector<std::size_t>& indices) const;
444 
446  void setAccelerationUpperLimit(std::size_t _index, double _acceleration);
447 
449  void setAccelerationUpperLimits(const Eigen::VectorXd& accelerations);
450 
453  void setAccelerationUpperLimits(const std::vector<std::size_t>& indices,
454  const Eigen::VectorXd& accelerations);
455 
457  double getAccelerationUpperLimit(std::size_t _index) const;
458 
460  Eigen::VectorXd getAccelerationUpperLimits() const;
461 
464  Eigen::VectorXd getAccelerationUpperLimits(
465  const std::vector<std::size_t>& indices) const;
466 
468 
469  //----------------------------------------------------------------------------
471  //----------------------------------------------------------------------------
472 
474  void setForce(std::size_t _index, double _force);
475 
477  double getForce(std::size_t _index) const;
478 
480  void setForces(const Eigen::VectorXd& _forces);
481 
483  void setForces(const std::vector<std::size_t>& _index,
484  const Eigen::VectorXd& _forces);
485 
487  Eigen::VectorXd getForces() const;
488 
490  Eigen::VectorXd getForces(const std::vector<std::size_t>& _indices) const;
491 
493  void resetGeneralizedForces();
494 
496  void setForceLowerLimit(std::size_t _index, double _force);
497 
499  void setForceLowerLimits(const Eigen::VectorXd& forces);
500 
502  void setForceLowerLimits(const std::vector<std::size_t>& indices,
503  const Eigen::VectorXd& forces);
504 
506  double getForceLowerLimit(std::size_t _index) const;
507 
509  Eigen::VectorXd getForceLowerLimits() const;
510 
512  Eigen::VectorXd getForceLowerLimits(
513  const std::vector<std::size_t>& indices) const;
514 
516  void setForceUpperLimit(std::size_t _index, double _force);
517 
519  void setForceUpperLimits(const Eigen::VectorXd& forces);
520 
522  void setForceUpperLimits(const std::vector<std::size_t>& indices,
523  const Eigen::VectorXd& forces);
524 
526  double getForceUpperLimit(std::size_t _index) const;
527 
529  Eigen::VectorXd getForceUpperLimits() const;
530 
532  Eigen::VectorXd getForceUpperLimits(
533  const std::vector<std::size_t>& indices) const;
534 
536 
537  //----------------------------------------------------------------------------
539  //----------------------------------------------------------------------------
540 
542  Eigen::VectorXd getVelocityChanges() const;
543 
544  //----------------------------------------------------------------------------
546  //----------------------------------------------------------------------------
547 
549  void setJointConstraintImpulses(const Eigen::VectorXd& _impulses);
550 
552  Eigen::VectorXd getJointConstraintImpulses() const;
553 
554 
555  //----------------------------------------------------------------------------
557  //----------------------------------------------------------------------------
558 
561  virtual math::Jacobian getJacobian(const JacobianNode* _node) const = 0;
562 
566  const JacobianNode* _node,
567  const Frame* _inCoordinatesOf) const = 0;
568 
573  const JacobianNode* _node,
574  const JacobianNode* _relativeTo,
575  const Frame* _inCoordinatesOf) const;
576 
581  const JacobianNode* _node,
582  const Eigen::Vector3d& _localOffset) const = 0;
583 
588  const JacobianNode* _node,
589  const Eigen::Vector3d& _localOffset,
590  const Frame* _inCoordinatesOf) const = 0;
591 
597  const JacobianNode* _node,
598  const Eigen::Vector3d& _localOffset,
599  const JacobianNode* _relativeTo,
600  const Frame* _inCoordinatesOf) const;
601 
604  virtual math::Jacobian getWorldJacobian(const JacobianNode* _node) const = 0;
605 
610  const JacobianNode* _node,
611  const Eigen::Vector3d& _localOffset) const = 0;
612 
616  const JacobianNode* _node,
617  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
618 
623  const JacobianNode* _node,
624  const Eigen::Vector3d& _localOffset,
625  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
626 
631  const JacobianNode* _node,
632  const JacobianNode* _relativeTo,
633  const Frame* _inCoordinatesOf = Frame::World()) const;
634 
640  const JacobianNode* _node,
641  const Eigen::Vector3d& _localOffset,
642  const JacobianNode* _relativeTo,
643  const Frame* _inCoordinatesOf = Frame::World()) const;
644 
646 
647  //----------------------------------------------------------------------------
649  //----------------------------------------------------------------------------
650 
654  const JacobianNode* _node,
655  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
656 
661  const JacobianNode* _node,
662  const JacobianNode* _relativeTo,
663  const Frame* _inCoordinatesOf = Frame::World()) const;
664 
668  const JacobianNode* _node) const = 0;
669 
673  const JacobianNode* _node,
674  const Frame* _inCoordinatesOf) const = 0;
675 
680  const JacobianNode* _node,
681  const Eigen::Vector3d& _localOffset) const = 0;
682 
687  const JacobianNode* _node,
688  const Eigen::Vector3d& _localOffset,
689  const Frame* _inCoordinatesOf) const = 0;
690 
695  const JacobianNode* _node,
696  const JacobianNode* _relativeTo,
697  const Frame* _inCoordinatesOf) const;
698 
704  const JacobianNode* _node,
705  const Eigen::Vector3d& _localOffset,
706  const JacobianNode* _relativeTo,
707  const Frame* _inCoordinatesOf) const;
708 
712  const JacobianNode* _node) const = 0;
713 
718  const JacobianNode* _node,
719  const Frame* _inCoordinatesOf) const = 0;
720 
725  const JacobianNode* _node,
726  const Eigen::Vector3d& _localOffset,
727  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
728 
732  const JacobianNode* _node,
733  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
734 
739  const JacobianNode* _node,
740  const Eigen::Vector3d& _localOffset,
741  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
742 
746  const JacobianNode* _node,
747  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
748 
750 
751  //----------------------------------------------------------------------------
753  //----------------------------------------------------------------------------
754 
758  virtual double getMass() const = 0;
759 
761  virtual const Eigen::MatrixXd& getMassMatrix() const = 0;
762 
767  virtual const Eigen::MatrixXd& getAugMassMatrix() const = 0;
768 
770  virtual const Eigen::MatrixXd& getInvMassMatrix() const = 0;
771 
773  virtual const Eigen::MatrixXd& getInvAugMassMatrix() const = 0;
774 
776  virtual const Eigen::VectorXd& getCoriolisForces() const = 0;
777 
779  virtual const Eigen::VectorXd& getGravityForces() const = 0;
780 
782  virtual const Eigen::VectorXd& getCoriolisAndGravityForces() const = 0;
783 
785  virtual const Eigen::VectorXd& getExternalForces() const = 0;
786 
788  virtual const Eigen::VectorXd& getConstraintForces() const = 0;
789 
791  virtual void clearExternalForces() = 0;
792 
794  virtual void clearInternalForces() = 0;
795 
797  double computeLagrangian() const;
798 
800  DART_DEPRECATED(6.1)
801  double getKineticEnergy() const;
802 
804  virtual double computeKineticEnergy() const = 0;
805 
807  DART_DEPRECATED(6.1)
808  double getPotentialEnergy() const;
809 
811  virtual double computePotentialEnergy() const = 0;
812 
814  DART_DEPRECATED(6.0)
815  virtual void clearCollidingBodies() = 0;
816 
818 
819  //----------------------------------------------------------------------------
821  //----------------------------------------------------------------------------
822 
825  virtual Eigen::Vector3d getCOM(
826  const Frame* _withRespectTo = Frame::World()) const = 0;
827 
831  const Frame* _relativeTo = Frame::World(),
832  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
833 
836  virtual Eigen::Vector3d getCOMLinearVelocity(
837  const Frame* _relativeTo = Frame::World(),
838  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
839 
843  const Frame* _relativeTo = Frame::World(),
844  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
845 
848  virtual Eigen::Vector3d getCOMLinearAcceleration(
849  const Frame* _relativeTo = Frame::World(),
850  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
851 
854  virtual math::Jacobian getCOMJacobian(
855  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
856 
860  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
861 
869  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
870 
878  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
879 
881 
882 protected:
883 
885  MetaSkeleton();
886 
887  //--------------------------------------------------------------------------
888  // Signals
889  //--------------------------------------------------------------------------
891 
892 public:
893 
894  //--------------------------------------------------------------------------
895  // Slot registers
896  //--------------------------------------------------------------------------
897  common::SlotRegister<NameChangedSignal> onNameChanged;
898 };
899 
900 } // namespace dynamics
901 } // namespace dart
902 
903 
904 #endif // DART_DYNAMICS_METASKELETON_HPP_
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
std::string * name
Definition: SkelParser.cpp:1642
std::size_t index
Definition: SkelParser.cpp:1617
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: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
Eigen::VectorXd getForces() const
Get the forces for all generalized coordinates.
Definition: MetaSkeleton.cpp:820
void setAccelerationLowerLimits(const Eigen::VectorXd &accelerations)
Set the lower limits for all generalized coordinates's acceleration.
Definition: MetaSkeleton.cpp:704
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:367
double getPotentialEnergy() const
Get the potential energy of this MetaSkeleton.
Definition: MetaSkeleton.cpp:1086
Eigen::VectorXd getAccelerationUpperLimits() const
Get the upper limits for all generalized coordinates's acceleration.
Definition: MetaSkeleton.cpp:776
void setVelocityLowerLimits(const Eigen::VectorXd &velocities)
Set the lower limits for all generalized coordinates's velocity.
Definition: MetaSkeleton.cpp:566
double getPositionUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's position.
Definition: MetaSkeleton.cpp:488
void setVelocityUpperLimits(const Eigen::VectorXd &velocities)
Set the upper limits for all generalized coordinates's velocity.
Definition: MetaSkeleton.cpp:610
double getAccelerationUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's acceleration.
Definition: MetaSkeleton.cpp:769
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:893
double getKineticEnergy() const
Get the kinetic energy of this MetaSkeleton.
Definition: MetaSkeleton.cpp:1080
virtual ~MetaSkeleton()=default
Default destructor.
void setAcceleration(std::size_t _index, double _acceleration)
Set the acceleration of a single generalized coordinate.
Definition: MetaSkeleton.cpp:647
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:744
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:353
double getForce(std::size_t _index) const
Get the force of a single generalized coordinate.
Definition: MetaSkeleton.cpp:798
void setForce(std::size_t _index, double _force)
Set the force of a single generalized coordinate.
Definition: MetaSkeleton.cpp:791
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:691
NameChangedSignal mNameChangedSignal
Definition: MetaSkeleton.hpp:890
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:416
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:495
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:373
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:849
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:722
double getForceUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's force.
Definition: MetaSkeleton.cpp:908
Eigen::VectorXd getPositionLowerLimits() const
Get the lower limits for all generalized coordinates.
Definition: MetaSkeleton.cpp:451
void setAccelerationUpperLimits(const Eigen::VectorXd &accelerations)
Set the upper limits for all generalized coordinates's acceleration.
Definition: MetaSkeleton.cpp:751
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:517
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:473
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:834
void setCommand(std::size_t _index, double _command)
Set a single command.
Definition: MetaSkeleton.cpp:324
void setVelocityUpperLimit(std::size_t _index, double _velocity)
Set the upper limit of a generalized coordinate's velocity.
Definition: MetaSkeleton.cpp:603
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:697
double getForceLowerLimit(std::size_t _index) const
Get the lower limit of a generalized coordinate's force.
Definition: MetaSkeleton.cpp:864
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:553
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:632
Eigen::VectorXd getVelocityLowerLimits() const
Get the lower limits for all generalized coordinates's velocity.
Definition: MetaSkeleton.cpp:588
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:805
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:524
Eigen::VectorXd getVelocities() const
Get the velocities for all generalized coordinates.
Definition: MetaSkeleton.cpp:539
double getPositionLowerLimit(std::size_t _index) const
Get the lower limit of a generalized coordinate's position.
Definition: MetaSkeleton.cpp:444
void setPositionLowerLimit(std::size_t _index, double _position)
Set the lower limit of a generalized coordinate's position.
Definition: MetaSkeleton.cpp:422
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:387
double getPosition(std::size_t _index) const
Get the position of a single generalized coordinate.
Definition: MetaSkeleton.cpp:380
void setCommands(const Eigen::VectorXd &_commands)
Set commands for all generalized coordinates.
Definition: MetaSkeleton.cpp:338
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:871
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:915
Eigen::VectorXd getAccelerationLowerLimits() const
Get the lower limits for all generalized coordinates's acceleration.
Definition: MetaSkeleton.cpp:729
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:402
MetaSkeletonPtr cloneMetaSkeleton() const
Creates an identical clone of this MetaSkeleton.
Definition: MetaSkeleton.cpp:318
double getAcceleration(std::size_t _index) const
Get the acceleration of a single generalized coordinate.
Definition: MetaSkeleton.cpp:654
void setPositionLowerLimits(const Eigen::VectorXd &positions)
Set the lower limits for all generalized coordinates.
Definition: MetaSkeleton.cpp:429
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:625
void setForceUpperLimit(std::size_t _index, double _force)
Set the upper limit of a generalized coordinate's force.
Definition: MetaSkeleton.cpp:886
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:842
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:661
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:930
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:466
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:897
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:559
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:581
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:331
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:676
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:1074
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:937
Eigen::VectorXd getJointConstraintImpulses() const
Get the constraint impulses for the generalized coordinates.
Definition: MetaSkeleton.cpp:944
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:510
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: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