DART
6.7.3
|
#include <Skeleton.hpp>
Classes | |
struct | Configuration |
The Configuration struct represents the joint configuration of a Skeleton. More... | |
struct | DataCache |
struct | DirtyFlags |
Public Types | |
enum | ConfigFlags { CONFIG_NOTHING = 0 , CONFIG_POSITIONS = 1 << 1 , CONFIG_VELOCITIES = 1 << 2 , CONFIG_ACCELERATIONS = 1 << 3 , CONFIG_FORCES = 1 << 4 , CONFIG_COMMANDS = 1 << 5 , CONFIG_ALL = 0xFF } |
using | AspectPropertiesData = detail::SkeletonAspectProperties |
using | AspectProperties = common::Aspect::MakeProperties< AspectPropertiesData > |
using | State = common::Composite::State |
using | Properties = common::Composite::Properties |
using | NameChangedSignal = common::Signal< void(std::shared_ptr< const MetaSkeleton > _skeleton, const std::string &_oldName, const std::string &_newName)> |
using | Impl = EmbedProperties< DerivedT, PropertiesDataT > |
using | Derived = typename Impl::Derived |
using | Aspect = typename Impl::Aspect |
using | Base = CompositeJoiner< Impl, CompositeBases... > |
Public Member Functions | |
void | integratePositions (double _dt) |
void | integrateVelocities (double _dt) |
Eigen::VectorXd | getPositionDifferences (const Eigen::VectorXd &_q2, const Eigen::VectorXd &_q1) const |
Return the difference of two generalized positions which are measured in the configuration space of this Skeleton. More... | |
Eigen::VectorXd | getVelocityDifferences (const Eigen::VectorXd &_dq2, const Eigen::VectorXd &_dq1) const |
Return the difference of two generalized velocities or accelerations which are measured in the tangent space at the identity. More... | |
void | computeForwardKinematics (bool _updateTransforms=true, bool _updateVels=true, bool _updateAccs=true) |
Compute forward kinematics. More... | |
void | computeForwardDynamics () |
Compute forward dynamics. More... | |
void | computeInverseDynamics (bool _withExternalForces=false, bool _withDampingForces=false, bool _withSpringForces=false) |
Compute inverse dynamics. More... | |
void | clearConstraintImpulses () |
Clear constraint impulses and cache data used for impulse-based forward dynamics algorithm, where the constraint impulses are spatial constraints on the BodyNodes and generalized constraints on the Joints. More... | |
void | updateBiasImpulse (BodyNode *_bodyNode) |
Update bias impulses. More... | |
void | updateBiasImpulse (BodyNode *_bodyNode, const Eigen::Vector6d &_imp) |
Update bias impulses due to impulse [_imp] on body node [_bodyNode]. More... | |
void | updateBiasImpulse (BodyNode *_bodyNode1, const Eigen::Vector6d &_imp1, BodyNode *_bodyNode2, const Eigen::Vector6d &_imp2) |
Update bias impulses due to impulse [_imp] on body node [_bodyNode]. More... | |
void | updateBiasImpulse (SoftBodyNode *_softBodyNode, PointMass *_pointMass, const Eigen::Vector3d &_imp) |
Update bias impulses due to impulse[_imp] on body node [_bodyNode]. More... | |
void | updateVelocityChange () |
Update velocity changes in body nodes and joints due to applied impulse. More... | |
void | setImpulseApplied (bool _val) |
Set whether this skeleton is constrained. More... | |
bool | isImpulseApplied () const |
Get whether this skeleton is constrained. More... | |
void | computeImpulseForwardDynamics () |
Compute impulse-based forward dynamics. More... | |
void | resetUnion () |
virtual math::Jacobian | getJacobian (const JacobianNode *_node) const=0 |
Get the spatial Jacobian targeting the origin of a BodyNode. More... | |
virtual math::Jacobian | getJacobian (const JacobianNode *_node, const Frame *_inCoordinatesOf) const=0 |
Get the spatial Jacobian targeting the origin of a BodyNode. More... | |
math::Jacobian | getJacobian (const JacobianNode *_node, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial Jacobian targeting the origin of a BodyNode relative to another BodyNode in the same Skeleton. More... | |
virtual math::Jacobian | getJacobian (const JacobianNode *_node, const Eigen::Vector3d &_localOffset) const=0 |
Get the spatial Jacobian targeting an offset in a BodyNode. More... | |
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. More... | |
math::Jacobian | getJacobian (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial Jacobian targeting an offset in a BodyNode relative to another BodyNode in the same Skeleton. More... | |
virtual math::LinearJacobian | getLinearJacobian (const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const=0 |
Get the linear Jacobian targeting the origin of a BodyNode. More... | |
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. More... | |
math::LinearJacobian | getLinearJacobian (const JacobianNode *_node, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf=Frame::World()) const |
Get the linear Jacobian targeting the origin of a BodyNode relative to another BodyNode in the same Skeleton. More... | |
math::LinearJacobian | getLinearJacobian (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf=Frame::World()) const |
Get the linear Jacobian targeting an offset in a BodyNode relative to another BodyNode in the same Skeleton. More... | |
virtual math::AngularJacobian | getAngularJacobian (const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const=0 |
Get the angular Jacobian of a BodyNode. More... | |
math::AngularJacobian | getAngularJacobian (const JacobianNode *_node, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf=Frame::World()) const |
Get the angular Jacobian of a BodyNode relative to another BodyNode in the same Skeleton. More... | |
virtual math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node) const=0 |
Get the spatial Jacobian time derivative targeting the origin of a BodyNode. More... | |
virtual math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node, const Frame *_inCoordinatesOf) const=0 |
Get the spatial Jacobian time derivative targeting the origin of a BodyNode. More... | |
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. More... | |
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. More... | |
math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial Jacobian time derivative targeting the origin of a BodyNode relative to another BodyNode in the same Skeleton. More... | |
math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial Jacobian time derivative targeting an offset in a BodyNode relative to another Bodynode in the same Skeleton. More... | |
virtual math::Jacobian | getJacobianClassicDeriv (const JacobianNode *_node) const=0 |
Get the spatial Jacobian (classical) time derivative targeting the origin of a BodyNode. More... | |
virtual math::Jacobian | getJacobianClassicDeriv (const JacobianNode *_node, const Frame *_inCoordinatesOf) const=0 |
Get the spatial Jacobian (classical) time derivative targeting the origin a BodyNode. More... | |
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. More... | |
virtual math::LinearJacobian | getLinearJacobianDeriv (const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const=0 |
of a BodyNode. More... | |
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. More... | |
virtual math::AngularJacobian | getAngularJacobianDeriv (const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const=0 |
Get the angular Jacobian time derivative of a BodyNode. More... | |
virtual std::size_t | incrementVersion () |
Increment the version for this object. More... | |
virtual std::size_t | getVersion () const |
Get the version number of this object. More... | |
MetaSkeletonPtr | cloneMetaSkeleton () const |
Creates an identical clone of this MetaSkeleton. More... | |
const AspectProperties & | getAspectProperties () const |
Configuration | |
void | setConfiguration (const Configuration &configuration) |
Set the configuration of this Skeleton. More... | |
Configuration | getConfiguration (int flags=CONFIG_ALL) const |
Get the configuration of this Skeleton. More... | |
Configuration | getConfiguration (const std::vector< std::size_t > &indices, int flags=CONFIG_ALL) const |
Get the configuration of the specified indices in this Skeleton. More... | |
State | |
void | setState (const State &state) |
Set the State of this Skeleton [alias for setCompositeState(~)]. More... | |
State | getState () const |
Get the State of this Skeleton [alias for getCompositeState()]. More... | |
Properties | |
void | setProperties (const Properties &properties) |
Set all properties of this Skeleton. More... | |
Properties | getProperties () const |
Get all properties of this Skeleton. More... | |
void | setProperties (const AspectProperties &properties) |
Set the Properties of this Skeleton. More... | |
const AspectProperties & | getSkeletonProperties () const |
Get the Properties of this Skeleton. More... | |
void | setAspectProperties (const AspectProperties &properties) |
Set the AspectProperties of this Skeleton. More... | |
const std::string & | setName (const std::string &_name) override |
Set name. More... | |
const std::string & | getName () const override |
Get name. More... | |
void | enableSelfCollision (bool enableAdjacentBodyCheck=false) |
Deprecated. More... | |
void | disableSelfCollision () |
Deprecated. Please use disableSelfCollisionCheck() instead. More... | |
void | setSelfCollisionCheck (bool enable) |
Set whether to check self-collision. More... | |
bool | getSelfCollisionCheck () const |
Return whether self-collision check is enabled. More... | |
void | enableSelfCollisionCheck () |
Enable self-collision check. More... | |
void | disableSelfCollisionCheck () |
Disable self-collision check. More... | |
bool | isEnabledSelfCollisionCheck () const |
Return true if self-collision check is enabled. More... | |
void | setAdjacentBodyCheck (bool enable) |
Set whether to check adjacent bodies. More... | |
bool | getAdjacentBodyCheck () const |
Return whether adjacent body check is enabled. More... | |
void | enableAdjacentBodyCheck () |
Enable collision check for adjacent bodies. More... | |
void | disableAdjacentBodyCheck () |
Disable collision check for adjacent bodies. More... | |
bool | isEnabledAdjacentBodyCheck () const |
Return true if self-collision check is enabled including adjacent bodies. More... | |
void | setMobile (bool _isMobile) |
Set whether this skeleton will be updated by forward dynamics. More... | |
bool | isMobile () const |
Get whether this skeleton will be updated by forward dynamics. More... | |
void | setTimeStep (double _timeStep) |
Set time step. More... | |
double | getTimeStep () const |
Get time step. More... | |
void | setGravity (const Eigen::Vector3d &_gravity) |
Set 3-dim gravitational acceleration. More... | |
const Eigen::Vector3d & | getGravity () const |
Get 3-dim gravitational acceleration. More... | |
Structural Properties | |
template<class JointType , class NodeType = BodyNode> | |
std::pair< JointType *, NodeType * > | createJointAndBodyNodePair (BodyNode *_parent=nullptr, const typename JointType::Properties &_jointProperties=typename JointType::Properties(), const typename NodeType::Properties &_bodyProperties=typename NodeType::Properties()) |
Create a Joint and child BodyNode pair of the given types. More... | |
std::size_t | getNumBodyNodes () const override |
Get number of body nodes. More... | |
std::size_t | getNumRigidBodyNodes () const |
Get number of rigid body nodes. More... | |
std::size_t | getNumSoftBodyNodes () const |
Get number of soft body nodes. More... | |
std::size_t | getNumTrees () const |
Get the number of independent trees that this Skeleton contains. More... | |
BodyNode * | getRootBodyNode (std::size_t _treeIdx=0) |
Get the root BodyNode of the tree whose index in this Skeleton is _treeIdx. More... | |
const BodyNode * | getRootBodyNode (std::size_t _treeIdx=0) const |
Get the const root BodyNode of the tree whose index in this Skeleton is _treeIdx. More... | |
Joint * | getRootJoint (std::size_t treeIdx=0u) |
Get the root Joint of the tree whose index in this Skeleton is treeIdx. More... | |
const Joint * | getRootJoint (std::size_t treeIdx=0u) const |
Get the const root Joint of the tree whose index in this Skeleton is treeIdx. More... | |
BodyNode * | getBodyNode (std::size_t _idx) override |
Get BodyNode whose index is _idx. More... | |
const BodyNode * | getBodyNode (std::size_t _idx) const override |
Get const BodyNode whose index is _idx. More... | |
SoftBodyNode * | getSoftBodyNode (std::size_t _idx) |
Get SoftBodyNode whose index is _idx. More... | |
const SoftBodyNode * | getSoftBodyNode (std::size_t _idx) const |
Get const SoftBodyNode whose index is _idx. More... | |
BodyNode * | getBodyNode (const std::string &name) override |
Returns the BodyNode of given name. More... | |
const BodyNode * | getBodyNode (const std::string &name) const override |
Returns the BodyNode of given name. More... | |
SoftBodyNode * | getSoftBodyNode (const std::string &_name) |
Get soft body node whose name is _name. More... | |
const SoftBodyNode * | getSoftBodyNode (const std::string &_name) const |
Get const soft body node whose name is _name. More... | |
const std::vector< BodyNode * > & | getBodyNodes () override |
Get all the BodyNodes that are held by this MetaSkeleton. More... | |
const std::vector< const BodyNode * > & | getBodyNodes () const override |
Get all the BodyNodes that are held by this MetaSkeleton. More... | |
std::vector< BodyNode * > | getBodyNodes (const std::string &name) override |
Returns all the BodyNodes of given name. More... | |
std::vector< const BodyNode * > | getBodyNodes (const std::string &name) const override |
Returns all the BodyNodes of given name. More... | |
bool | hasBodyNode (const BodyNode *bodyNode) const override |
Returns whether this Skeleton contains bodyNode . More... | |
std::size_t | getIndexOf (const BodyNode *_bn, bool _warning=true) const override |
Get the index of a specific BodyNode within this ReferentialSkeleton. More... | |
const std::vector< BodyNode * > & | getTreeBodyNodes (std::size_t _treeIdx) |
Get the BodyNodes belonging to a tree in this Skeleton. More... | |
std::vector< const BodyNode * > | getTreeBodyNodes (std::size_t _treeIdx) const |
Get the BodyNodes belonging to a tree in this Skeleton. More... | |
std::size_t | getNumJoints () const override |
Get number of Joints. More... | |
Joint * | getJoint (std::size_t _idx) override |
Get Joint whose index is _idx. More... | |
const Joint * | getJoint (std::size_t _idx) const override |
Get const Joint whose index is _idx. More... | |
Joint * | getJoint (const std::string &name) override |
Returns the Joint of given name. More... | |
const Joint * | getJoint (const std::string &name) const override |
Returns the joint of given name. More... | |
std::vector< Joint * > | getJoints () override |
Returns all the joints that are held by this MetaSkeleton. More... | |
std::vector< const Joint * > | getJoints () const override |
Returns all the joints that are held by this MetaSkeleton. More... | |
std::vector< Joint * > | getJoints (const std::string &name) override |
Returns all the Joint of given name. More... | |
std::vector< const Joint * > | getJoints (const std::string &name) const override |
Returns all the Joint of given name. More... | |
bool | hasJoint (const Joint *joint) const override |
Returns whether this Skeleton contains join . More... | |
std::size_t | getIndexOf (const Joint *_joint, bool _warning=true) const override |
Get the index of a specific Joint within this ReferentialSkeleton. More... | |
std::size_t | getNumDofs () const override |
Return the number of degrees of freedom in this skeleton. More... | |
DegreeOfFreedom * | getDof (std::size_t _idx) override |
Get degree of freedom (aka generalized coordinate) whose index is _idx. More... | |
const DegreeOfFreedom * | getDof (std::size_t _idx) const override |
Get degree of freedom (aka generalized coordinate) whose index is _idx. More... | |
DegreeOfFreedom * | getDof (const std::string &_name) |
Get degree of freedom (aka generalized coordinate) whose name is _name. More... | |
const DegreeOfFreedom * | getDof (const std::string &_name) const |
Get degree of freedom (aka generalized coordinate) whose name is _name. More... | |
const std::vector< DegreeOfFreedom * > & | getDofs () override |
Get the vector of DegreesOfFreedom for this MetaSkeleton. More... | |
std::vector< const DegreeOfFreedom * > | getDofs () const override |
Get a vector of const DegreesOfFreedom for this MetaSkeleton. More... | |
std::size_t | getIndexOf (const DegreeOfFreedom *_dof, bool _warning=true) const override |
Get the index of a specific DegreeOfFreedom within this ReferentialSkeleton. More... | |
const std::vector< DegreeOfFreedom * > & | getTreeDofs (std::size_t _treeIdx) |
Get the DegreesOfFreedom belonging to a tree in this Skeleton. More... | |
const std::vector< const DegreeOfFreedom * > & | getTreeDofs (std::size_t _treeIdx) const |
Get the DegreesOfFreedom belonging to a tree in this Skeleton. More... | |
bool | checkIndexingConsistency () const |
This function is only meant for debugging purposes. More... | |
const std::shared_ptr< WholeBodyIK > & | getIK (bool _createIfNull=false) |
Get a pointer to a WholeBodyIK module for this Skeleton. More... | |
const std::shared_ptr< WholeBodyIK > & | getOrCreateIK () |
Get a pointer to a WholeBodyIK module for this Skeleton. More... | |
std::shared_ptr< const WholeBodyIK > | getIK () const |
Get a pointer to a WholeBodyIK module for this Skeleton. More... | |
const std::shared_ptr< WholeBodyIK > & | createIK () |
Create a new WholeBodyIK module for this Skeleton. More... | |
void | clearIK () |
Wipe away the WholeBodyIK module for this Skeleton, leaving it as a nullptr. More... | |
std::size_t | getNumMarkers () const |
Marker * | getMarker (std::size_t index) |
const Marker * | getMarker (std::size_t index) const |
std::size_t | getNumMarkers (std::size_t treeIndex) const |
Marker * | getMarker (std::size_t treeIndex, std::size_t nodeIndex) |
const Marker * | getMarker (std::size_t treeIndex, std::size_t nodeIndex) const |
Marker * | getMarker (const std::string &name) |
const Marker * | getMarker (const std::string &name) const |
std::size_t | getNumShapeNodes () const |
ShapeNode * | getShapeNode (std::size_t index) |
const ShapeNode * | getShapeNode (std::size_t index) const |
std::size_t | getNumShapeNodes (std::size_t treeIndex) const |
ShapeNode * | getShapeNode (std::size_t treeIndex, std::size_t nodeIndex) |
const ShapeNode * | getShapeNode (std::size_t treeIndex, std::size_t nodeIndex) const |
ShapeNode * | getShapeNode (const std::string &name) |
const ShapeNode * | getShapeNode (const std::string &name) const |
std::size_t | getNumEndEffectors () const |
EndEffector * | getEndEffector (std::size_t index) |
const EndEffector * | getEndEffector (std::size_t index) const |
std::size_t | getNumEndEffectors (std::size_t treeIndex) const |
EndEffector * | getEndEffector (std::size_t treeIndex, std::size_t nodeIndex) |
const EndEffector * | getEndEffector (std::size_t treeIndex, std::size_t nodeIndex) const |
EndEffector * | getEndEffector (const std::string &name) |
const EndEffector * | getEndEffector (const std::string &name) const |
Support Polygon | |
const math::SupportPolygon & | getSupportPolygon () const |
Get the support polygon of this Skeleton, which is computed based on the gravitational projection of the support geometries of all EndEffectors in this Skeleton that are currently in support mode. More... | |
const math::SupportPolygon & | getSupportPolygon (std::size_t _treeIdx) const |
Same as getSupportPolygon(), but it will only use EndEffectors within the specified tree within this Skeleton. More... | |
const std::vector< std::size_t > & | getSupportIndices () const |
Get a list of the EndEffector indices that correspond to each of the points in the support polygon. More... | |
const std::vector< std::size_t > & | getSupportIndices (std::size_t _treeIdx) const |
Same as getSupportIndices(), but it corresponds to the support polygon of the specified tree within this Skeleton. More... | |
const std::pair< Eigen::Vector3d, Eigen::Vector3d > & | getSupportAxes () const |
Get the axes that correspond to each component in the support polygon. More... | |
const std::pair< Eigen::Vector3d, Eigen::Vector3d > & | getSupportAxes (std::size_t _treeIdx) const |
Same as getSupportAxes(), but it corresponds to the support polygon of the specified tree within this Skeleton. More... | |
const Eigen::Vector2d & | getSupportCentroid () const |
Get the centroid of the support polygon for this Skeleton. More... | |
const Eigen::Vector2d & | getSupportCentroid (std::size_t _treeIdx) const |
Get the centroid of the support polygon for a tree in this Skeleton. More... | |
std::size_t | getSupportVersion () const |
The version number of a support polygon will be incremented each time the support polygon needs to be recomputed. More... | |
std::size_t | getSupportVersion (std::size_t _treeIdx) const |
Same as getSupportVersion(), but it corresponds to the support polygon of the specified tree within this Skeleton. More... | |
Jacobians | |
math::Jacobian | getJacobian (const JacobianNode *_node) const override |
Get the spatial Jacobian targeting the origin of a BodyNode. More... | |
math::Jacobian | getJacobian (const JacobianNode *_node, const Frame *_inCoordinatesOf) const override |
Get the spatial Jacobian targeting the origin of a BodyNode. More... | |
math::Jacobian | getJacobian (const JacobianNode *_node, const Eigen::Vector3d &_localOffset) const override |
Get the spatial Jacobian targeting an offset in a BodyNode. More... | |
math::Jacobian | getJacobian (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf) const override |
Get the spatial Jacobian targeting an offset in a BodyNode. More... | |
math::Jacobian | getWorldJacobian (const JacobianNode *_node) const override |
Get the spatial Jacobian targeting the origin of a BodyNode. More... | |
math::Jacobian | getWorldJacobian (const JacobianNode *_node, const Eigen::Vector3d &_localOffset) const override |
Get the spatial Jacobian targeting an offset in a BodyNode. More... | |
math::LinearJacobian | getLinearJacobian (const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the linear Jacobian targeting the origin of a BodyNode. More... | |
math::LinearJacobian | getLinearJacobian (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the linear Jacobian targeting an offset in a BodyNode. More... | |
math::AngularJacobian | getAngularJacobian (const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the angular Jacobian of a BodyNode. More... | |
math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node) const override |
Get the spatial Jacobian time derivative targeting the origin of a BodyNode. More... | |
math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node, const Frame *_inCoordinatesOf) const override |
Get the spatial Jacobian time derivative targeting the origin of a BodyNode. More... | |
math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node, const Eigen::Vector3d &_localOffset) const override |
Get the spatial Jacobian time derivative targeting an offset in a BodyNode. More... | |
math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf) const override |
Get the spatial Jacobian time derivative targeting an offset in a BodyNode. More... | |
math::Jacobian | getJacobianClassicDeriv (const JacobianNode *_node) const override |
Get the spatial Jacobian (classical) time derivative targeting the origin of a BodyNode. More... | |
math::Jacobian | getJacobianClassicDeriv (const JacobianNode *_node, const Frame *_inCoordinatesOf) const override |
Get the spatial Jacobian (classical) time derivative targeting the origin a BodyNode. More... | |
math::Jacobian | getJacobianClassicDeriv (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the spatial Jacobian (classical) time derivative targeting an offset in a BodyNode. More... | |
math::LinearJacobian | getLinearJacobianDeriv (const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override |
of a BodyNode. More... | |
math::LinearJacobian | getLinearJacobianDeriv (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the linear Jacobian (classical) time derivative targeting an offset in a BodyNode. More... | |
math::AngularJacobian | getAngularJacobianDeriv (const JacobianNode *_node, const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the angular Jacobian time derivative of a BodyNode. More... | |
Equations of Motion | |
double | getMass () const override |
Get total mass of the skeleton. More... | |
const Eigen::MatrixXd & | getMassMatrix (std::size_t _treeIdx) const |
Get the mass matrix of a specific tree in the Skeleton. More... | |
const Eigen::MatrixXd & | getMassMatrix () const override |
Get the Mass Matrix of the MetaSkeleton. More... | |
const Eigen::MatrixXd & | getAugMassMatrix (std::size_t _treeIdx) const |
Get the augmented mass matrix of a specific tree in the Skeleton. More... | |
const Eigen::MatrixXd & | getAugMassMatrix () const override |
Get augmented mass matrix of the skeleton. More... | |
const Eigen::MatrixXd & | getInvMassMatrix (std::size_t _treeIdx) const |
Get the inverse mass matrix of a specific tree in the Skeleton. More... | |
const Eigen::MatrixXd & | getInvMassMatrix () const override |
Get inverse of Mass Matrix of the MetaSkeleton. More... | |
const Eigen::MatrixXd & | getInvAugMassMatrix (std::size_t _treeIdx) const |
Get the inverse augmented mass matrix of a tree. More... | |
const Eigen::MatrixXd & | getInvAugMassMatrix () const override |
Get inverse of augmented Mass Matrix of the MetaSkeleton. More... | |
const Eigen::VectorXd & | getCoriolisForces (std::size_t _treeIdx) const |
Get the Coriolis force vector of a tree in this Skeleton. More... | |
const Eigen::VectorXd & | getCoriolisForces () const override |
Get Coriolis force vector of the MetaSkeleton's BodyNodes. More... | |
const Eigen::VectorXd & | getGravityForces (std::size_t _treeIdx) const |
Get the gravity forces for a tree in this Skeleton. More... | |
const Eigen::VectorXd & | getGravityForces () const override |
Get gravity force vector of the MetaSkeleton. More... | |
const Eigen::VectorXd & | getCoriolisAndGravityForces (std::size_t _treeIdx) const |
Get the combined vector of Coriolis force and gravity force of a tree. More... | |
const Eigen::VectorXd & | getCoriolisAndGravityForces () const override |
Get combined vector of Coriolis force and gravity force of the MetaSkeleton. More... | |
const Eigen::VectorXd & | getExternalForces (std::size_t _treeIdx) const |
Get the external force vector of a tree in the Skeleton. More... | |
const Eigen::VectorXd & | getExternalForces () const override |
Get external force vector of the MetaSkeleton. More... | |
const Eigen::VectorXd & | getConstraintForces (std::size_t _treeIdx) const |
Get damping force of the skeleton. More... | |
const Eigen::VectorXd & | getConstraintForces () const override |
Get constraint force vector. More... | |
void | clearExternalForces () override |
Clear the external forces of the BodyNodes in this MetaSkeleton. More... | |
void | clearInternalForces () override |
Clear the internal forces of the BodyNodes in this MetaSkeleton. More... | |
void | notifyArticulatedInertiaUpdate (std::size_t _treeIdx) |
Notify that the articulated inertia and everything that depends on it needs to be updated. More... | |
void | dirtyArticulatedInertia (std::size_t _treeIdx) |
Notify that the articulated inertia and everything that depends on it needs to be updated. More... | |
void | notifySupportUpdate (std::size_t _treeIdx) |
Notify that the support polygon of a tree needs to be updated. More... | |
void | dirtySupportPolygon (std::size_t _treeIdx) |
Notify that the support polygon of a tree needs to be updated. More... | |
double | computeKineticEnergy () const override |
Get the kinetic energy of this MetaSkeleton. More... | |
double | computePotentialEnergy () const override |
Get the potential energy of this MetaSkeleton. More... | |
void | clearCollidingBodies () override |
Clear collision flags of the BodyNodes in this MetaSkeleton. More... | |
Center of Mass Jacobian | |
Eigen::Vector3d | getCOM (const Frame *_withRespectTo=Frame::World()) const override |
Get the Skeleton's COM with respect to any Frame (default is World Frame) More... | |
Eigen::Vector6d | getCOMSpatialVelocity (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the Skeleton's COM spatial velocity in terms of any Frame (default is World Frame) More... | |
Eigen::Vector3d | getCOMLinearVelocity (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the Skeleton's COM linear velocity in terms of any Frame (default is World Frame) More... | |
Eigen::Vector6d | getCOMSpatialAcceleration (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the Skeleton's COM spatial acceleration in terms of any Frame (default is World Frame) More... | |
Eigen::Vector3d | getCOMLinearAcceleration (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the Skeleton's COM linear acceleration in terms of any Frame (default is World Frame) More... | |
math::Jacobian | getCOMJacobian (const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the Skeleton's COM Jacobian in terms of any Frame (default is World Frame) More... | |
math::LinearJacobian | getCOMLinearJacobian (const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the Skeleton's COM Linear Jacobian in terms of any Frame (default is World Frame) More... | |
math::Jacobian | getCOMJacobianSpatialDeriv (const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the Skeleton's COM Jacobian spatial time derivative in terms of any Frame (default is World Frame). More... | |
math::LinearJacobian | getCOMLinearJacobianDeriv (const Frame *_inCoordinatesOf=Frame::World()) const override |
Get the Skeleton's COM Linear Jacobian time derivative in terms of any Frame (default is World Frame). More... | |
Command | |
void | setCommand (std::size_t _index, double _command) |
Set a single command. More... | |
double | getCommand (std::size_t _index) const |
Get a single command. More... | |
void | setCommands (const Eigen::VectorXd &_commands) |
Set commands for all generalized coordinates. More... | |
void | setCommands (const std::vector< std::size_t > &_indices, const Eigen::VectorXd &_commands) |
Set commands for a subset of the generalized coordinates. More... | |
Eigen::VectorXd | getCommands () const |
Get commands for all generalized coordinates. More... | |
Eigen::VectorXd | getCommands (const std::vector< std::size_t > &_indices) const |
Get commands for a subset of the generalized coordinates. More... | |
void | resetCommands () |
Set all commands to zero. More... | |
Position | |
void | setPosition (std::size_t index, double _position) |
Set the position of a single generalized coordinate. More... | |
double | getPosition (std::size_t _index) const |
Get the position of a single generalized coordinate. More... | |
void | setPositions (const Eigen::VectorXd &_positions) |
Set the positions for all generalized coordinates. More... | |
void | setPositions (const std::vector< std::size_t > &_indices, const Eigen::VectorXd &_positions) |
Set the positions for a subset of the generalized coordinates. More... | |
Eigen::VectorXd | getPositions () const |
Get the positions for all generalized coordinates. More... | |
Eigen::VectorXd | getPositions (const std::vector< std::size_t > &_indices) const |
Get the positions for a subset of the generalized coordinates. More... | |
void | resetPositions () |
Set all positions to zero. More... | |
void | setPositionLowerLimit (std::size_t _index, double _position) |
Set the lower limit of a generalized coordinate's position. More... | |
void | setPositionLowerLimits (const Eigen::VectorXd &positions) |
Set the lower limits for all generalized coordinates. More... | |
void | setPositionLowerLimits (const std::vector< std::size_t > &indices, const Eigen::VectorXd &positions) |
Set the lower limits for a subset of the generalized coordinates. More... | |
double | getPositionLowerLimit (std::size_t _index) const |
Get the lower limit of a generalized coordinate's position. More... | |
Eigen::VectorXd | getPositionLowerLimits () const |
Get the lower limits for all generalized coordinates. More... | |
Eigen::VectorXd | getPositionLowerLimits (const std::vector< std::size_t > &indices) const |
Get the lower limits for a subset of the generalized coordinates. More... | |
void | setPositionUpperLimit (std::size_t _index, double _position) |
Set the upper limit of a generalized coordainte's position. More... | |
void | setPositionUpperLimits (const Eigen::VectorXd &positions) |
Set the upper limits for all generalized coordinates. More... | |
void | setPositionUpperLimits (const std::vector< std::size_t > &indices, const Eigen::VectorXd &positions) |
Set the upper limits for a subset of the generalized coordinates. More... | |
double | getPositionUpperLimit (std::size_t _index) const |
Get the upper limit of a generalized coordinate's position. More... | |
Eigen::VectorXd | getPositionUpperLimits () const |
Get the upper limits for all generalized coordinates. More... | |
Eigen::VectorXd | getPositionUpperLimits (const std::vector< std::size_t > &indices) const |
Get the upper limits for a subset of the generalized coordinates. More... | |
Velocity | |
void | setVelocity (std::size_t _index, double _velocity) |
Set the velocity of a single generalized coordinate. More... | |
double | getVelocity (std::size_t _index) const |
Get the velocity of a single generalized coordinate. More... | |
void | setVelocities (const Eigen::VectorXd &_velocities) |
Set the velocities of all generalized coordinates. More... | |
void | setVelocities (const std::vector< std::size_t > &_indices, const Eigen::VectorXd &_velocities) |
Set the velocities of a subset of the generalized coordinates. More... | |
Eigen::VectorXd | getVelocities () const |
Get the velocities for all generalized coordinates. More... | |
Eigen::VectorXd | getVelocities (const std::vector< std::size_t > &_indices) const |
Get the velocities for a subset of the generalized coordinates. More... | |
void | resetVelocities () |
Set all velocities to zero. More... | |
void | setVelocityLowerLimit (std::size_t _index, double _velocity) |
Set the lower limit of a generalized coordinate's velocity. More... | |
void | setVelocityLowerLimits (const Eigen::VectorXd &velocities) |
Set the lower limits for all generalized coordinates's velocity. More... | |
void | setVelocityLowerLimits (const std::vector< std::size_t > &indices, const Eigen::VectorXd &velocities) |
Set the lower limits for a subset of the generalized coordinates's velocity. More... | |
double | getVelocityLowerLimit (std::size_t _index) |
Get the lower limit of a generalized coordinate's velocity. More... | |
Eigen::VectorXd | getVelocityLowerLimits () const |
Get the lower limits for all generalized coordinates's velocity. More... | |
Eigen::VectorXd | getVelocityLowerLimits (const std::vector< std::size_t > &indices) const |
Get the lower limits for a subset of the generalized coordinates's velocity. More... | |
void | setVelocityUpperLimit (std::size_t _index, double _velocity) |
Set the upper limit of a generalized coordinate's velocity. More... | |
void | setVelocityUpperLimits (const Eigen::VectorXd &velocities) |
Set the upper limits for all generalized coordinates's velocity. More... | |
void | setVelocityUpperLimits (const std::vector< std::size_t > &indices, const Eigen::VectorXd &velocities) |
Set the upper limits for a subset of the generalized coordinates's velocity. More... | |
double | getVelocityUpperLimit (std::size_t _index) |
Get the upper limit of a generalized coordinate's velocity. More... | |
Eigen::VectorXd | getVelocityUpperLimits () const |
Get the upper limits for all generalized coordinates's velocity. More... | |
Eigen::VectorXd | getVelocityUpperLimits (const std::vector< std::size_t > &indices) const |
Get the upper limits for a subset of the generalized coordinates's velocity. More... | |
Acceleration | |
void | setAcceleration (std::size_t _index, double _acceleration) |
Set the acceleration of a single generalized coordinate. More... | |
double | getAcceleration (std::size_t _index) const |
Get the acceleration of a single generalized coordinate. More... | |
void | setAccelerations (const Eigen::VectorXd &_accelerations) |
Set the accelerations of all generalized coordinates. More... | |
void | setAccelerations (const std::vector< std::size_t > &_indices, const Eigen::VectorXd &_accelerations) |
Set the accelerations of a subset of the generalized coordinates. More... | |
Eigen::VectorXd | getAccelerations () const |
Get the accelerations for all generalized coordinates. More... | |
Eigen::VectorXd | getAccelerations (const std::vector< std::size_t > &_indices) const |
Get the accelerations for a subset of the generalized coordinates. More... | |
void | resetAccelerations () |
Set all accelerations to zero. More... | |
void | setAccelerationLowerLimit (std::size_t _index, double _acceleration) |
Set the lower limit of a generalized coordinate's acceleration. More... | |
void | setAccelerationLowerLimits (const Eigen::VectorXd &accelerations) |
Set the lower limits for all generalized coordinates's acceleration. More... | |
void | setAccelerationLowerLimits (const std::vector< std::size_t > &indices, const Eigen::VectorXd &accelerations) |
Set the lower limits for a subset of the generalized coordinates's acceleration. More... | |
double | getAccelerationLowerLimit (std::size_t _index) const |
Get the lower limit of a generalized coordinate's acceleration. More... | |
Eigen::VectorXd | getAccelerationLowerLimits () const |
Get the lower limits for all generalized coordinates's acceleration. More... | |
Eigen::VectorXd | getAccelerationLowerLimits (const std::vector< std::size_t > &indices) const |
Get the lower limits for a subset of the generalized coordinates's acceleration. More... | |
void | setAccelerationUpperLimit (std::size_t _index, double _acceleration) |
Set the upper limit of a generalized coordinate's acceleration. More... | |
void | setAccelerationUpperLimits (const Eigen::VectorXd &accelerations) |
Set the upper limits for all generalized coordinates's acceleration. More... | |
void | setAccelerationUpperLimits (const std::vector< std::size_t > &indices, const Eigen::VectorXd &accelerations) |
Set the upper limits for a subset of the generalized coordinates's acceleration. More... | |
double | getAccelerationUpperLimit (std::size_t _index) const |
Get the upper limit of a generalized coordinate's acceleration. More... | |
Eigen::VectorXd | getAccelerationUpperLimits () const |
Get the upper limits for all generalized coordinates's acceleration. More... | |
Eigen::VectorXd | getAccelerationUpperLimits (const std::vector< std::size_t > &indices) const |
Get the upper limits for a subset of the generalized coordinates's acceleration. More... | |
Force | |
void | setForce (std::size_t _index, double _force) |
Set the force of a single generalized coordinate. More... | |
double | getForce (std::size_t _index) const |
Get the force of a single generalized coordinate. More... | |
void | setForces (const Eigen::VectorXd &_forces) |
Set the forces of all generalized coordinates. More... | |
void | setForces (const std::vector< std::size_t > &_index, const Eigen::VectorXd &_forces) |
Set the forces of a subset of the generalized coordinates. More... | |
Eigen::VectorXd | getForces () const |
Get the forces for all generalized coordinates. More... | |
Eigen::VectorXd | getForces (const std::vector< std::size_t > &_indices) const |
Get the forces for a subset of the generalized coordinates. More... | |
void | resetGeneralizedForces () |
Set all forces of the generalized coordinates to zero. More... | |
void | setForceLowerLimit (std::size_t _index, double _force) |
Set the lower limit of a generalized coordinate's force. More... | |
void | setForceLowerLimits (const Eigen::VectorXd &forces) |
Set the lower limits for all generalized coordinates's force. More... | |
void | setForceLowerLimits (const std::vector< std::size_t > &indices, const Eigen::VectorXd &forces) |
Set the lower limits for a subset of the generalized coordinates's force. More... | |
double | getForceLowerLimit (std::size_t _index) const |
Get the lower limit of a generalized coordinate's force. More... | |
Eigen::VectorXd | getForceLowerLimits () const |
Get the lower limits for all generalized coordinates's force. More... | |
Eigen::VectorXd | getForceLowerLimits (const std::vector< std::size_t > &indices) const |
Get the lower limits for a subset of the generalized coordinates's force. More... | |
void | setForceUpperLimit (std::size_t _index, double _force) |
Set the upper limit of a generalized coordinate's force. More... | |
void | setForceUpperLimits (const Eigen::VectorXd &forces) |
Set the upperlimits for all generalized coordinates's force. More... | |
void | setForceUpperLimits (const std::vector< std::size_t > &indices, const Eigen::VectorXd &forces) |
Set the upper limits for a subset of the generalized coordinates's force. More... | |
double | getForceUpperLimit (std::size_t _index) const |
Get the upper limit of a generalized coordinate's force. More... | |
Eigen::VectorXd | getForceUpperLimits () const |
Get the upper limits for all generalized coordinates's force. More... | |
Eigen::VectorXd | getForceUpperLimits (const std::vector< std::size_t > &indices) const |
Get the upper limits for a subset of the generalized coordinates's force. More... | |
Velocity Change | |
Eigen::VectorXd | getVelocityChanges () const |
Get the velocity changes for all the generalized coordinates. More... | |
Constraint Impulse | |
void | setJointConstraintImpulses (const Eigen::VectorXd &_impulses) |
Set the constraint impulses for the generalized coordinates. More... | |
Eigen::VectorXd | getJointConstraintImpulses () const |
Get the constraint impulses for the generalized coordinates. More... | |
Jacobians | |
math::Jacobian | getJacobian (const JacobianNode *_node, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial Jacobian targeting the origin of a BodyNode relative to another BodyNode in the same Skeleton. More... | |
math::Jacobian | getJacobian (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial Jacobian targeting an offset in a BodyNode relative to another BodyNode in the same Skeleton. More... | |
math::LinearJacobian | getLinearJacobian (const JacobianNode *_node, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf=Frame::World()) const |
Get the linear Jacobian targeting the origin of a BodyNode relative to another BodyNode in the same Skeleton. More... | |
math::LinearJacobian | getLinearJacobian (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf=Frame::World()) const |
Get the linear Jacobian targeting an offset in a BodyNode relative to another BodyNode in the same Skeleton. More... | |
Jacobian derivatives | |
math::AngularJacobian | getAngularJacobian (const JacobianNode *_node, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf=Frame::World()) const |
Get the angular Jacobian of a BodyNode relative to another BodyNode in the same Skeleton. More... | |
math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial Jacobian time derivative targeting the origin of a BodyNode relative to another BodyNode in the same Skeleton. More... | |
math::Jacobian | getJacobianSpatialDeriv (const JacobianNode *_node, const Eigen::Vector3d &_localOffset, const JacobianNode *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial Jacobian time derivative targeting an offset in a BodyNode relative to another Bodynode in the same Skeleton. More... | |
Equations of Motion | |
double | computeLagrangian () const |
Compute and return Lagrangian of this MetaSkeleton. More... | |
double | getKineticEnergy () const |
Get the kinetic energy of this MetaSkeleton. More... | |
double | getPotentialEnergy () const |
Get the potential energy of this MetaSkeleton. More... | |
Public Attributes | |
std::weak_ptr< Skeleton > | mUnionRootSkeleton |
std::size_t | mUnionSize |
std::size_t | mUnionIndex |
common::SlotRegister< NameChangedSignal > | onNameChanged |
Protected Types | |
using | SpecializedTreeNodes = std::map< std::type_index, std::vector< NodeMap::iterator > * > |
Protected Member Functions | |
Skeleton (const AspectPropertiesData &_properties) | |
Constructor called by create() More... | |
void | setPtr (const SkeletonPtr &_ptr) |
Setup this Skeleton with its shared_ptr. More... | |
void | constructNewTree () |
Construct a new tree in the Skeleton. More... | |
void | registerBodyNode (BodyNode *_newBodyNode) |
Register a BodyNode with the Skeleton. Internal use only. More... | |
void | registerJoint (Joint *_newJoint) |
Register a Joint with the Skeleton. Internal use only. More... | |
void | registerNode (NodeMap &nodeMap, Node *_newNode, std::size_t &_index) |
Register a Node with the Skeleton. Internal use only. More... | |
void | registerNode (Node *_newNode) |
Register a Node with the Skeleton. Internal use only. More... | |
void | destructOldTree (std::size_t tree) |
Remove an old tree from the Skeleton. More... | |
void | unregisterBodyNode (BodyNode *_oldBodyNode) |
Remove a BodyNode from the Skeleton. Internal use only. More... | |
void | unregisterJoint (Joint *_oldJoint) |
Remove a Joint from the Skeleton. Internal use only. More... | |
void | unregisterNode (NodeMap &nodeMap, Node *_oldNode, std::size_t &_index) |
Remove a Node from the Skeleton. Internal use only. More... | |
void | unregisterNode (Node *_oldNode) |
Remove a Node from the Skeleton. Internal use only. More... | |
bool | moveBodyNodeTree (Joint *_parentJoint, BodyNode *_bodyNode, SkeletonPtr _newSkeleton, BodyNode *_parentNode) |
Move a subtree of BodyNodes from this Skeleton to another Skeleton. More... | |
template<class JointType > | |
JointType * | moveBodyNodeTree (BodyNode *_bodyNode, const SkeletonPtr &_newSkeleton, BodyNode *_parentNode, const typename JointType::Properties &_joint) |
Move a subtree of BodyNodes from this Skeleton to another Skeleton while changing the Joint type of the top parent Joint. More... | |
std::pair< Joint *, BodyNode * > | cloneBodyNodeTree (Joint *_parentJoint, const BodyNode *_bodyNode, const SkeletonPtr &_newSkeleton, BodyNode *_parentNode, bool _recursive) const |
Copy a subtree of BodyNodes onto another Skeleton while leaving the originals intact. More... | |
template<class JointType > | |
std::pair< JointType *, BodyNode * > | cloneBodyNodeTree (const BodyNode *_bodyNode, const SkeletonPtr &_newSkeleton, BodyNode *_parentNode, const typename JointType::Properties &_joint, bool _recursive) const |
Copy a subtree of BodyNodes onto another Skeleton while leaving the originals intact, but alter the top parent Joint to a new type. More... | |
std::vector< const BodyNode * > | constructBodyNodeTree (const BodyNode *_bodyNode) const |
Create a vector representation of a subtree of BodyNodes. More... | |
std::vector< BodyNode * > | constructBodyNodeTree (BodyNode *_bodyNode) |
std::vector< BodyNode * > | extractBodyNodeTree (BodyNode *_bodyNode) |
Create a vector representation of a subtree of BodyNodes and remove that subtree from this Skeleton without deleting them. More... | |
void | receiveBodyNodeTree (const std::vector< BodyNode * > &_tree) |
Take in and register a subtree of BodyNodes. More... | |
void | updateTotalMass () |
Update the computation for total mass. More... | |
void | updateCacheDimensions (DataCache &_cache) |
Update the dimensions for a specific cache. More... | |
void | updateCacheDimensions (std::size_t _treeIdx) |
Update the dimensions for a tree's cache. More... | |
void | updateArticulatedInertia (std::size_t _tree) const |
Update the articulated inertia of a tree. More... | |
void | updateArticulatedInertia () const |
Update the articulated inertias of the skeleton. More... | |
void | updateMassMatrix (std::size_t _treeIdx) const |
Update the mass matrix of a tree. More... | |
void | updateMassMatrix () const |
Update mass matrix of the skeleton. More... | |
void | updateAugMassMatrix (std::size_t _treeIdx) const |
void | updateAugMassMatrix () const |
Update augmented mass matrix of the skeleton. More... | |
void | updateInvMassMatrix (std::size_t _treeIdx) const |
Update the inverse mass matrix of a tree. More... | |
void | updateInvMassMatrix () const |
Update inverse of mass matrix of the skeleton. More... | |
void | updateInvAugMassMatrix (std::size_t _treeIdx) const |
Update the inverse augmented mass matrix of a tree. More... | |
void | updateInvAugMassMatrix () const |
Update inverse of augmented mass matrix of the skeleton. More... | |
void | updateCoriolisForces (std::size_t _treeIdx) const |
Update Coriolis force vector for a tree in the Skeleton. More... | |
void | updateCoriolisForces () const |
Update Coriolis force vector of the skeleton. More... | |
void | updateGravityForces (std::size_t _treeIdx) const |
Update the gravity force vector of a tree. More... | |
void | updateGravityForces () const |
Update gravity force vector of the skeleton. More... | |
void | updateCoriolisAndGravityForces (std::size_t _treeIdx) const |
Update the combined vector for a tree in this Skeleton. More... | |
void | updateCoriolisAndGravityForces () const |
Update combined vector of the skeleton. More... | |
void | updateExternalForces (std::size_t _treeIdx) const |
Update external force vector to generalized forces for a tree. More... | |
void | updateExternalForces () const |
update external force vector to generalized forces. More... | |
const Eigen::VectorXd & | computeConstraintForces (DataCache &cache) const |
Compute the constraint force vector for a tree. More... | |
const std::string & | addEntryToBodyNodeNameMgr (BodyNode *_newNode) |
Add a BodyNode to the BodyNode NameManager. More... | |
const std::string & | addEntryToJointNameMgr (Joint *_newJoint, bool _updateDofNames=true) |
Add a Joint to to the Joint NameManager. More... | |
void | addEntryToSoftBodyNodeNameMgr (SoftBodyNode *_newNode) |
Add a SoftBodyNode to the SoftBodyNode NameManager. More... | |
void | setVersionDependentObject (VersionCounter *dependent) |
void | sendDestructionNotification () const |
Send a destruction notification to all Observers. More... | |
void | addObserver (Observer *_observer) const |
Add an Observer to the list of Observers. More... | |
void | removeObserver (Observer *_observer) const |
Remove an Observer from the list of Observers. More... | |
Private Attributes | |
VersionCounter * | mDependent |
Friends | |
class | BodyNode |
class | SoftBodyNode |
class | Joint |
template<class > | |
class | GenericJoint |
class | DegreeOfFreedom |
class | Node |
class | ShapeNode |
class | EndEffector |
Constructor and Destructor | |
static SkeletonPtr | create (const std::string &_name="Skeleton") |
Create a new Skeleton inside of a shared_ptr. More... | |
static SkeletonPtr | create (const AspectPropertiesData &properties) |
Create a new Skeleton inside of a shared_ptr. More... | |
SkeletonPtr | getPtr () |
Get the shared_ptr that manages this Skeleton. More... | |
ConstSkeletonPtr | getPtr () const |
Get the shared_ptr that manages this Skeleton. More... | |
SkeletonPtr | getSkeleton () |
Same as getPtr(), but this allows Skeleton to have a similar interface as BodyNode and Joint for template programming. More... | |
ConstSkeletonPtr | getSkeleton () const |
Same as getPtr(), but this allows Skeleton to have a similar interface as BodyNode and Joint for template programming. More... | |
std::mutex & | getMutex () const |
Get the mutex that protects the state of this Skeleton. More... | |
std::unique_ptr< common::LockableReference > | getLockableReference () const override |
Get the mutex that protects the state of this Skeleton. More... | |
Skeleton (const Skeleton &)=delete | |
virtual | ~Skeleton () |
Destructor. More... | |
Skeleton & | operator= (const Skeleton &_other)=delete |
Remove copy operator. More... | |
SkeletonPtr | clone () const |
Create an identical clone of this Skeleton. More... | |
SkeletonPtr | clone (const std::string &cloneName) const |
Create an identical clone of this Skeleton, except that it has a new name. More... | |
SkeletonPtr | cloneSkeleton () const |
Creates and returns a clone of this Skeleton. More... | |
SkeletonPtr | cloneSkeleton (const std::string &cloneName) const |
Creates and returns a clone of this Skeleton. More... | |
MetaSkeletonPtr | cloneMetaSkeleton (const std::string &cloneName) const override |
Creates an identical clone of this MetaSkeleton. More... | |
class Skeleton
|
inherited |
using dart::dynamics::Skeleton::AspectProperties = common::Aspect::MakeProperties<AspectPropertiesData> |
|
inherited |
|
inherited |
|
inherited |
|
inherited |
|
protected |
|
delete |
|
virtual |
Destructor.
|
protected |
Constructor called by create()
|
protected |
|
protected |
|
protected |
Add a SoftBodyNode to the SoftBodyNode NameManager.
|
protectedinherited |
Add an Observer to the list of Observers.
bool dart::dynamics::Skeleton::checkIndexingConsistency | ( | ) | const |
This function is only meant for debugging purposes.
It will verify that all objects held in the Skeleton have the correct information about their indexing.
|
overridevirtual |
Clear collision flags of the BodyNodes in this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
void dart::dynamics::Skeleton::clearConstraintImpulses | ( | ) |
Clear constraint impulses and cache data used for impulse-based forward dynamics algorithm, where the constraint impulses are spatial constraints on the BodyNodes and generalized constraints on the Joints.
|
overridevirtual |
Clear the external forces of the BodyNodes in this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
void dart::dynamics::Skeleton::clearIK | ( | ) |
Wipe away the WholeBodyIK module for this Skeleton, leaving it as a nullptr.
|
overridevirtual |
Clear the internal forces of the BodyNodes in this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
SkeletonPtr dart::dynamics::Skeleton::clone | ( | ) | const |
Create an identical clone of this Skeleton.
SkeletonPtr dart::dynamics::Skeleton::clone | ( | const std::string & | cloneName | ) | const |
Create an identical clone of this Skeleton, except that it has a new name.
|
protected |
|
protected |
Copy a subtree of BodyNodes onto another Skeleton while leaving the originals intact.
|
inherited |
Creates an identical clone of this MetaSkeleton.
|
overridevirtual |
Creates an identical clone of this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
SkeletonPtr dart::dynamics::Skeleton::cloneSkeleton | ( | ) | const |
Creates and returns a clone of this Skeleton.
SkeletonPtr dart::dynamics::Skeleton::cloneSkeleton | ( | const std::string & | cloneName | ) | const |
Creates and returns a clone of this Skeleton.
|
protected |
Compute the constraint force vector for a tree.
void dart::dynamics::Skeleton::computeForwardDynamics | ( | ) |
Compute forward dynamics.
void dart::dynamics::Skeleton::computeForwardKinematics | ( | bool | _updateTransforms = true , |
bool | _updateVels = true , |
||
bool | _updateAccs = true |
||
) |
Compute forward kinematics.
In general, this function doesn't need to be called for forward kinematics to update. Forward kinematics will always be computed when it's needed and will only perform the computations that are necessary for what the user requests. This works by performing some bookkeeping internally with dirty flags whenever a position, velocity, or acceleration is set, either internally or by the user.
On one hand, this results in some overhead due to the extra effort of bookkeeping, but on the other hand we have much greater code safety, and in some cases performance can be dramatically improved with the auto- updating. For example, this function is inefficient when only one portion of the BodyNodes needed to be updated rather than the entire Skeleton, which is common when performing inverse kinematics on a limb or on some subsection of a Skeleton.
This function might be useful in a case where the user wants to perform all the forward kinematics computations during a particular time window rather than waiting for it to be computed at the exact time that it's needed.
One example would be a real time controller. Let's say a controller gets encoder data at time t0 but needs to wait until t1 before it receives the force-torque sensor data that it needs in order to compute the output for an operational space controller. Instead of being idle from t0 to t1, it could use that time to compute the forward kinematics by calling this function.
void dart::dynamics::Skeleton::computeImpulseForwardDynamics | ( | ) |
Compute impulse-based forward dynamics.
void dart::dynamics::Skeleton::computeInverseDynamics | ( | bool | _withExternalForces = false , |
bool | _withDampingForces = false , |
||
bool | _withSpringForces = false |
||
) |
Compute inverse dynamics.
|
overridevirtual |
Get the kinetic energy of this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
inherited |
Compute and return Lagrangian of this MetaSkeleton.
|
overridevirtual |
Get the potential energy of this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
protected |
|
protected |
Create a vector representation of a subtree of BodyNodes.
|
protected |
Construct a new tree in the Skeleton.
|
static |
Create a new Skeleton inside of a shared_ptr.
|
static |
Create a new Skeleton inside of a shared_ptr.
const std::shared_ptr< WholeBodyIK > & dart::dynamics::Skeleton::createIK | ( | ) |
Create a new WholeBodyIK module for this Skeleton.
If an IK module already exists in this Skeleton, it will be destroyed and replaced by a brand new one.
std::pair< JointType *, NodeType * > dart::dynamics::Skeleton::createJointAndBodyNodePair | ( | BodyNode * | _parent = nullptr , |
const typename JointType::Properties & | _jointProperties = typename JointType::Properties() , |
||
const typename NodeType::Properties & | _bodyProperties = typename NodeType::Properties() |
||
) |
|
protected |
Remove an old tree from the Skeleton.
void dart::dynamics::Skeleton::dirtyArticulatedInertia | ( | std::size_t | _treeIdx | ) |
Notify that the articulated inertia and everything that depends on it needs to be updated.
void dart::dynamics::Skeleton::dirtySupportPolygon | ( | std::size_t | _treeIdx | ) |
Notify that the support polygon of a tree needs to be updated.
void dart::dynamics::Skeleton::disableAdjacentBodyCheck | ( | ) |
Disable collision check for adjacent bodies.
This option is effective only when the self-collision check is enabled.
void dart::dynamics::Skeleton::disableSelfCollision | ( | ) |
Deprecated. Please use disableSelfCollisionCheck() instead.
void dart::dynamics::Skeleton::disableSelfCollisionCheck | ( | ) |
Disable self-collision check.
void dart::dynamics::Skeleton::enableAdjacentBodyCheck | ( | ) |
Enable collision check for adjacent bodies.
This option is effective only when the self-collision check is enabled.
void dart::dynamics::Skeleton::enableSelfCollision | ( | bool | enableAdjacentBodyCheck = false | ) |
Deprecated.
Please use enableSelfCollisionCheck() and setAdjacentBodyCheck() instead.
void dart::dynamics::Skeleton::enableSelfCollisionCheck | ( | ) |
Enable self-collision check.
|
protected |
Create a vector representation of a subtree of BodyNodes and remove that subtree from this Skeleton without deleting them.
|
inherited |
Get the acceleration of a single generalized coordinate.
|
inherited |
Get the lower limit of a generalized coordinate's acceleration.
|
inherited |
Get the lower limits for all generalized coordinates's acceleration.
|
inherited |
Get the lower limits for a subset of the generalized coordinates's acceleration.
|
inherited |
Get the accelerations for all generalized coordinates.
|
inherited |
Get the accelerations for a subset of the generalized coordinates.
|
inherited |
Get the upper limit of a generalized coordinate's acceleration.
|
inherited |
Get the upper limits for all generalized coordinates's acceleration.
|
inherited |
Get the upper limits for a subset of the generalized coordinates's acceleration.
bool dart::dynamics::Skeleton::getAdjacentBodyCheck | ( | ) | const |
Return whether adjacent body check is enabled.
|
overridevirtual |
Get the angular Jacobian of a BodyNode.
You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::AngularJacobian dart::dynamics::MetaSkeleton::getAngularJacobian |
|
inherited |
math::AngularJacobian dart::dynamics::MetaSkeleton::getAngularJacobian |
|
overridevirtual |
Get the angular Jacobian time derivative of a BodyNode.
You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::AngularJacobian dart::dynamics::MetaSkeleton::getAngularJacobianDeriv |
|
inlineinherited |
|
overridevirtual |
Get augmented mass matrix of the skeleton.
This matrix is used in ConstraintDynamics to compute constraint forces. The matrix is M + h*D + h*h*K where D is diagonal joint damping coefficient matrix, K is diagonal joint stiffness matrix, and h is simulation time step.
Implements dart::dynamics::MetaSkeleton.
const Eigen::MatrixXd & dart::dynamics::Skeleton::getAugMassMatrix | ( | std::size_t | _treeIdx | ) | const |
Get the augmented mass matrix of a specific tree in the Skeleton.
|
overridevirtual |
Returns the BodyNode of given name.
[in] | name | The BodyNode name that want to search. |
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Returns the BodyNode of given name.
[in] | name | The BodyNode name that want to search. |
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get const BodyNode whose index is _idx.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get BodyNode whose index is _idx.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get all the BodyNodes that are held by this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get all the BodyNodes that are held by this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Returns all the BodyNodes of given name.
[in] | name | The BodyNode name that want to search. |
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Returns all the BodyNodes of given name.
[in] | name | The BodyNode name that want to search. |
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the Skeleton's COM with respect to any Frame (default is World Frame)
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the Skeleton's COM Jacobian in terms of any Frame (default is World Frame)
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the Skeleton's COM Jacobian spatial time derivative in terms of any Frame (default is World Frame).
NOTE: Since this is a spatial time derivative, it is only meant to be used with spatial acceleration vectors. If you are using classical linear vectors, then use getCOMLinearJacobianDeriv() instead.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the Skeleton's COM linear acceleration in terms of any Frame (default is World Frame)
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the Skeleton's COM Linear Jacobian in terms of any Frame (default is World Frame)
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the Skeleton's COM Linear Jacobian time derivative in terms of any Frame (default is World Frame).
NOTE: Since this is a classical time derivative, it is only meant to be used with classical acceleration vectors. If you are using spatial vectors, then use getCOMJacobianSpatialDeriv() instead.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the Skeleton's COM linear velocity in terms of any Frame (default is World Frame)
Implements dart::dynamics::MetaSkeleton.
|
inherited |
Get a single command.
|
inherited |
Get commands for all generalized coordinates.
|
inherited |
Get commands for a subset of the generalized coordinates.
|
overridevirtual |
Get the Skeleton's COM spatial acceleration in terms of any Frame (default is World Frame)
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the Skeleton's COM spatial velocity in terms of any Frame (default is World Frame)
Implements dart::dynamics::MetaSkeleton.
Skeleton::Configuration dart::dynamics::Skeleton::getConfiguration | ( | const std::vector< std::size_t > & | indices, |
int | flags = CONFIG_ALL |
||
) | const |
Get the configuration of the specified indices in this Skeleton.
Skeleton::Configuration dart::dynamics::Skeleton::getConfiguration | ( | int | flags = CONFIG_ALL | ) | const |
Get the configuration of this Skeleton.
|
overridevirtual |
Get constraint force vector.
Implements dart::dynamics::MetaSkeleton.
const Eigen::VectorXd & dart::dynamics::Skeleton::getConstraintForces | ( | std::size_t | _treeIdx | ) | const |
Get damping force of the skeleton.
Get constraint force vector for a tree
|
overridevirtual |
Get combined vector of Coriolis force and gravity force of the MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
const Eigen::VectorXd & dart::dynamics::Skeleton::getCoriolisAndGravityForces | ( | std::size_t | _treeIdx | ) | const |
Get the combined vector of Coriolis force and gravity force of a tree.
|
overridevirtual |
Get Coriolis force vector of the MetaSkeleton's BodyNodes.
Implements dart::dynamics::MetaSkeleton.
const Eigen::VectorXd & dart::dynamics::Skeleton::getCoriolisForces | ( | std::size_t | _treeIdx | ) | const |
Get the Coriolis force vector of a tree in this Skeleton.
DegreeOfFreedom * dart::dynamics::Skeleton::getDof | ( | const std::string & | _name | ) |
Get degree of freedom (aka generalized coordinate) whose name is _name.
const DegreeOfFreedom * dart::dynamics::Skeleton::getDof | ( | const std::string & | _name | ) | const |
Get degree of freedom (aka generalized coordinate) whose name is _name.
|
overridevirtual |
Get degree of freedom (aka generalized coordinate) whose index is _idx.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get degree of freedom (aka generalized coordinate) whose index is _idx.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get a vector of const DegreesOfFreedom for this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the vector of DegreesOfFreedom for this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
EndEffector * dart::dynamics::Skeleton::getEndEffector | ( | const std::string & | name | ) |
const EndEffector * dart::dynamics::Skeleton::getEndEffector | ( | const std::string & | name | ) | const |
EndEffector * dart::dynamics::Skeleton::getEndEffector | ( | std::size_t | index | ) |
const EndEffector * dart::dynamics::Skeleton::getEndEffector | ( | std::size_t | index | ) | const |
EndEffector * dart::dynamics::Skeleton::getEndEffector | ( | std::size_t | treeIndex, |
std::size_t | nodeIndex | ||
) |
const EndEffector * dart::dynamics::Skeleton::getEndEffector | ( | std::size_t | treeIndex, |
std::size_t | nodeIndex | ||
) | const |
|
overridevirtual |
Get external force vector of the MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
const Eigen::VectorXd & dart::dynamics::Skeleton::getExternalForces | ( | std::size_t | _treeIdx | ) | const |
Get the external force vector of a tree in the Skeleton.
|
inherited |
Get the force of a single generalized coordinate.
|
inherited |
Get the lower limit of a generalized coordinate's force.
|
inherited |
Get the lower limits for all generalized coordinates's force.
|
inherited |
Get the lower limits for a subset of the generalized coordinates's force.
|
inherited |
Get the forces for all generalized coordinates.
|
inherited |
Get the forces for a subset of the generalized coordinates.
|
inherited |
Get the upper limit of a generalized coordinate's force.
|
inherited |
Get the upper limits for all generalized coordinates's force.
|
inherited |
Get the upper limits for a subset of the generalized coordinates's force.
const Eigen::Vector3d & dart::dynamics::Skeleton::getGravity | ( | ) | const |
Get 3-dim gravitational acceleration.
|
overridevirtual |
Get gravity force vector of the MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
const Eigen::VectorXd & dart::dynamics::Skeleton::getGravityForces | ( | std::size_t | _treeIdx | ) | const |
Get the gravity forces for a tree in this Skeleton.
std::shared_ptr< const WholeBodyIK > dart::dynamics::Skeleton::getIK | ( | ) | const |
Get a pointer to a WholeBodyIK module for this Skeleton.
Because this is a const function, a new IK module cannot be created if one does not already exist.
const std::shared_ptr< WholeBodyIK > & dart::dynamics::Skeleton::getIK | ( | bool | _createIfNull = false | ) |
Get a pointer to a WholeBodyIK module for this Skeleton.
If _createIfNull is true, then the IK module will be generated if one does not already exist.
|
overridevirtual |
Get the index of a specific BodyNode within this ReferentialSkeleton.
Returns INVALID_INDEX if it is not held in this ReferentialSkeleton. When _warning is true, a warning message will be printed if the BodyNode is not in the MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the index of a specific DegreeOfFreedom within this ReferentialSkeleton.
Returns INVALID_INDEX if it is not held in this ReferentialSkeleton. When _warning is true, a warning message will be printed if the DegreeOfFreedom is not in the MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the index of a specific Joint within this ReferentialSkeleton.
Returns INVALID_INDEX if it is not held in this ReferentialSkeleton. When _warning is true, a warning message will be printed if the Joint is not in the MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get inverse of augmented Mass Matrix of the MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
const Eigen::MatrixXd & dart::dynamics::Skeleton::getInvAugMassMatrix | ( | std::size_t | _treeIdx | ) | const |
Get the inverse augmented mass matrix of a tree.
|
overridevirtual |
Get inverse of Mass Matrix of the MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
const Eigen::MatrixXd & dart::dynamics::Skeleton::getInvMassMatrix | ( | std::size_t | _treeIdx | ) | const |
Get the inverse mass matrix of a specific tree in the Skeleton.
|
overridevirtual |
Get the spatial Jacobian targeting the origin of a BodyNode.
The Jacobian is expressed in the Frame of the BodyNode.
Implements dart::dynamics::MetaSkeleton.
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobian |
|
overridevirtual |
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobian |
|
overridevirtual |
Get the spatial Jacobian targeting an offset in a BodyNode.
The _offset is expected in coordinates of the BodyNode Frame. You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobian |
|
inherited |
math::Jacobian dart::dynamics::MetaSkeleton::getJacobian |
|
overridevirtual |
Get the spatial Jacobian targeting the origin of a BodyNode.
You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobian |
|
inherited |
math::Jacobian dart::dynamics::MetaSkeleton::getJacobian |
|
overridevirtual |
Get the spatial Jacobian (classical) time derivative targeting the origin of a BodyNode.
The Jacobian is expressed in the World Frame.
Implements dart::dynamics::MetaSkeleton.
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobianClassicDeriv |
|
overridevirtual |
Get the spatial Jacobian (classical) time derivative targeting an offset in a BodyNode.
The _offset is expected in coordinates of the BodyNode Frame. You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobianClassicDeriv |
|
overridevirtual |
Get the spatial Jacobian (classical) time derivative targeting the origin a BodyNode.
The _offset is expected in coordinates of the BodyNode Frame. You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobianClassicDeriv |
|
overridevirtual |
Get the spatial Jacobian time derivative targeting the origin of a BodyNode.
The Jacobian is expressed in the Frame of the BodyNode.
Implements dart::dynamics::MetaSkeleton.
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobianSpatialDeriv |
|
overridevirtual |
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobianSpatialDeriv |
|
overridevirtual |
Get the spatial Jacobian time derivative targeting an offset in a BodyNode.
The _offset is expected in coordinates of the BodyNode Frame. You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobianSpatialDeriv |
|
inherited |
math::Jacobian dart::dynamics::MetaSkeleton::getJacobianSpatialDeriv |
|
overridevirtual |
Get the spatial Jacobian time derivative targeting the origin of a BodyNode.
You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::Jacobian dart::dynamics::MetaSkeleton::getJacobianSpatialDeriv |
|
inherited |
math::Jacobian dart::dynamics::MetaSkeleton::getJacobianSpatialDeriv |
|
overridevirtual |
Returns the joint of given name.
[in] | name | The joint name that want to search. |
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Returns the Joint of given name.
[in] | name | The joint name that want to search. |
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get const Joint whose index is _idx.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get Joint whose index is _idx.
Implements dart::dynamics::MetaSkeleton.
|
inherited |
Get the constraint impulses for the generalized coordinates.
|
overridevirtual |
Returns all the joints that are held by this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Returns all the joints that are held by this MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Returns all the Joint of given name.
This MetaSkeleton can contain multiple Joints with the same name when this MetaSkeleton contains Joints from multiple Skeletons.
[in] | name | The joint name that want to search. |
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Returns all the Joint of given name.
This MetaSkeleton can contain multiple Joints with the same name when this MetaSkeleton contains Joints from multiple Skeletons.
[in] | name | The joint name that want to search. |
Implements dart::dynamics::MetaSkeleton.
|
inherited |
Get the kinetic energy of this MetaSkeleton.
|
overridevirtual |
Get the linear Jacobian targeting an offset in a BodyNode.
The _offset is expected in coordinates of the BodyNode Frame. You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::LinearJacobian dart::dynamics::MetaSkeleton::getLinearJacobian |
|
inherited |
math::LinearJacobian dart::dynamics::MetaSkeleton::getLinearJacobian |
|
overridevirtual |
Get the linear Jacobian targeting the origin of a BodyNode.
You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::LinearJacobian dart::dynamics::MetaSkeleton::getLinearJacobian |
|
inherited |
math::LinearJacobian dart::dynamics::MetaSkeleton::getLinearJacobian |
|
overridevirtual |
Get the linear Jacobian (classical) time derivative targeting an offset in a BodyNode.
The _offset is expected in coordinates of the BodyNode Frame. You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::LinearJacobian dart::dynamics::MetaSkeleton::getLinearJacobianDeriv |
|
overridevirtual |
of a BodyNode.
The _offset is expected in coordinates of the BodyNode Frame. You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::MetaSkeleton.
virtual math::LinearJacobian dart::dynamics::MetaSkeleton::getLinearJacobianDeriv |
|
overridevirtual |
Get the mutex that protects the state of this Skeleton.
Implements dart::dynamics::MetaSkeleton.
Marker * dart::dynamics::Skeleton::getMarker | ( | const std::string & | name | ) |
const Marker * dart::dynamics::Skeleton::getMarker | ( | const std::string & | name | ) | const |
Marker * dart::dynamics::Skeleton::getMarker | ( | std::size_t | index | ) |
const Marker * dart::dynamics::Skeleton::getMarker | ( | std::size_t | index | ) | const |
Marker * dart::dynamics::Skeleton::getMarker | ( | std::size_t | treeIndex, |
std::size_t | nodeIndex | ||
) |
const Marker * dart::dynamics::Skeleton::getMarker | ( | std::size_t | treeIndex, |
std::size_t | nodeIndex | ||
) | const |
|
overridevirtual |
Get total mass of the skeleton.
The total mass is calculated as BodyNodes are added and is updated as BodyNode mass is changed, so this is a constant-time O(1) operation for the Skeleton class.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the Mass Matrix of the MetaSkeleton.
Implements dart::dynamics::MetaSkeleton.
const Eigen::MatrixXd & dart::dynamics::Skeleton::getMassMatrix | ( | std::size_t | _treeIdx | ) | const |
Get the mass matrix of a specific tree in the Skeleton.
std::mutex & dart::dynamics::Skeleton::getMutex | ( | ) | const |
Get the mutex that protects the state of this Skeleton.
|
overridevirtual |
Get name.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get number of body nodes.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Return the number of degrees of freedom in this skeleton.
Implements dart::dynamics::MetaSkeleton.
std::size_t dart::dynamics::Skeleton::getNumEndEffectors | ( | ) | const |
std::size_t dart::dynamics::Skeleton::getNumEndEffectors | ( | std::size_t | treeIndex | ) | const |
|
overridevirtual |
Get number of Joints.
Implements dart::dynamics::MetaSkeleton.
std::size_t dart::dynamics::Skeleton::getNumMarkers | ( | ) | const |
std::size_t dart::dynamics::Skeleton::getNumMarkers | ( | std::size_t | treeIndex | ) | const |
std::size_t dart::dynamics::Skeleton::getNumRigidBodyNodes | ( | ) | const |
Get number of rigid body nodes.
std::size_t dart::dynamics::Skeleton::getNumShapeNodes | ( | ) | const |
std::size_t dart::dynamics::Skeleton::getNumShapeNodes | ( | std::size_t | treeIndex | ) | const |
std::size_t dart::dynamics::Skeleton::getNumSoftBodyNodes | ( | ) | const |
Get number of soft body nodes.
std::size_t dart::dynamics::Skeleton::getNumTrees | ( | ) | const |
Get the number of independent trees that this Skeleton contains.
const std::shared_ptr< WholeBodyIK > & dart::dynamics::Skeleton::getOrCreateIK | ( | ) |
Get a pointer to a WholeBodyIK module for this Skeleton.
The IK module will be generated if one does not already exist. This function is actually the same as getIK(true).
|
inherited |
Get the position of a single generalized coordinate.
Eigen::VectorXd dart::dynamics::Skeleton::getPositionDifferences | ( | const Eigen::VectorXd & | _q2, |
const Eigen::VectorXd & | _q1 | ||
) | const |
Return the difference of two generalized positions which are measured in the configuration space of this Skeleton.
If the configuration space is Euclidean space, this function returns _q2 - _q1. Otherwise, it depends on the type of the configuration space.
|
inherited |
Get the lower limit of a generalized coordinate's position.
|
inherited |
Get the lower limits for all generalized coordinates.
|
inherited |
Get the lower limits for a subset of the generalized coordinates.
|
inherited |
Get the positions for all generalized coordinates.
|
inherited |
Get the positions for a subset of the generalized coordinates.
|
inherited |
Get the upper limit of a generalized coordinate's position.
|
inherited |
Get the upper limits for all generalized coordinates.
|
inherited |
Get the upper limits for a subset of the generalized coordinates.
|
inherited |
Get the potential energy of this MetaSkeleton.
Skeleton::Properties dart::dynamics::Skeleton::getProperties | ( | ) | const |
Get all properties of this Skeleton.
SkeletonPtr dart::dynamics::Skeleton::getPtr | ( | ) |
Get the shared_ptr that manages this Skeleton.
ConstSkeletonPtr dart::dynamics::Skeleton::getPtr | ( | ) | const |
Get the shared_ptr that manages this Skeleton.
BodyNode * dart::dynamics::Skeleton::getRootBodyNode | ( | std::size_t | _treeIdx = 0 | ) |
const BodyNode * dart::dynamics::Skeleton::getRootBodyNode | ( | std::size_t | _treeIdx = 0 | ) | const |
Joint * dart::dynamics::Skeleton::getRootJoint | ( | std::size_t | treeIdx = 0u | ) |
const Joint * dart::dynamics::Skeleton::getRootJoint | ( | std::size_t | treeIdx = 0u | ) | const |
bool dart::dynamics::Skeleton::getSelfCollisionCheck | ( | ) | const |
Return whether self-collision check is enabled.
ShapeNode * dart::dynamics::Skeleton::getShapeNode | ( | const std::string & | name | ) |
const ShapeNode * dart::dynamics::Skeleton::getShapeNode | ( | const std::string & | name | ) | const |
ShapeNode * dart::dynamics::Skeleton::getShapeNode | ( | std::size_t | index | ) |
const ShapeNode * dart::dynamics::Skeleton::getShapeNode | ( | std::size_t | index | ) | const |
ShapeNode * dart::dynamics::Skeleton::getShapeNode | ( | std::size_t | treeIndex, |
std::size_t | nodeIndex | ||
) |
const ShapeNode * dart::dynamics::Skeleton::getShapeNode | ( | std::size_t | treeIndex, |
std::size_t | nodeIndex | ||
) | const |
SkeletonPtr dart::dynamics::Skeleton::getSkeleton | ( | ) |
ConstSkeletonPtr dart::dynamics::Skeleton::getSkeleton | ( | ) | const |
const Skeleton::AspectProperties & dart::dynamics::Skeleton::getSkeletonProperties | ( | ) | const |
Get the Properties of this Skeleton.
SoftBodyNode * dart::dynamics::Skeleton::getSoftBodyNode | ( | const std::string & | _name | ) |
Get soft body node whose name is _name.
const SoftBodyNode * dart::dynamics::Skeleton::getSoftBodyNode | ( | const std::string & | _name | ) | const |
Get const soft body node whose name is _name.
SoftBodyNode * dart::dynamics::Skeleton::getSoftBodyNode | ( | std::size_t | _idx | ) |
Get SoftBodyNode whose index is _idx.
const SoftBodyNode * dart::dynamics::Skeleton::getSoftBodyNode | ( | std::size_t | _idx | ) | const |
Get const SoftBodyNode whose index is _idx.
Skeleton::State dart::dynamics::Skeleton::getState | ( | ) | const |
Get the State of this Skeleton [alias for getCompositeState()].
const std::pair< Eigen::Vector3d, Eigen::Vector3d > & dart::dynamics::Skeleton::getSupportAxes | ( | ) | const |
Get the axes that correspond to each component in the support polygon.
These axes are needed in order to map the points on a support polygon into 3D space. If gravity is along the z-direction, then these axes will simply be <1,0,0> and <0,1,0>.
const std::pair< Eigen::Vector3d, Eigen::Vector3d > & dart::dynamics::Skeleton::getSupportAxes | ( | std::size_t | _treeIdx | ) | const |
Same as getSupportAxes(), but it corresponds to the support polygon of the specified tree within this Skeleton.
const Eigen::Vector2d & dart::dynamics::Skeleton::getSupportCentroid | ( | ) | const |
Get the centroid of the support polygon for this Skeleton.
If the support polygon is an empty set, the components of this vector will be nan.
const Eigen::Vector2d & dart::dynamics::Skeleton::getSupportCentroid | ( | std::size_t | _treeIdx | ) | const |
Get the centroid of the support polygon for a tree in this Skeleton.
If the support polygon is an empty set, the components of this vector will be nan.
const std::vector< std::size_t > & dart::dynamics::Skeleton::getSupportIndices | ( | ) | const |
Get a list of the EndEffector indices that correspond to each of the points in the support polygon.
const std::vector< std::size_t > & dart::dynamics::Skeleton::getSupportIndices | ( | std::size_t | _treeIdx | ) | const |
Same as getSupportIndices(), but it corresponds to the support polygon of the specified tree within this Skeleton.
const math::SupportPolygon & dart::dynamics::Skeleton::getSupportPolygon | ( | ) | const |
const math::SupportPolygon & dart::dynamics::Skeleton::getSupportPolygon | ( | std::size_t | _treeIdx | ) | const |
Same as getSupportPolygon(), but it will only use EndEffectors within the specified tree within this Skeleton.
std::size_t dart::dynamics::Skeleton::getSupportVersion | ( | ) | const |
The version number of a support polygon will be incremented each time the support polygon needs to be recomputed.
This number can be used to immediately determine whether the support polygon has changed since the last time you asked for it, allowing you to be more efficient in how you handle the data.
std::size_t dart::dynamics::Skeleton::getSupportVersion | ( | std::size_t | _treeIdx | ) | const |
Same as getSupportVersion(), but it corresponds to the support polygon of the specified tree within this Skeleton.
double dart::dynamics::Skeleton::getTimeStep | ( | ) | const |
Get time step.
const std::vector< BodyNode * > & dart::dynamics::Skeleton::getTreeBodyNodes | ( | std::size_t | _treeIdx | ) |
Get the BodyNodes belonging to a tree in this Skeleton.
std::vector< const BodyNode * > dart::dynamics::Skeleton::getTreeBodyNodes | ( | std::size_t | _treeIdx | ) | const |
Get the BodyNodes belonging to a tree in this Skeleton.
const std::vector< DegreeOfFreedom * > & dart::dynamics::Skeleton::getTreeDofs | ( | std::size_t | _treeIdx | ) |
Get the DegreesOfFreedom belonging to a tree in this Skeleton.
const std::vector< const DegreeOfFreedom * > & dart::dynamics::Skeleton::getTreeDofs | ( | std::size_t | _treeIdx | ) | const |
Get the DegreesOfFreedom belonging to a tree in this Skeleton.
|
inherited |
Get the velocities for all generalized coordinates.
|
inherited |
Get the velocities for a subset of the generalized coordinates.
|
inherited |
Get the velocity of a single generalized coordinate.
|
inherited |
Get the velocity changes for all the generalized coordinates.
Eigen::VectorXd dart::dynamics::Skeleton::getVelocityDifferences | ( | const Eigen::VectorXd & | _dq2, |
const Eigen::VectorXd & | _dq1 | ||
) | const |
Return the difference of two generalized velocities or accelerations which are measured in the tangent space at the identity.
Since the tangent spaces are vector spaces, this function always returns _dq2 - _dq1.
|
inherited |
Get the lower limit of a generalized coordinate's velocity.
|
inherited |
Get the lower limits for all generalized coordinates's velocity.
|
inherited |
Get the lower limits for a subset of the generalized coordinates's velocity.
|
inherited |
Get the upper limit of a generalized coordinate's velocity.
|
inherited |
Get the upper limits for all generalized coordinates's velocity.
|
inherited |
Get the upper limits for a subset of the generalized coordinates's velocity.
|
virtualinherited |
Get the version number of this object.
|
overridevirtual |
Get the spatial Jacobian targeting the origin of a BodyNode.
The Jacobian is expressed in the World Frame.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Get the spatial Jacobian targeting an offset in a BodyNode.
The _offset is expected in coordinates of the BodyNode Frame. The Jacobian is expressed in the World Frame.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Returns whether this Skeleton contains bodyNode
.
Implements dart::dynamics::MetaSkeleton.
|
overridevirtual |
Returns whether this Skeleton contains join
.
Implements dart::dynamics::MetaSkeleton.
|
virtualinherited |
Increment the version for this object.
Reimplemented in dart::dynamics::Shape.
void dart::dynamics::Skeleton::integratePositions | ( | double | _dt | ) |
void dart::dynamics::Skeleton::integrateVelocities | ( | double | _dt | ) |
bool dart::dynamics::Skeleton::isEnabledAdjacentBodyCheck | ( | ) | const |
Return true if self-collision check is enabled including adjacent bodies.
bool dart::dynamics::Skeleton::isEnabledSelfCollisionCheck | ( | ) | const |
Return true if self-collision check is enabled.
bool dart::dynamics::Skeleton::isImpulseApplied | ( | ) | const |
Get whether this skeleton is constrained.
bool dart::dynamics::Skeleton::isMobile | ( | ) | const |
Get whether this skeleton will be updated by forward dynamics.
|
protected |
|
protected |
void dart::dynamics::Skeleton::notifyArticulatedInertiaUpdate | ( | std::size_t | _treeIdx | ) |
Notify that the articulated inertia and everything that depends on it needs to be updated.
void dart::dynamics::Skeleton::notifySupportUpdate | ( | std::size_t | _treeIdx | ) |
Notify that the support polygon of a tree needs to be updated.
Remove copy operator.
|
protected |
Take in and register a subtree of BodyNodes.
|
protected |
|
protected |
|
protected |
|
protected |
|
protectedinherited |
Remove an Observer from the list of Observers.
|
inherited |
Set all accelerations to zero.
|
inherited |
Set all commands to zero.
|
inherited |
Set all forces of the generalized coordinates to zero.
|
inherited |
Set all positions to zero.
|
inline |
|
inherited |
Set all velocities to zero.
|
protectedinherited |
Send a destruction notification to all Observers.
This will cause all Observers to behave as if this Subject has been permanently deleted, so it should only be called when that behavior is desired.
|
inherited |
Set the acceleration of a single generalized coordinate.
|
inherited |
Set the lower limit of a generalized coordinate's acceleration.
|
inherited |
Set the lower limits for all generalized coordinates's acceleration.
|
inherited |
Set the lower limits for a subset of the generalized coordinates's acceleration.
|
inherited |
Set the accelerations of all generalized coordinates.
|
inherited |
Set the accelerations of a subset of the generalized coordinates.
|
inherited |
Set the upper limit of a generalized coordinate's acceleration.
|
inherited |
Set the upper limits for all generalized coordinates's acceleration.
|
inherited |
Set the upper limits for a subset of the generalized coordinates's acceleration.
void dart::dynamics::Skeleton::setAdjacentBodyCheck | ( | bool | enable | ) |
Set whether to check adjacent bodies.
This option is effective only when the self-collision check is enabled.
void dart::dynamics::Skeleton::setAspectProperties | ( | const AspectProperties & | properties | ) |
Set the AspectProperties of this Skeleton.
|
inherited |
Set a single command.
|
inherited |
Set commands for all generalized coordinates.
|
inherited |
Set commands for a subset of the generalized coordinates.
void dart::dynamics::Skeleton::setConfiguration | ( | const Configuration & | configuration | ) |
Set the configuration of this Skeleton.
|
inherited |
Set the force of a single generalized coordinate.
|
inherited |
Set the lower limit of a generalized coordinate's force.
|
inherited |
Set the lower limits for all generalized coordinates's force.
|
inherited |
Set the lower limits for a subset of the generalized coordinates's force.
|
inherited |
Set the forces of all generalized coordinates.
|
inherited |
Set the forces of a subset of the generalized coordinates.
|
inherited |
Set the upper limit of a generalized coordinate's force.
|
inherited |
Set the upperlimits for all generalized coordinates's force.
|
inherited |
Set the upper limits for a subset of the generalized coordinates's force.
void dart::dynamics::Skeleton::setGravity | ( | const Eigen::Vector3d & | _gravity | ) |
Set 3-dim gravitational acceleration.
The gravity is used for calculating gravity force vector of the skeleton.
void dart::dynamics::Skeleton::setImpulseApplied | ( | bool | _val | ) |
Set whether this skeleton is constrained.
ConstraintSolver will mark this.
|
inherited |
Set the constraint impulses for the generalized coordinates.
void dart::dynamics::Skeleton::setMobile | ( | bool | _isMobile | ) |
Set whether this skeleton will be updated by forward dynamics.
[in] | _isMobile | True if this skeleton is mobile. |
|
overridevirtual |
Set name.
Implements dart::dynamics::MetaSkeleton.
|
inherited |
Set the position of a single generalized coordinate.
|
inherited |
Set the lower limit of a generalized coordinate's position.
|
inherited |
Set the lower limits for all generalized coordinates.
|
inherited |
Set the lower limits for a subset of the generalized coordinates.
|
inherited |
Set the positions for all generalized coordinates.
|
inherited |
Set the positions for a subset of the generalized coordinates.
|
inherited |
Set the upper limit of a generalized coordainte's position.
|
inherited |
Set the upper limits for all generalized coordinates.
|
inherited |
Set the upper limits for a subset of the generalized coordinates.
void dart::dynamics::Skeleton::setProperties | ( | const AspectProperties & | properties | ) |
Set the Properties of this Skeleton.
void dart::dynamics::Skeleton::setProperties | ( | const Properties & | properties | ) |
Set all properties of this Skeleton.
|
protected |
Setup this Skeleton with its shared_ptr.
void dart::dynamics::Skeleton::setSelfCollisionCheck | ( | bool | enable | ) |
Set whether to check self-collision.
void dart::dynamics::Skeleton::setState | ( | const State & | state | ) |
Set the State of this Skeleton [alias for setCompositeState(~)].
void dart::dynamics::Skeleton::setTimeStep | ( | double | _timeStep | ) |
Set time step.
This timestep is used for implicit joint damping force.
|
inherited |
Set the velocities of all generalized coordinates.
|
inherited |
Set the velocities of a subset of the generalized coordinates.
|
inherited |
Set the velocity of a single generalized coordinate.
|
inherited |
Set the lower limit of a generalized coordinate's velocity.
|
inherited |
Set the lower limits for all generalized coordinates's velocity.
|
inherited |
Set the lower limits for a subset of the generalized coordinates's velocity.
|
inherited |
Set the upper limit of a generalized coordinate's velocity.
|
inherited |
Set the upper limits for all generalized coordinates's velocity.
|
inherited |
Set the upper limits for a subset of the generalized coordinates's velocity.
|
protectedinherited |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
Update the articulated inertias of the skeleton.
|
protected |
Update the articulated inertia of a tree.
|
protected |
Update augmented mass matrix of the skeleton.
|
protected |
void dart::dynamics::Skeleton::updateBiasImpulse | ( | BodyNode * | _bodyNode | ) |
Update bias impulses.
void dart::dynamics::Skeleton::updateBiasImpulse | ( | BodyNode * | _bodyNode, |
const Eigen::Vector6d & | _imp | ||
) |
Update bias impulses due to impulse [_imp] on body node [_bodyNode].
_bodyNode | Body node contraint impulse, _imp, is applied |
_imp | Constraint impulse expressed in body frame of _bodyNode |
void dart::dynamics::Skeleton::updateBiasImpulse | ( | BodyNode * | _bodyNode1, |
const Eigen::Vector6d & | _imp1, | ||
BodyNode * | _bodyNode2, | ||
const Eigen::Vector6d & | _imp2 | ||
) |
Update bias impulses due to impulse [_imp] on body node [_bodyNode].
_bodyNode | Body node contraint impulse, _imp1, is applied |
_imp | Constraint impulse expressed in body frame of _bodyNode1 |
_bodyNode | Body node contraint impulse, _imp2, is applied |
_imp | Constraint impulse expressed in body frame of _bodyNode2 |
void dart::dynamics::Skeleton::updateBiasImpulse | ( | SoftBodyNode * | _softBodyNode, |
PointMass * | _pointMass, | ||
const Eigen::Vector3d & | _imp | ||
) |
Update bias impulses due to impulse[_imp] on body node [_bodyNode].
|
protected |
Update the dimensions for a specific cache.
|
protected |
Update the dimensions for a tree's cache.
|
protected |
Update combined vector of the skeleton.
|
protected |
Update the combined vector for a tree in this Skeleton.
|
protected |
Update Coriolis force vector of the skeleton.
|
protected |
Update Coriolis force vector for a tree in the Skeleton.
|
protected |
update external force vector to generalized forces.
|
protected |
Update external force vector to generalized forces for a tree.
|
protected |
Update gravity force vector of the skeleton.
|
protected |
Update the gravity force vector of a tree.
|
protected |
Update inverse of augmented mass matrix of the skeleton.
|
protected |
Update the inverse augmented mass matrix of a tree.
|
protected |
Update inverse of mass matrix of the skeleton.
|
protected |
Update the inverse mass matrix of a tree.
|
protected |
Update mass matrix of the skeleton.
|
protected |
Update the mass matrix of a tree.
|
protected |
Update the computation for total mass.
void dart::dynamics::Skeleton::updateVelocityChange | ( | ) |
Update velocity changes in body nodes and joints due to applied impulse.
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
protectedinherited |
Aspect::Properties data, directly accessible to your derived class.
|
privateinherited |
|
protected |
Flag for status of impulse testing.
|
mutableprotected |
|
protectedinherited |
|
protected |
NameManager for tracking BodyNodes.
|
protected |
NameManager for tracking DegreesOfFreedom.
|
protected |
NameManager for tracking Joints.
|
protected |
NameManager for tracking SoftBodyNodes.
|
mutableprotectedinherited |
List of current Observers.
|
protected |
The resource-managing pointer to this Skeleton.
|
mutableprotected |
|
protected |
List of Soft body node list in the skeleton.
|
protected |
|
protected |
Total mass.
|
mutableprotected |
std::size_t dart::dynamics::Skeleton::mUnionIndex |
std::weak_ptr<Skeleton> dart::dynamics::Skeleton::mUnionRootSkeleton |
std::size_t dart::dynamics::Skeleton::mUnionSize |
|
protectedinherited |
|
protected |
WholeBodyIK module for this Skeleton.
|
inherited |