DART  6.6.2
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 
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 
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 
484  void resetGeneralizedForces();
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 
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 
839  virtual Eigen::Vector3d getCOMLinearAcceleration(
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 
851  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
852 
860  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
861 
869  const Frame* _inCoordinatesOf = Frame::World()) const = 0;
870 
872 
873 protected:
874 
876  MetaSkeleton();
877 
878  //--------------------------------------------------------------------------
879  // Signals
880  //--------------------------------------------------------------------------
882 
883 public:
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 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
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
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: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< 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: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 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:716
double getForceUpperLimit(std::size_t _index) const
Get the upper limit of a generalized coordinate's force.
Definition: MetaSkeleton.cpp:902
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::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:467
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
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: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
Eigen::VectorXd getVelocityLowerLimits() const
Get the lower limits for all generalized coordinates's velocity.
Definition: MetaSkeleton.cpp:582
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:799
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:518
Eigen::VectorXd getVelocities() const
Get the velocities for all generalized coordinates.
Definition: MetaSkeleton.cpp:533
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
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: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 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: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
Eigen::VectorXd getAccelerationLowerLimits() const
Get the lower limits for all generalized coordinates's acceleration.
Definition: MetaSkeleton.cpp:723
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: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 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:619
void setForceUpperLimit(std::size_t _index, double _force)
Set the upper limit of a generalized coordinate's force.
Definition: MetaSkeleton.cpp:880
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:836
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: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.
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: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 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: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 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:575
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:325
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: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 const Eigen::MatrixXd & getMassMatrix() const =0
Get the Mass Matrix of the 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: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 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:504
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: MathTypes.hpp:47
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:108
Eigen::Matrix< double, 3, Eigen::Dynamic > AngularJacobian
Definition: MathTypes.hpp:107
Eigen::Matrix< double, 3, Eigen::Dynamic > LinearJacobian
Definition: MathTypes.hpp:106
Definition: BulletCollisionDetector.cpp:63