DART 6.6.2
Loading...
Searching...
No Matches
MetaSkeleton.hpp
Go to the documentation of this file.
1/*
2 * Copyright (c) 2011-2018, 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
47
48namespace dart {
49namespace dynamics {
50
51class BodyNode;
52class SoftBodyNode;
53class PointMass;
54class Joint;
55class DegreeOfFreedom;
56class Marker;
57
62{
63public:
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
75 virtual std::unique_ptr<common::LockableReference> getLockableReference()
76 const = 0;
77 // TODO: Rename this to getMutex once the deprecated Skeleton::getMutex() is
78 // removed in DART 7.
79
80 //----------------------------------------------------------------------------
82 //----------------------------------------------------------------------------
83
85 virtual const std::string& setName(const std::string& _name) = 0;
86
88 virtual const std::string& getName() const = 0;
89
91
92 //----------------------------------------------------------------------------
94 //----------------------------------------------------------------------------
95
97 virtual std::size_t getNumBodyNodes() const = 0;
98
100 virtual BodyNode* getBodyNode(std::size_t _idx) = 0;
101
103 virtual const BodyNode* getBodyNode(std::size_t _idx) const = 0;
104
109 virtual BodyNode* getBodyNode(const std::string& name) = 0;
110
115 virtual const BodyNode* getBodyNode(const std::string& name) const = 0;
116
118 virtual const std::vector<BodyNode*>& getBodyNodes() = 0;
119
121 virtual const std::vector<const BodyNode*>& getBodyNodes() const = 0;
122
126 virtual std::vector<BodyNode*> getBodyNodes(const std::string& name) = 0;
127
131 virtual std::vector<const BodyNode*> getBodyNodes(
132 const std::string& name) const = 0;
133
135 virtual bool hasBodyNode(const BodyNode* bodyNode) const = 0;
136
141 virtual std::size_t getIndexOf(const BodyNode* _bn, bool _warning=true) const = 0;
142
144 virtual std::size_t getNumJoints() const = 0;
145
147 virtual Joint* getJoint(std::size_t _idx) = 0;
148
150 virtual const Joint* getJoint(std::size_t _idx) const = 0;
151
155 virtual Joint* getJoint(const std::string& name) = 0;
156
160 virtual const Joint* getJoint(const std::string& name) const = 0;
161
163 virtual std::vector<Joint*> getJoints() = 0;
164
166 virtual std::vector<const Joint*> getJoints() const = 0;
167
175 virtual std::vector<Joint*> getJoints(const std::string& name) = 0;
176
184 virtual std::vector<const Joint*> getJoints(
185 const std::string& name) const = 0;
186
188 virtual bool hasJoint(const Joint* joint) const = 0;
189
194 virtual std::size_t getIndexOf(const Joint* _joint, bool _warning=true) const = 0;
195
197 virtual std::size_t getNumDofs() const = 0;
198
200 virtual DegreeOfFreedom* getDof(std::size_t _idx) = 0;
201
203 virtual const DegreeOfFreedom* getDof(std::size_t _idx) const = 0;
204
206 virtual const std::vector<DegreeOfFreedom*>& getDofs() = 0;
207
209 virtual std::vector<const DegreeOfFreedom*> getDofs() const = 0;
210
215 virtual std::size_t getIndexOf(const DegreeOfFreedom* _dof,
216 bool _warning=true) const = 0;
217
219
220 //----------------------------------------------------------------------------
222 //----------------------------------------------------------------------------
223
225 void setCommand(std::size_t _index, double _command);
226
228 double getCommand(std::size_t _index) const;
229
231 void setCommands(const Eigen::VectorXd& _commands);
232
234 void setCommands(const std::vector<std::size_t>& _indices,
235 const Eigen::VectorXd& _commands);
236
238 Eigen::VectorXd getCommands() const;
239
241 Eigen::VectorXd getCommands(const std::vector<std::size_t>& _indices) const;
242
244 void resetCommands();
245
247
248 //----------------------------------------------------------------------------
250 //----------------------------------------------------------------------------
251
253 void setPosition(std::size_t index, double _position);
254
256 double getPosition(std::size_t _index) const;
257
259 void setPositions(const Eigen::VectorXd& _positions);
260
262 void setPositions(const std::vector<std::size_t>& _indices,
263 const Eigen::VectorXd& _positions);
264
266 Eigen::VectorXd getPositions() const;
267
269 Eigen::VectorXd getPositions(const std::vector<std::size_t>& _indices) const;
270
272 void resetPositions();
273
275 void setPositionLowerLimit(std::size_t _index, double _position);
276
278 void setPositionLowerLimits(const Eigen::VectorXd& positions);
279
281 void setPositionLowerLimits(const std::vector<std::size_t>& indices,
282 const Eigen::VectorXd& positions);
283
285 double getPositionLowerLimit(std::size_t _index) const;
286
288 Eigen::VectorXd getPositionLowerLimits() const;
289
291 Eigen::VectorXd getPositionLowerLimits(
292 const std::vector<std::size_t>& indices) const;
293
295 void setPositionUpperLimit(std::size_t _index, double _position);
296
298 void setPositionUpperLimits(const Eigen::VectorXd& positions);
299
301 void setPositionUpperLimits(const std::vector<std::size_t>& indices,
302 const Eigen::VectorXd& positions);
303
305 double getPositionUpperLimit(std::size_t _index) const;
306
308 Eigen::VectorXd getPositionUpperLimits() const;
309
311 Eigen::VectorXd getPositionUpperLimits(
312 const std::vector<std::size_t>& indices) const;
313
315
316 //----------------------------------------------------------------------------
318 //----------------------------------------------------------------------------
319
321 void setVelocity(std::size_t _index, double _velocity);
322
324 double getVelocity(std::size_t _index) const;
325
327 void setVelocities(const Eigen::VectorXd& _velocities);
328
330 void setVelocities(const std::vector<std::size_t>& _indices,
331 const Eigen::VectorXd& _velocities);
332
334 Eigen::VectorXd getVelocities() const;
335
337 Eigen::VectorXd getVelocities(const std::vector<std::size_t>& _indices) const;
338
340 void resetVelocities();
341
343 void setVelocityLowerLimit(std::size_t _index, double _velocity);
344
346 void setVelocityLowerLimits(const Eigen::VectorXd& velocities);
347
350 void setVelocityLowerLimits(const std::vector<std::size_t>& indices,
351 const Eigen::VectorXd& velocities);
352
354 double getVelocityLowerLimit(std::size_t _index);
355
357 Eigen::VectorXd getVelocityLowerLimits() const;
358
361 Eigen::VectorXd getVelocityLowerLimits(
362 const std::vector<std::size_t>& indices) const;
363
365 void setVelocityUpperLimit(std::size_t _index, double _velocity);
366
368 void setVelocityUpperLimits(const Eigen::VectorXd& velocities);
369
372 void setVelocityUpperLimits(const std::vector<std::size_t>& indices,
373 const Eigen::VectorXd& velocities);
374
376 double getVelocityUpperLimit(std::size_t _index);
377
379 Eigen::VectorXd getVelocityUpperLimits() const;
380
383 Eigen::VectorXd getVelocityUpperLimits(
384 const std::vector<std::size_t>& indices) const;
385
387
388 //----------------------------------------------------------------------------
390 //----------------------------------------------------------------------------
391
393 void setAcceleration(std::size_t _index, double _acceleration);
394
396 double getAcceleration(std::size_t _index) const;
397
399 void setAccelerations(const Eigen::VectorXd& _accelerations);
400
402 void setAccelerations(const std::vector<std::size_t>& _indices,
403 const Eigen::VectorXd& _accelerations);
404
406 Eigen::VectorXd getAccelerations() const;
407
409 Eigen::VectorXd getAccelerations(const std::vector<std::size_t>& _indices) const;
410
412 void resetAccelerations();
413
415 void setAccelerationLowerLimit(std::size_t _index, double _acceleration);
416
418 void setAccelerationLowerLimits(const Eigen::VectorXd& accelerations);
419
422 void setAccelerationLowerLimits(const std::vector<std::size_t>& indices,
423 const Eigen::VectorXd& accelerations);
424
426 double getAccelerationLowerLimit(std::size_t _index) const;
427
429 Eigen::VectorXd getAccelerationLowerLimits() const;
430
433 Eigen::VectorXd getAccelerationLowerLimits(
434 const std::vector<std::size_t>& indices) const;
435
437 void setAccelerationUpperLimit(std::size_t _index, double _acceleration);
438
440 void setAccelerationUpperLimits(const Eigen::VectorXd& accelerations);
441
444 void setAccelerationUpperLimits(const std::vector<std::size_t>& indices,
445 const Eigen::VectorXd& accelerations);
446
448 double getAccelerationUpperLimit(std::size_t _index) const;
449
451 Eigen::VectorXd getAccelerationUpperLimits() const;
452
455 Eigen::VectorXd getAccelerationUpperLimits(
456 const std::vector<std::size_t>& indices) const;
457
459
460 //----------------------------------------------------------------------------
462 //----------------------------------------------------------------------------
463
465 void setForce(std::size_t _index, double _force);
466
468 double getForce(std::size_t _index) const;
469
471 void setForces(const Eigen::VectorXd& _forces);
472
474 void setForces(const std::vector<std::size_t>& _index,
475 const Eigen::VectorXd& _forces);
476
478 Eigen::VectorXd getForces() const;
479
481 Eigen::VectorXd getForces(const std::vector<std::size_t>& _indices) const;
482
485
487 void setForceLowerLimit(std::size_t _index, double _force);
488
490 void setForceLowerLimits(const Eigen::VectorXd& forces);
491
493 void setForceLowerLimits(const std::vector<std::size_t>& indices,
494 const Eigen::VectorXd& forces);
495
497 double getForceLowerLimit(std::size_t _index) const;
498
500 Eigen::VectorXd getForceLowerLimits() const;
501
503 Eigen::VectorXd getForceLowerLimits(
504 const std::vector<std::size_t>& indices) const;
505
507 void setForceUpperLimit(std::size_t _index, double _force);
508
510 void setForceUpperLimits(const Eigen::VectorXd& forces);
511
513 void setForceUpperLimits(const std::vector<std::size_t>& indices,
514 const Eigen::VectorXd& forces);
515
517 double getForceUpperLimit(std::size_t _index) const;
518
520 Eigen::VectorXd getForceUpperLimits() const;
521
523 Eigen::VectorXd getForceUpperLimits(
524 const std::vector<std::size_t>& indices) const;
525
527
528 //----------------------------------------------------------------------------
530 //----------------------------------------------------------------------------
531
533 Eigen::VectorXd getVelocityChanges() const;
534
535 //----------------------------------------------------------------------------
537 //----------------------------------------------------------------------------
538
540 void setJointConstraintImpulses(const Eigen::VectorXd& _impulses);
541
543 Eigen::VectorXd getJointConstraintImpulses() const;
544
545
546 //----------------------------------------------------------------------------
548 //----------------------------------------------------------------------------
549
552 virtual math::Jacobian getJacobian(const JacobianNode* _node) const = 0;
553
557 const JacobianNode* _node,
558 const Frame* _inCoordinatesOf) const = 0;
559
564 const JacobianNode* _node,
565 const JacobianNode* _relativeTo,
566 const Frame* _inCoordinatesOf) const;
567
572 const JacobianNode* _node,
573 const Eigen::Vector3d& _localOffset) const = 0;
574
579 const JacobianNode* _node,
580 const Eigen::Vector3d& _localOffset,
581 const Frame* _inCoordinatesOf) const = 0;
582
588 const JacobianNode* _node,
589 const Eigen::Vector3d& _localOffset,
590 const JacobianNode* _relativeTo,
591 const Frame* _inCoordinatesOf) const;
592
595 virtual math::Jacobian getWorldJacobian(const JacobianNode* _node) const = 0;
596
601 const JacobianNode* _node,
602 const Eigen::Vector3d& _localOffset) const = 0;
603
607 const JacobianNode* _node,
608 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
609
614 const JacobianNode* _node,
615 const Eigen::Vector3d& _localOffset,
616 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
617
622 const JacobianNode* _node,
623 const JacobianNode* _relativeTo,
624 const Frame* _inCoordinatesOf = Frame::World()) const;
625
631 const JacobianNode* _node,
632 const Eigen::Vector3d& _localOffset,
633 const JacobianNode* _relativeTo,
634 const Frame* _inCoordinatesOf = Frame::World()) const;
635
637
638 //----------------------------------------------------------------------------
640 //----------------------------------------------------------------------------
641
645 const JacobianNode* _node,
646 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
647
652 const JacobianNode* _node,
653 const JacobianNode* _relativeTo,
654 const Frame* _inCoordinatesOf = Frame::World()) const;
655
659 const JacobianNode* _node) const = 0;
660
664 const JacobianNode* _node,
665 const Frame* _inCoordinatesOf) const = 0;
666
671 const JacobianNode* _node,
672 const Eigen::Vector3d& _localOffset) const = 0;
673
678 const JacobianNode* _node,
679 const Eigen::Vector3d& _localOffset,
680 const Frame* _inCoordinatesOf) const = 0;
681
686 const JacobianNode* _node,
687 const JacobianNode* _relativeTo,
688 const Frame* _inCoordinatesOf) const;
689
695 const JacobianNode* _node,
696 const Eigen::Vector3d& _localOffset,
697 const JacobianNode* _relativeTo,
698 const Frame* _inCoordinatesOf) const;
699
703 const JacobianNode* _node) const = 0;
704
709 const JacobianNode* _node,
710 const Frame* _inCoordinatesOf) const = 0;
711
716 const JacobianNode* _node,
717 const Eigen::Vector3d& _localOffset,
718 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
719
723 const JacobianNode* _node,
724 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
725
730 const JacobianNode* _node,
731 const Eigen::Vector3d& _localOffset,
732 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
733
737 const JacobianNode* _node,
738 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
739
741
742 //----------------------------------------------------------------------------
744 //----------------------------------------------------------------------------
745
749 virtual double getMass() const = 0;
750
752 virtual const Eigen::MatrixXd& getMassMatrix() const = 0;
753
758 virtual const Eigen::MatrixXd& getAugMassMatrix() const = 0;
759
761 virtual const Eigen::MatrixXd& getInvMassMatrix() const = 0;
762
764 virtual const Eigen::MatrixXd& getInvAugMassMatrix() const = 0;
765
767 virtual const Eigen::VectorXd& getCoriolisForces() const = 0;
768
770 virtual const Eigen::VectorXd& getGravityForces() const = 0;
771
773 virtual const Eigen::VectorXd& getCoriolisAndGravityForces() const = 0;
774
776 virtual const Eigen::VectorXd& getExternalForces() const = 0;
777
779 virtual const Eigen::VectorXd& getConstraintForces() const = 0;
780
782 virtual void clearExternalForces() = 0;
783
785 virtual void clearInternalForces() = 0;
786
788 double computeLagrangian() const;
789
791 DART_DEPRECATED(6.1)
792 double getKineticEnergy() const;
793
795 virtual double computeKineticEnergy() const = 0;
796
798 DART_DEPRECATED(6.1)
799 double getPotentialEnergy() const;
800
802 virtual double computePotentialEnergy() const = 0;
803
805 DART_DEPRECATED(6.0)
806 virtual void clearCollidingBodies() = 0;
807
809
810 //----------------------------------------------------------------------------
812 //----------------------------------------------------------------------------
813
816 virtual Eigen::Vector3d getCOM(
817 const Frame* _withRespectTo = Frame::World()) const = 0;
818
821 virtual Eigen::Vector6d getCOMSpatialVelocity(
822 const Frame* _relativeTo = Frame::World(),
823 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
824
827 virtual Eigen::Vector3d getCOMLinearVelocity(
828 const Frame* _relativeTo = Frame::World(),
829 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
830
834 const Frame* _relativeTo = Frame::World(),
835 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
836
840 const Frame* _relativeTo = Frame::World(),
841 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
842
845 virtual math::Jacobian getCOMJacobian(
846 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
847
850 virtual math::LinearJacobian getCOMLinearJacobian(
851 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
852
859 virtual math::Jacobian getCOMJacobianSpatialDeriv(
860 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
861
868 virtual math::LinearJacobian getCOMLinearJacobianDeriv(
869 const Frame* _inCoordinatesOf = Frame::World()) const = 0;
870
872
873protected:
874
876 MetaSkeleton();
877
878 //--------------------------------------------------------------------------
879 // Signals
880 //--------------------------------------------------------------------------
882
883public:
884
885 //--------------------------------------------------------------------------
886 // Slot registers
887 //--------------------------------------------------------------------------
888 common::SlotRegister<NameChangedSignal> onNameChanged;
889};
890
891} // namespace dynamics
892} // namespace dart
893
894
895#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:814
void setAccelerationLowerLimits(const Eigen::VectorXd &accelerations)
Set the lower limits for all generalized coordinates's acceleration.
Definition MetaSkeleton.cpp:698
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:361
double getPotentialEnergy() const
Get the potential energy of this MetaSkeleton.
Definition MetaSkeleton.cpp:1080
Eigen::VectorXd getAccelerationUpperLimits() const
Get the upper limits for all generalized coordinates's acceleration.
Definition MetaSkeleton.cpp:770
void setVelocityLowerLimits(const Eigen::VectorXd &velocities)
Set the lower limits for all generalized coordinates's velocity.
Definition MetaSkeleton.cpp:560
double getPositionUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's position.
Definition MetaSkeleton.cpp:482
void setVelocityUpperLimits(const Eigen::VectorXd &velocities)
Set the upper limits for all generalized coordinates's velocity.
Definition MetaSkeleton.cpp:604
double getAccelerationUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's acceleration.
Definition MetaSkeleton.cpp:763
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:887
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:1074
virtual ~MetaSkeleton()=default
Default destructor.
void setAcceleration(std::size_t _index, double _acceleration)
Set the acceleration of a single generalized coordinate.
Definition MetaSkeleton.cpp:641
void setAccelerationUpperLimit(std::size_t _index, double _acceleration)
Set the upper limit of a generalized coordinate's acceleration.
Definition MetaSkeleton.cpp:738
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:347
double getForce(std::size_t _index) const
Get the force of a single generalized coordinate.
Definition MetaSkeleton.cpp:792
void setForce(std::size_t _index, double _force)
Set the force of a single generalized coordinate.
Definition MetaSkeleton.cpp:785
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:685
NameChangedSignal mNameChangedSignal
Definition MetaSkeleton.hpp:881
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:410
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:489
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:367
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:843
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:716
double getForceUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's force.
Definition MetaSkeleton.cpp:902
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:445
void setAccelerationUpperLimits(const Eigen::VectorXd &accelerations)
Set the upper limits for all generalized coordinates's acceleration.
Definition MetaSkeleton.cpp:745
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:511
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:467
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:828
void setCommand(std::size_t _index, double _command)
Set a single command.
Definition MetaSkeleton.cpp:318
void setVelocityUpperLimit(std::size_t _index, double _velocity)
Set the upper limit of a generalized coordinate's velocity.
Definition MetaSkeleton.cpp:597
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:691
double getForceLowerLimit(std::size_t _index) const
Get the lower limit of a generalized coordinate's force.
Definition MetaSkeleton.cpp:858
void resetVelocities()
Set all velocities to zero.
Definition MetaSkeleton.cpp:547
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:626
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:582
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:799
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:518
Eigen::VectorXd getVelocities() const
Get the velocities for all generalized coordinates.
Definition MetaSkeleton.cpp:533
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:438
void setPositionLowerLimit(std::size_t _index, double _position)
Set the lower limit of a generalized coordinate's position.
Definition MetaSkeleton.cpp:416
void setPositions(const Eigen::VectorXd &_positions)
Set the positions for all generalized coordinates.
Definition MetaSkeleton.cpp:381
double getPosition(std::size_t _index) const
Get the position of a single generalized coordinate.
Definition MetaSkeleton.cpp:374
void setCommands(const Eigen::VectorXd &_commands)
Set commands for all generalized coordinates.
Definition MetaSkeleton.cpp:332
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:865
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:909
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:723
Eigen::VectorXd getPositions() const
Get the positions for all generalized coordinates.
Definition MetaSkeleton.cpp:396
double getAcceleration(std::size_t _index) const
Get the acceleration of a single generalized coordinate.
Definition MetaSkeleton.cpp:648
void setPositionLowerLimits(const Eigen::VectorXd &positions)
Set the lower limits for all generalized coordinates.
Definition MetaSkeleton.cpp:423
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:619
void setForceUpperLimit(std::size_t _index, double _force)
Set the upper limit of a generalized coordinate's force.
Definition MetaSkeleton.cpp:880
void setForceLowerLimit(std::size_t _index, double _force)
Set the lower limit of a generalized coordinate's force.
Definition MetaSkeleton.cpp:836
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:655
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:924
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:460
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:888
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:553
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:575
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:325
Eigen::VectorXd getAccelerations() const
Get the accelerations for all generalized coordinates.
Definition MetaSkeleton.cpp:670
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:1068
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 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:931
Eigen::VectorXd getJointConstraintImpulses() const
Get the constraint impulses for the generalized coordinates.
Definition MetaSkeleton.cpp:938
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:504
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 MathTypes.hpp:47
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