DART 6.10.1
Loading...
Searching...
No Matches
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
47
48namespace dart {
49namespace dynamics {
50
51class BodyNode;
52class SoftBodyNode;
53class PointMass;
54class Joint;
55class DegreeOfFreedom;
56class Marker;
57
62{
63public:
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
507
509 void setForceLowerLimit(std::size_t _index, double _force);
510
512 void setForceLowerLimits(const Eigen::VectorXd& forces);
513
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
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
837 virtual Eigen::Vector6d getCOMSpatialVelocity(
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
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
866 virtual math::LinearJacobian getCOMLinearJacobian(
867 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
868
875 virtual math::Jacobian getCOMJacobianSpatialDeriv(
876 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
877
884 virtual math::LinearJacobian getCOMLinearJacobianDeriv(
885 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
886
888
889protected:
891 MetaSkeleton();
892
893 //--------------------------------------------------------------------------
894 // Signals
895 //--------------------------------------------------------------------------
897
898public:
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 const Joint * getJoint(const std::string &name) const =0
Returns the joint of given name.
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
virtual BodyNode * getBodyNode(std::size_t _idx)=0
Get BodyNode whose index is _idx.
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
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< BodyNode * > & getBodyNodes()=0
Get all the BodyNodes that are held by this MetaSkeleton.
virtual const Eigen::MatrixXd & getMassMatrix() const =0
Get the Mass Matrix of the MetaSkeleton.
virtual const std::vector< DegreeOfFreedom * > & getDofs()=0
Get the vector of DegreesOfFreedom for this MetaSkeleton.
virtual const Eigen::VectorXd & getGravityForces() const =0
Get gravity force vector of the MetaSkeleton.
virtual void clearExternalForces()=0
Clear the external forces of the BodyNodes in this MetaSkeleton.
virtual BodyNode * getBodyNode(const std::string &name)=0
Returns the BodyNode of given name.
virtual const BodyNode * getBodyNode(const std::string &name) const =0
Returns the BodyNode of given name.
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 std::unique_ptr< common::LockableReference > getLockableReference() const =0
Returns mutex.
virtual std::vector< const BodyNode * > getBodyNodes(const std::string &name) const =0
Returns all the BodyNodes of given name.
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)
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
virtual Joint * getJoint(std::size_t _idx)=0
Get Joint whose index is _idx.
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::MatrixXd & getInvMassMatrix() const =0
Get inverse of Mass Matrix of the MetaSkeleton.
void setPositionUpperLimits(const Eigen::VectorXd &positions)
Set the upper limits for all generalized coordinates.
Definition MetaSkeleton.cpp:485
virtual std::vector< BodyNode * > getBodyNodes(const std::string &name)=0
Returns all the BodyNodes of given name.
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
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
virtual const Eigen::VectorXd & getConstraintForces() const =0
Get constraint force vector.
virtual std::vector< Joint * > getJoints(const std::string &name)=0
Returns all the Joint of given name.
virtual DegreeOfFreedom * getDof(std::size_t _idx)=0
Get degree of freedom (aka generalized coordinate) whose index is _idx.
virtual std::vector< const Joint * > getJoints() const =0
Returns all the joints that are held by this MetaSkeleton.
Eigen::VectorXd getVelocityLowerLimits() const
Get the lower limits for all generalized coordinates's velocity.
Definition MetaSkeleton.cpp:602
virtual std::vector< const DegreeOfFreedom * > getDofs() const =0
Get a vector of const DegreesOfFreedom for this MetaSkeleton.
void setForces(const Eigen::VectorXd &_forces)
Set the forces of all generalized coordinates.
Definition MetaSkeleton.cpp:828
virtual Joint * getJoint(const std::string &name)=0
Returns 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
virtual const std::string & setName(const std::string &_name)=0
Set the name of this MetaSkeleton.
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
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 DegreeOfFreedom * getDof(std::size_t _idx) const =0
Get degree of freedom (aka generalized coordinate) 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 const Eigen::VectorXd & getCoriolisAndGravityForces() const =0
Get combined vector of Coriolis force and gravity force of the MetaSkeleton.
virtual const Eigen::MatrixXd & getAugMassMatrix() const =0
Get augmented mass matrix of the skeleton.
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
virtual const Eigen::VectorXd & getCoriolisForces() const =0
Get Coriolis force vector of the MetaSkeleton's BodyNodes.
virtual const Eigen::VectorXd & getExternalForces() const =0
Get external force vector of the MetaSkeleton.
Eigen::VectorXd getAccelerationLowerLimits() const
Get the lower limits for all generalized coordinates's acceleration.
Definition MetaSkeleton.cpp:748
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 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
void setForceLowerLimit(std::size_t _index, double _force)
Set the lower limit of a generalized coordinate's force.
Definition MetaSkeleton.cpp:865
virtual const Joint * getJoint(std::size_t _idx) const =0
Get const Joint whose index is _idx.
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.
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 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 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 math::Jacobian getJacobianSpatialDeriv(const JacobianNode *_node) const =0
Get the spatial Jacobian time derivative targeting the origin of a BodyNode.
virtual const std::vector< const BodyNode * > & getBodyNodes() const =0
Get all the BodyNodes that are held by this MetaSkeleton.
double getVelocityLowerLimit(std::size_t _index)
Get the lower limit of a generalized coordinate's velocity.
Definition MetaSkeleton.cpp:595
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
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 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 std::vector< Joint * > getJoints()=0
Returns all the joints that are held by this MetaSkeleton.
virtual std::vector< const Joint * > getJoints(const std::string &name) const =0
Returns all the Joint of given name.
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 std::string & getName() const =0
Get the name of this MetaSkeleton.
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 const Eigen::MatrixXd & getInvAugMassMatrix() const =0
Get inverse of augmented Mass Matrix of the MetaSkeleton.
void setVelocity(std::size_t _index, double _velocity)
Set the velocity of a single generalized coordinate.
Definition MetaSkeleton.cpp:522
virtual const BodyNode * getBodyNode(std::size_t _idx) const =0
Get const BodyNode whose index is _idx.
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
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