DART
6.7.3
|
BodyNode class represents a single node of the skeleton. More...
#include <BodyNode.hpp>
Public Types | |
using | ColShapeAddedSignal = common::Signal< void(const BodyNode *, ConstShapePtr _newColShape)> |
using | ColShapeRemovedSignal = ColShapeAddedSignal |
using | StructuralChangeSignal = common::Signal< void(const BodyNode *)> |
using | CompositeProperties = common::Composite::Properties |
using | AllNodeStates = detail::AllNodeStates |
using | NodeStateMap = detail::NodeStateMap |
using | AllNodeProperties = detail::AllNodeProperties |
using | NodePropertiesMap = detail::NodePropertiesMap |
using | AspectProperties = detail::BodyNodeAspectProperties |
using | Properties = common::Composite::MakeProperties< BodyNode > |
using | Impl = EmbedStateAndProperties< DerivedT, StateDataT, PropertiesDataT > |
using | Derived = typename Impl::Derived |
using | AspectStateData = typename Impl::AspectStateData |
using | AspectState = typename Impl::AspectState |
using | AspectPropertiesData = typename Impl::AspectPropertiesData |
using | Aspect = typename Impl::Aspect |
using | Base = CompositeJoiner< Impl, CompositeBases... > |
using | EntitySignal = common::Signal< void(const Entity *)> |
using | FrameChangedSignal = common::Signal< void(const Entity *, const Frame *_oldFrame, const Frame *_newFrame)> |
using | NameChangedSignal = common::Signal< void(const Entity *, const std::string &_oldName, const std::string &_newName)> |
template<class Mixin > | |
using | MakeState = common::MakeCloneable< State, Mixin > |
Use the MakeState class to easily create a State extension from an existing class or struct. More... | |
template<class Mixin > | |
using | MakeProperties = common::MakeCloneable< Properties, Mixin > |
Use the MakeProperties class to easily create a Properties extension from an existing class or struct. More... | |
Public Member Functions | |
BodyNode (const BodyNode &)=delete | |
virtual | ~BodyNode () |
Destructor. More... | |
virtual SoftBodyNode * | asSoftBodyNode () |
Convert 'this' into a SoftBodyNode pointer if this BodyNode is a SoftBodyNode, otherwise return nullptr. More... | |
virtual const SoftBodyNode * | asSoftBodyNode () const |
Convert 'const this' into a SoftBodyNode pointer if this BodyNode is a SoftBodyNode, otherwise return nullptr. More... | |
void | setAllNodeStates (const AllNodeStates &states) |
Set the Node::State of all Nodes attached to this BodyNode. More... | |
AllNodeStates | getAllNodeStates () const |
Get the Node::State of all Nodes attached to this BodyNode. More... | |
void | setAllNodeProperties (const AllNodeProperties &properties) |
Set the Node::Properties of all Nodes attached to this BodyNode. More... | |
AllNodeProperties | getAllNodeProperties () const |
Get the Node::Properties of all Nodes attached to this BodyNode. More... | |
void | setProperties (const CompositeProperties &_properties) |
Same as setCompositeProperties() More... | |
void | setProperties (const AspectProperties &_properties) |
Set the UniqueProperties of this BodyNode. More... | |
void | setAspectState (const AspectState &state) |
Set the AspectState of this BodyNode. More... | |
void | setAspectProperties (const AspectProperties &properties) |
Set the AspectProperties of this BodyNode. More... | |
Properties | getBodyNodeProperties () const |
Get the Properties of this BodyNode. More... | |
void | copy (const BodyNode &otherBodyNode) |
Copy the Properties of another BodyNode. More... | |
void | copy (const BodyNode *otherBodyNode) |
Copy the Properties of another BodyNode. More... | |
BodyNode & | operator= (const BodyNode &_otherBodyNode) |
Same as copy(const BodyNode&) More... | |
void | duplicateNodes (const BodyNode *otherBodyNode) |
Give this BodyNode a copy of each Node from otherBodyNode. More... | |
void | matchNodes (const BodyNode *otherBodyNode) |
Make the Nodes of this BodyNode match the Nodes of otherBodyNode. More... | |
const std::string & | setName (const std::string &_name) override |
Set name. More... | |
const std::string & | getName () const override |
Get the name of this Node. More... | |
void | setGravityMode (bool _gravityMode) |
Set whether gravity affects this body. More... | |
bool | getGravityMode () const |
Return true if gravity mode is enabled. More... | |
bool | isCollidable () const |
Return true if this body can collide with others bodies. More... | |
void | setCollidable (bool _isCollidable) |
Set whether this body node will collide with others in the world. More... | |
void | setMass (double mass) |
Set the mass of the bodynode. More... | |
double | getMass () const |
Return the mass of the bodynode. More... | |
void | setMomentOfInertia (double _Ixx, double _Iyy, double _Izz, double _Ixy=0.0, double _Ixz=0.0, double _Iyz=0.0) |
Set moment of inertia defined around the center of mass. More... | |
void | getMomentOfInertia (double &_Ixx, double &_Iyy, double &_Izz, double &_Ixy, double &_Ixz, double &_Iyz) const |
Return moment of inertia defined around the center of mass. More... | |
const Eigen::Matrix6d & | getSpatialInertia () const |
Return spatial inertia. More... | |
void | setInertia (const Inertia &inertia) |
Set the inertia data for this BodyNode. More... | |
const Inertia & | getInertia () const |
Get the inertia data for this BodyNode. More... | |
const math::Inertia & | getArticulatedInertia () const |
Return the articulated body inertia. More... | |
const math::Inertia & | getArticulatedInertiaImplicit () const |
Return the articulated body inertia for implicit joint damping and spring forces. More... | |
void | setLocalCOM (const Eigen::Vector3d &_com) |
Set center of mass expressed in body frame. More... | |
const Eigen::Vector3d & | getLocalCOM () const |
Return center of mass expressed in body frame. More... | |
Eigen::Vector3d | getCOM (const Frame *_withRespectTo=Frame::World()) const |
Return the center of mass with respect to an arbitrary Frame. More... | |
Eigen::Vector3d | getCOMLinearVelocity (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const |
Return the linear velocity of the center of mass, expressed in terms of arbitrary Frames. More... | |
Eigen::Vector6d | getCOMSpatialVelocity () const |
Return the spatial velocity of the center of mass, expressed in coordinates of this Frame and relative to the World Frame. More... | |
Eigen::Vector6d | getCOMSpatialVelocity (const Frame *_relativeTo, const Frame *_inCoordinatesOf) const |
Return the spatial velocity of the center of mass, expressed in terms of arbitrary Frames. More... | |
Eigen::Vector3d | getCOMLinearAcceleration (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const |
Return the linear acceleration of the center of mass, expressed in terms of arbitary Frames. More... | |
Eigen::Vector6d | getCOMSpatialAcceleration () const |
Return the acceleration of the center of mass expressed in coordinates of this BodyNode Frame and relative to the World Frame. More... | |
Eigen::Vector6d | getCOMSpatialAcceleration (const Frame *_relativeTo, const Frame *_inCoordinatesOf) const |
Return the spatial acceleration of the center of mass, expressed in terms of arbitrary Frames. More... | |
void | setFrictionCoeff (double _coeff) |
Set coefficient of friction in range of [0, ~]. More... | |
double | getFrictionCoeff () const |
Return frictional coefficient. More... | |
void | setRestitutionCoeff (double _coeff) |
Set coefficient of restitution in range of [0, 1]. More... | |
double | getRestitutionCoeff () const |
Return coefficient of restitution. More... | |
std::size_t | getIndexInSkeleton () const |
Return the index of this BodyNode within its Skeleton. More... | |
std::size_t | getIndexInTree () const |
Return the index of this BodyNode within its tree. More... | |
std::size_t | getTreeIndex () const |
Return the index of the tree that this BodyNode belongs to. More... | |
SkeletonPtr | remove (const std::string &_name="temporary") |
Remove this BodyNode and all of its children (recursively) from their Skeleton. More... | |
bool | moveTo (BodyNode *_newParent) |
Remove this BodyNode and all of its children (recursively) from their current parent BodyNode, and move them to another parent BodyNode. More... | |
bool | moveTo (const SkeletonPtr &_newSkeleton, BodyNode *_newParent) |
This is a version of moveTo(BodyNode*) that allows you to explicitly move this BodyNode into a different Skeleton. More... | |
template<class JointType > | |
JointType * | moveTo (BodyNode *_newParent, const typename JointType::Properties &_joint=typename JointType::Properties()) |
A version of moveTo(BodyNode*) that also changes the Joint type of the parent Joint of this BodyNode. More... | |
template<class JointType > | |
JointType * | moveTo (const SkeletonPtr &_newSkeleton, BodyNode *_newParent, const typename JointType::Properties &_joint=typename JointType::Properties()) |
A version of moveTo(SkeletonPtr, BodyNode*) that also changes the Joint type of the parent Joint of this BodyNode. More... | |
SkeletonPtr | split (const std::string &_skeletonName) |
Remove this BodyNode and all of its children (recursively) from their current Skeleton and move them into a newly created Skeleton. More... | |
template<class JointType > | |
SkeletonPtr | split (const std::string &_skeletonName, const typename JointType::Properties &_joint=typename JointType::Properties()) |
A version of split(const std::string&) that also changes the Joint type of the parent Joint of this BodyNode. More... | |
template<class JointType > | |
JointType * | changeParentJointType (const typename JointType::Properties &_joint=typename JointType::Properties()) |
Change the Joint type of this BodyNode's parent Joint. More... | |
std::pair< Joint *, BodyNode * > | copyTo (BodyNode *_newParent, bool _recursive=true) |
Create clones of this BodyNode and all of its children recursively (unless _recursive is set to false) and attach the clones to the specified BodyNode. More... | |
std::pair< Joint *, BodyNode * > | copyTo (const SkeletonPtr &_newSkeleton, BodyNode *_newParent, bool _recursive=true) const |
Create clones of this BodyNode and all of its children recursively (unless recursive is set to false) and attach the clones to the specified BodyNode of the specified Skeleton. More... | |
template<class JointType > | |
std::pair< JointType *, BodyNode * > | copyTo (BodyNode *_newParent, const typename JointType::Properties &_joint=typename JointType::Properties(), bool _recursive=true) |
A version of copyTo(BodyNode*) that also changes the Joint type of the parent Joint of this BodyNode. More... | |
template<class JointType > | |
std::pair< JointType *, BodyNode * > | copyTo (const SkeletonPtr &_newSkeleton, BodyNode *_newParent, const typename JointType::Properties &_joint=typename JointType::Properties(), bool _recursive=true) const |
A version of copyTo(Skeleton*,BodyNode*) that also changes the Joint type of the parent Joint of this BodyNode. More... | |
SkeletonPtr | copyAs (const std::string &_skeletonName, bool _recursive=true) const |
Create clones of this BodyNode and all of its children (recursively) and create a new Skeleton with the specified name to attach them to. More... | |
template<class JointType > | |
SkeletonPtr | copyAs (const std::string &_skeletonName, const typename JointType::Properties &_joint=typename JointType::Properties(), bool _recursive=true) const |
A version of copyAs(const std::string&) that also changes the Joint type of the root BodyNode. More... | |
SkeletonPtr | getSkeleton () override |
Return the Skeleton that this Node is attached to. More... | |
ConstSkeletonPtr | getSkeleton () const override |
Return the Skeleton that this Node is attached to. More... | |
Joint * | getParentJoint () |
Return the parent Joint of this BodyNode. More... | |
const Joint * | getParentJoint () const |
Return the (const) parent Joint of this BodyNode. More... | |
BodyNode * | getParentBodyNode () |
Return the parent BodyNdoe of this BodyNode. More... | |
const BodyNode * | getParentBodyNode () const |
Return the (const) parent BodyNode of this BodyNode. More... | |
template<class JointType , class NodeType = BodyNode> | |
std::pair< JointType *, NodeType * > | createChildJointAndBodyNodePair (const typename JointType::Properties &_jointProperties=typename JointType::Properties(), const typename NodeType::Properties &_bodyProperties=typename NodeType::Properties()) |
Create a Joint and BodyNode pair as a child of this BodyNode. More... | |
std::size_t | getNumChildBodyNodes () const |
Return the number of child BodyNodes. More... | |
BodyNode * | getChildBodyNode (std::size_t _index) |
Return the _index-th child BodyNode of this BodyNode. More... | |
const BodyNode * | getChildBodyNode (std::size_t _index) const |
Return the (const) _index-th child BodyNode of this BodyNode. More... | |
std::size_t | getNumChildJoints () const |
Return the number of child Joints. More... | |
Joint * | getChildJoint (std::size_t _index) |
Return the _index-th child Joint of this BodyNode. More... | |
const Joint * | getChildJoint (std::size_t _index) const |
Return the (const) _index-th child Joint of this BodyNode. More... | |
template<class NodeType , typename ... Args> | |
NodeType * | createNode (Args &&... args) |
Create some Node type and attach it to this BodyNode. More... | |
std::size_t | getNumShapeNodes () const |
ShapeNode * | getShapeNode (std::size_t index) |
const ShapeNode * | getShapeNode (std::size_t index) const |
template<class ShapeNodeProperties > | |
ShapeNode * | createShapeNode (ShapeNodeProperties properties, bool automaticName=true) |
Create an ShapeNode attached to this BodyNode. More... | |
template<class ShapeType > | |
ShapeNode * | createShapeNode (const std::shared_ptr< ShapeType > &shape) |
Create a ShapeNode with an automatically assigned name: <BodyNodeName>ShapeNode<#>. More... | |
template<class ShapeType , class StringType > | |
ShapeNode * | createShapeNode (const std::shared_ptr< ShapeType > &shape, StringType &&name) |
Create a ShapeNode with the specified name. More... | |
const std::vector< ShapeNode * > | getShapeNodes () |
Return the list of ShapeNodes. More... | |
const std::vector< const ShapeNode * > | getShapeNodes () const |
Return the list of (const) ShapeNodes. More... | |
void | removeAllShapeNodes () |
Remove all ShapeNodes from this BodyNode. More... | |
template<class... Aspects> | |
ShapeNode * | createShapeNodeWith (const ShapePtr &shape) |
Create a ShapeNode with the specified Aspects and an automatically assigned name: <BodyNodeName>ShapeNode<#>. More... | |
template<class... Aspects> | |
ShapeNode * | createShapeNodeWith (const ShapePtr &shape, const std::string &name) |
Create a ShapeNode with the specified name and Aspects. More... | |
template<class Aspect > | |
std::size_t | getNumShapeNodesWith () const |
Return the number of ShapeNodes containing given Aspect in this BodyNode. More... | |
template<class Aspect > | |
const std::vector< ShapeNode * > | getShapeNodesWith () |
Return the list of ShapeNodes containing given Aspect. More... | |
template<class Aspect > | |
const std::vector< const ShapeNode * > | getShapeNodesWith () const |
Return the list of ShapeNodes containing given Aspect. More... | |
template<class Aspect > | |
void | removeAllShapeNodesWith () |
Remove all ShapeNodes containing given Aspect from this BodyNode. More... | |
std::size_t | getNumEndEffectors () const |
EndEffector * | getEndEffector (std::size_t index) |
const EndEffector * | getEndEffector (std::size_t index) const |
EndEffector * | createEndEffector (const EndEffector::BasicProperties &_properties) |
Create an EndEffector attached to this BodyNode. More... | |
EndEffector * | createEndEffector (const std::string &_name="EndEffector") |
Create an EndEffector with the specified name. More... | |
EndEffector * | createEndEffector (const char *_name) |
Create an EndEffector with the specified name. More... | |
std::size_t | getNumMarkers () const |
Marker * | getMarker (std::size_t index) |
const Marker * | getMarker (std::size_t index) const |
Marker * | createMarker (const std::string &name="marker", const Eigen::Vector3d &position=Eigen::Vector3d::Zero(), const Eigen::Vector4d &color=Eigen::Vector4d::Constant(1.0)) |
Create a Marker with the given fields. More... | |
Marker * | createMarker (const Marker::BasicProperties &properties) |
Create a Marker given its basic properties. More... | |
bool | dependsOn (std::size_t _genCoordIndex) const override |
Return true if _genCoordIndex-th generalized coordinate. More... | |
std::size_t | getNumDependentGenCoords () const override |
The number of the generalized coordinates which affect this JacobianNode. More... | |
std::size_t | getDependentGenCoordIndex (std::size_t _arrayIndex) const override |
Return a generalized coordinate index from the array index (< getNumDependentDofs) More... | |
const std::vector< std::size_t > & | getDependentGenCoordIndices () const override |
Indices of the generalized coordinates which affect this JacobianNode. More... | |
std::size_t | getNumDependentDofs () const override |
Same as getNumDependentGenCoords() More... | |
DegreeOfFreedom * | getDependentDof (std::size_t _index) override |
Get a pointer to the _indexth dependent DegreeOfFreedom for this BodyNode. More... | |
const DegreeOfFreedom * | getDependentDof (std::size_t _index) const override |
Get a pointer to the _indexth dependent DegreeOfFreedom for this BodyNode. More... | |
const std::vector< DegreeOfFreedom * > & | getDependentDofs () override |
Return a std::vector of DegreeOfFreedom pointers that this Node depends on. More... | |
const std::vector< const DegreeOfFreedom * > & | getDependentDofs () const override |
Return a std::vector of DegreeOfFreedom pointers that this Node depends on. More... | |
const std::vector< const DegreeOfFreedom * > | getChainDofs () const override |
Returns a DegreeOfFreedom vector containing the dofs that form a Chain leading up to this JacobianNode from the root of the Skeleton. More... | |
const Eigen::Isometry3d & | getRelativeTransform () const override |
Get the transform of this BodyNode with respect to its parent BodyNode, which is also its parent Frame. More... | |
const Eigen::Vector6d & | getRelativeSpatialVelocity () const override |
Get the spatial velocity of this Frame relative to its parent Frame, in its own coordinates. More... | |
const Eigen::Vector6d & | getRelativeSpatialAcceleration () const override |
Get the spatial acceleration of this Frame relative to its parent Frame, in the coordinates of this Frame. More... | |
const Eigen::Vector6d & | getPrimaryRelativeAcceleration () const override |
The Featherstone ABI algorithm exploits a component of the spatial acceleration which we refer to as the partial acceleration, accessible by getPartialAcceleration(). More... | |
const Eigen::Vector6d & | getPartialAcceleration () const override |
Return the partial acceleration of this body. More... | |
const math::Jacobian & | getJacobian () const override final |
Return the generalized Jacobian targeting the origin of this BodyNode. More... | |
const math::Jacobian & | getWorldJacobian () const override final |
Return the generalized Jacobian targeting the origin of this BodyNode. More... | |
const math::Jacobian & | getJacobianSpatialDeriv () const override final |
Return the spatial time derivative of the generalized Jacobian targeting the origin of this BodyNode. More... | |
const math::Jacobian & | getJacobianClassicDeriv () const override final |
Return the classical time derivative of the generalized Jacobian targeting the origin of this BodyNode. More... | |
const Eigen::Vector6d & | getBodyVelocityChange () const |
Return the velocity change due to the constraint impulse. More... | |
void | setColliding (bool _isColliding) |
Set whether this body node is colliding with other objects. More... | |
bool | isColliding () |
Return whether this body node is set to be colliding with other objects. More... | |
void | addExtForce (const Eigen::Vector3d &_force, const Eigen::Vector3d &_offset=Eigen::Vector3d::Zero(), bool _isForceLocal=false, bool _isOffsetLocal=true) |
Add applying linear Cartesian forces to this node. More... | |
void | setExtForce (const Eigen::Vector3d &_force, const Eigen::Vector3d &_offset=Eigen::Vector3d::Zero(), bool _isForceLocal=false, bool _isOffsetLocal=true) |
Set Applying linear Cartesian forces to this node. More... | |
void | addExtTorque (const Eigen::Vector3d &_torque, bool _isLocal=false) |
Add applying Cartesian torque to the node. More... | |
void | setExtTorque (const Eigen::Vector3d &_torque, bool _isLocal=false) |
Set applying Cartesian torque to the node. More... | |
virtual void | clearExternalForces () |
Clean up structures that store external forces: mContacts, mFext, mExtForceBody and mExtTorqueBody. More... | |
virtual void | clearInternalForces () |
Clear out the generalized forces of the parent Joint and any other forces related to this BodyNode that are internal to the Skeleton. More... | |
const Eigen::Vector6d & | getExternalForceLocal () const |
Eigen::Vector6d | getExternalForceGlobal () const |
const Eigen::Vector6d & | getBodyForce () const |
Get spatial body force transmitted from the parent joint. More... | |
bool | isReactive () const |
Return true if the body can react to force or constraint impulse. More... | |
void | setConstraintImpulse (const Eigen::Vector6d &_constImp) |
Set constraint impulse. More... | |
void | addConstraintImpulse (const Eigen::Vector6d &_constImp) |
Add constraint impulse. More... | |
void | addConstraintImpulse (const Eigen::Vector3d &_constImp, const Eigen::Vector3d &_offset, bool _isImpulseLocal=false, bool _isOffsetLocal=true) |
Add constraint impulse. More... | |
virtual void | clearConstraintImpulse () |
Clear constraint impulses and cache data used for impulse-based forward dynamics algorithm. More... | |
const Eigen::Vector6d & | getConstraintImpulse () const |
Return constraint impulse. More... | |
double | computeLagrangian (const Eigen::Vector3d &gravity) const |
Return Lagrangian of this body. More... | |
virtual double | getKineticEnergy () const |
Return kinetic energy. More... | |
double | computeKineticEnergy () const |
Return kinetic energy. More... | |
virtual double | getPotentialEnergy (const Eigen::Vector3d &_gravity) const |
Return potential energy. More... | |
double | computePotentialEnergy (const Eigen::Vector3d &gravity) const |
Return potential energy. More... | |
Eigen::Vector3d | getLinearMomentum () const |
Return linear momentum. More... | |
Eigen::Vector3d | getAngularMomentum (const Eigen::Vector3d &_pivot=Eigen::Vector3d::Zero()) |
Return angular momentum. More... | |
void | dirtyTransform () override |
Notify the transformation updates of this Frame and all its children are needed. More... | |
void | dirtyVelocity () override |
Notify the velocity updates of this Frame and all its children are needed. More... | |
void | dirtyAcceleration () override |
Notify the acceleration updates of this Frame and all its children are needed. More... | |
void | notifyArticulatedInertiaUpdate () |
Notify the Skeleton that the tree of this BodyNode needs an articulated inertia update. More... | |
void | dirtyArticulatedInertia () |
Notify the Skeleton that the tree of this BodyNode needs an articulated inertia update. More... | |
void | notifyExternalForcesUpdate () |
Tell the Skeleton that the external forces need to be updated. More... | |
void | dirtyExternalForces () |
Tell the Skeleton that the external forces need to be updated. More... | |
void | notifyCoriolisUpdate () |
Tell the Skeleton that the coriolis forces need to be update. More... | |
void | dirtyCoriolisForces () |
Tell the Skeleton that the coriolis forces need to be update. More... | |
const AspectState & | getAspectState () const |
const AspectProperties & | getAspectProperties () const |
math::Jacobian | getJacobian (const Frame *_inCoordinatesOf) const override final |
A version of getJacobian() that lets you specify a coordinate Frame to express the Jacobian in. More... | |
math::Jacobian | getJacobian (const Eigen::Vector3d &_offset) const override final |
Return the generalized Jacobian targeting an offset within the Frame of this JacobianNode. More... | |
math::Jacobian | getJacobian (const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf) const override final |
A version of getJacobian(const Eigen::Vector3d&) that lets you specify a coordinate Frame to express the Jacobian in. More... | |
math::Jacobian | getWorldJacobian (const Eigen::Vector3d &_offset) const override final |
Return the generalized Jacobian targeting an offset in this JacobianNode. More... | |
math::LinearJacobian | getLinearJacobian (const Frame *_inCoordinatesOf=Frame::World()) const override final |
Return the linear Jacobian targeting the origin of this BodyNode. More... | |
math::LinearJacobian | getLinearJacobian (const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf=Frame::World()) const override final |
Return the generalized Jacobian targeting an offset within the Frame of this BodyNode. More... | |
math::AngularJacobian | getAngularJacobian (const Frame *_inCoordinatesOf=Frame::World()) const override final |
Return the angular Jacobian targeting the origin of this BodyNode. More... | |
math::Jacobian | getJacobianSpatialDeriv (const Frame *_inCoordinatesOf) const override final |
A version of getJacobianSpatialDeriv() that can return the Jacobian in coordinates of any Frame. More... | |
math::Jacobian | getJacobianSpatialDeriv (const Eigen::Vector3d &_offset) const override final |
Return the spatial time derivative of the generalized Jacobian targeting an offset in the Frame of this BodyNode. More... | |
math::Jacobian | getJacobianSpatialDeriv (const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf) const override final |
A version of getJacobianSpatialDeriv(const Eigen::Vector3d&) that allows an arbitrary coordinate Frame to be specified. More... | |
math::Jacobian | getJacobianClassicDeriv (const Frame *_inCoordinatesOf) const override final |
A version of getJacobianClassicDeriv() that can return the Jacobian in coordinates of any Frame. More... | |
math::Jacobian | getJacobianClassicDeriv (const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf=Frame::World()) const override final |
A version of getJacobianClassicDeriv() that can compute the Jacobian for an offset within the Frame of this BodyNode. More... | |
math::LinearJacobian | getLinearJacobianDeriv (const Frame *_inCoordinatesOf=Frame::World()) const override final |
Return the linear Jacobian (classical) time derivative, in terms of any coordinate Frame. More... | |
math::LinearJacobian | getLinearJacobianDeriv (const Eigen::Vector3d &_offset, const Frame *_inCoordinatesOf=Frame::World()) const override final |
A version of getLinearJacobianDeriv() that can compute the Jacobian for an offset within the Frame of this BodyNode. More... | |
math::AngularJacobian | getAngularJacobianDeriv (const Frame *_inCoordinatesOf=Frame::World()) const override final |
Return the angular Jacobian time derivative, in terms of any coordinate Frame. More... | |
const std::shared_ptr< InverseKinematics > & | getIK (bool _createIfNull=false) |
Get a pointer to an IK module for this JacobianNode. More... | |
std::shared_ptr< const InverseKinematics > | getIK () const |
Get a pointer to an IK module for this JacobianNode. More... | |
const std::shared_ptr< InverseKinematics > & | getOrCreateIK () |
Get a pointer to an IK module for this JacobianNode. More... | |
const std::shared_ptr< InverseKinematics > & | createIK () |
Create a new IK module for this JacobianNode. More... | |
void | clearIK () |
Wipe away the IK module for this JacobianNode, leaving it as a nullptr. More... | |
void | notifyJacobianUpdate () |
Notify this BodyNode and all its descendents that their Jacobians need to be updated. More... | |
void | dirtyJacobian () |
Notify this BodyNode and all its descendents that their Jacobians need to be updated. More... | |
void | notifyJacobianDerivUpdate () |
Notify this BodyNode and all its descendents that their Jacobian derivatives need to be updated. More... | |
void | dirtyJacobianDeriv () |
Notify this BodyNode and all its descendents that their Jacobian derivatives need to be updated. More... | |
virtual const std::string & | setName (const std::string &newName)=0 |
Set the name of this Node. More... | |
virtual const std::string & | getName () const=0 |
Get the name of this Node. More... | |
const Eigen::Isometry3d & | getWorldTransform () const |
Get the transform of this Frame with respect to the World Frame. More... | |
Eigen::Isometry3d | getTransform (const Frame *_withRespectTo=Frame::World()) const |
Get the transform of this Frame with respect to some other Frame. More... | |
Eigen::Isometry3d | getTransform (const Frame *withRespectTo, const Frame *inCoordinatesOf) const |
Get the transform of this Frame with respect to some other Frame. More... | |
const Eigen::Vector6d & | getSpatialVelocity () const |
Get the total spatial velocity of this Frame in the coordinates of this Frame. More... | |
Eigen::Vector6d | getSpatialVelocity (const Frame *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial velocity of this Frame relative to some other Frame. More... | |
Eigen::Vector6d | getSpatialVelocity (const Eigen::Vector3d &_offset) const |
Get the spatial velocity of a fixed point in this Frame. More... | |
Eigen::Vector6d | getSpatialVelocity (const Eigen::Vector3d &_offset, const Frame *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial velocity of a fixed point in this Frame. More... | |
Eigen::Vector3d | getLinearVelocity (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const |
Get the linear portion of classical velocity of this Frame relative to some other Frame. More... | |
Eigen::Vector3d | getLinearVelocity (const Eigen::Vector3d &_offset, const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const |
Get the linear velocity of a point that is fixed in this Frame. More... | |
Eigen::Vector3d | getAngularVelocity (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const |
Get the angular portion of classical velocity of this Frame relative to some other Frame. More... | |
const Eigen::Vector6d & | getSpatialAcceleration () const |
Get the total spatial acceleration of this Frame in the coordinates of this Frame. More... | |
Eigen::Vector6d | getSpatialAcceleration (const Frame *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial acceleration of this Frame relative to some other Frame. More... | |
Eigen::Vector6d | getSpatialAcceleration (const Eigen::Vector3d &_offset) const |
Get the spatial acceleration of a fixed point in this Frame. More... | |
Eigen::Vector6d | getSpatialAcceleration (const Eigen::Vector3d &_offset, const Frame *_relativeTo, const Frame *_inCoordinatesOf) const |
Get the spatial acceleration of a fixed point in this Frame. More... | |
Eigen::Vector3d | getLinearAcceleration (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const |
Get the linear portion of classical acceleration of this Frame relative to some other Frame. More... | |
Eigen::Vector3d | getLinearAcceleration (const Eigen::Vector3d &_offset, const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const |
Eigen::Vector3d | getAngularAcceleration (const Frame *_relativeTo=Frame::World(), const Frame *_inCoordinatesOf=Frame::World()) const |
Get the angular portion of classical acceleration of this Frame relative to some other Frame. More... | |
const std::set< Entity * > & | getChildEntities () |
Get a container with the Entities that are children of this Frame. More... | |
const std::set< const Entity * > | getChildEntities () const |
Get a container with the Entities that are children of this Frame. More... | |
std::size_t | getNumChildEntities () const |
Get the number of Entities that are currently children of this Frame. More... | |
const std::set< Frame * > & | getChildFrames () |
Get a container with the Frames that are children of this Frame. More... | |
std::set< const Frame * > | getChildFrames () const |
Get a container with the Frames that are children of this Frame. More... | |
std::size_t | getNumChildFrames () const |
Get the number of Frames that are currently children of this Frame. More... | |
bool | isShapeFrame () const |
Returns true if this Frame is a ShapeFrame. More... | |
virtual ShapeFrame * | asShapeFrame () |
Convert 'this' into a ShapeFrame pointer if Frame is a ShapeFrame, otherwise return nullptr. More... | |
virtual const ShapeFrame * | asShapeFrame () const |
Convert 'const this' into a ShapeFrame pointer if Frame is a ShapeFrame, otherwise return nullptr. More... | |
bool | isWorld () const |
Returns true if this Frame is the World Frame. More... | |
Frame * | getParentFrame () |
Get the parent (reference) frame of this Entity. More... | |
const Frame * | getParentFrame () const |
Get the parent (reference) frame of this Entity. More... | |
bool | descendsFrom (const Frame *_someFrame) const |
True if and only if this Entity depends on (i.e. More... | |
bool | isFrame () const |
True iff this Entity is also a Frame. More... | |
bool | isQuiet () const |
Returns true if this Entity is set to be quiet. More... | |
virtual void | notifyTransformUpdate () |
Notify the transformation update of this Entity that its parent Frame's pose is needed. More... | |
bool | needsTransformUpdate () const |
Returns true iff a transform update is needed for this Entity. More... | |
virtual void | notifyVelocityUpdate () |
Notify the velocity update of this Entity that its parent Frame's velocity is needed. More... | |
bool | needsVelocityUpdate () const |
Returns true iff a velocity update is needed for this Entity. More... | |
virtual void | notifyAccelerationUpdate () |
Notify the acceleration of this Entity that its parent Frame's acceleration is needed. More... | |
bool | needsAccelerationUpdate () const |
Returns true iff an acceleration update is needed for this Entity. More... | |
virtual void | setNodeState (const State &otherState) |
Set the State of this Node. By default, this does nothing. More... | |
virtual std::unique_ptr< State > | getNodeState () const |
Get the State of this Node. More... | |
virtual void | copyNodeStateTo (std::unique_ptr< State > &outputState) const |
Copy the State of this Node into a unique_ptr. More... | |
virtual void | setNodeProperties (const Properties &properties) |
Set the Properties of this Node. By default, this does nothing. More... | |
virtual std::unique_ptr< Properties > | getNodeProperties () const |
Get the Properties of this Node. More... | |
virtual void | copyNodePropertiesTo (std::unique_ptr< Properties > &outputProperties) const |
Copy the Properties of this Node into a unique_ptr. More... | |
BodyNodePtr | getBodyNodePtr () |
Get a pointer to the BodyNode that this Node is associated with. More... | |
ConstBodyNodePtr | getBodyNodePtr () const |
Get a pointer to the BodyNode that this Node is associated with. More... | |
bool | isRemoved () const |
Returns true if this Node has been staged for removal from its 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... | |
Static Public Member Functions | |
static Frame * | World () |
Public Attributes | |
Slot registers | |
common::SlotRegister< ColShapeAddedSignal > | onColShapeAdded |
Slot register for collision shape added signal. More... | |
common::SlotRegister< ColShapeRemovedSignal > | onColShapeRemoved |
Slot register for collision shape removed signal. More... | |
common::SlotRegister< StructuralChangeSignal > | onStructuralChange |
Raised when (1) parent BodyNode is changed, (2) moved between Skeletons, (3) parent Joint is changed. More... | |
Protected Types | |
enum | ConstructAbstractTag { ConstructAbstract } |
Used when constructing a pure abstract class, because calling the Frame constructor is just a formality. More... | |
enum | ConstructFrameTag { ConstructFrame } |
Used when constructing a Frame class, because the Frame constructor will take care of setting up the parameters you pass into it. More... | |
Protected Member Functions | |
BodyNode (BodyNode *_parentBodyNode, Joint *_parentJoint, const Properties &_properties) | |
Constructor called by Skeleton class. More... | |
BodyNode (const std::tuple< BodyNode *, Joint *, Properties > &args) | |
Delegating constructor. More... | |
virtual BodyNode * | clone (BodyNode *_parentBodyNode, Joint *_parentJoint, bool cloneNodes) const |
Create a clone of this BodyNode. More... | |
Node * | cloneNode (BodyNode *bn) const override final |
This is needed in order to inherit the Node class, but it does nothing. More... | |
virtual void | init (const SkeletonPtr &_skeleton) |
Initialize the vector members with proper sizes. More... | |
void | addChildBodyNode (BodyNode *_body) |
Add a child bodynode into the bodynode. More... | |
virtual void | changeParentFrame (Frame *_newParentFrame) override |
Used by derived classes to change their parent frames. More... | |
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... | |
std::string | registerNameChange (const std::string &newName) |
Inform the Skeleton that the name of this Node has changed. More... | |
void | attach () |
Attach the Node to its BodyNode. More... | |
void | stageForRemoval () |
When all external references to the Node disappear, it will be deleted. More... | |
void | setVersionDependentObject (VersionCounter *dependent) |
Recursive dynamics routines | |
void | processNewEntity (Entity *_newChildEntity) override |
Separate generic child Entities from child BodyNodes for more efficient update notices. More... | |
void | processRemovedEntity (Entity *_oldChildEntity) override |
Remove this Entity from mChildBodyNodes or mNonBodyNodeEntities. More... | |
virtual void | updateTransform () |
Update transformation. More... | |
virtual void | updateVelocity () |
Update spatial body velocity. More... | |
virtual void | updatePartialAcceleration () const |
Update partial spatial body acceleration due to parent joint's velocity. More... | |
virtual void | updateArtInertia (double _timeStep) const |
Update articulated body inertia for forward dynamics. More... | |
virtual void | updateBiasForce (const Eigen::Vector3d &_gravity, double _timeStep) |
Update bias force associated with the articulated body inertia for forward dynamics. More... | |
virtual void | updateBiasImpulse () |
Update bias impulse associated with the articulated body inertia for impulse-based forward dynamics. More... | |
virtual void | updateAccelerationID () |
Update spatial body acceleration with the partial spatial body acceleration for inverse dynamics. More... | |
virtual void | updateAccelerationFD () |
Update spatial body acceleration for forward dynamics. More... | |
virtual void | updateVelocityChangeFD () |
Update spatical body velocity change for impluse-based forward dynamics. More... | |
virtual void | updateTransmittedForceID (const Eigen::Vector3d &_gravity, bool _withExternalForces=false) |
Update spatial body force for inverse dynamics. More... | |
virtual void | updateTransmittedForceFD () |
Update spatial body force for forward dynamics. More... | |
virtual void | updateTransmittedImpulse () |
Update spatial body force for impulse-based forward dynamics. More... | |
virtual void | updateJointForceID (double _timeStep, bool _withDampingForces, bool _withSpringForces) |
Update the joint force for inverse dynamics. More... | |
virtual void | updateJointForceFD (double _timeStep, bool _withDampingForces, bool _withSpringForces) |
Update the joint force for forward dynamics. More... | |
virtual void | updateJointImpulseFD () |
Update the joint impulse for forward dynamics. More... | |
virtual void | updateConstrainedTerms (double _timeStep) |
Update constrained terms due to the constraint impulses for foward dynamics. More... | |
Equations of motion related routines | |
virtual void | updateMassMatrix () |
virtual void | aggregateMassMatrix (Eigen::MatrixXd &_MCol, std::size_t _col) |
virtual void | aggregateAugMassMatrix (Eigen::MatrixXd &_MCol, std::size_t _col, double _timeStep) |
virtual void | updateInvMassMatrix () |
virtual void | updateInvAugMassMatrix () |
virtual void | aggregateInvMassMatrix (Eigen::MatrixXd &_InvMCol, std::size_t _col) |
virtual void | aggregateInvAugMassMatrix (Eigen::MatrixXd &_InvMCol, std::size_t _col, double _timeStep) |
virtual void | aggregateCoriolisForceVector (Eigen::VectorXd &_C) |
virtual void | aggregateGravityForceVector (Eigen::VectorXd &_g, const Eigen::Vector3d &_gravity) |
virtual void | updateCombinedVector () |
virtual void | aggregateCombinedVector (Eigen::VectorXd &_Cg, const Eigen::Vector3d &_gravity) |
virtual void | aggregateExternalForces (Eigen::VectorXd &_Fext) |
Aggregate the external forces mFext in the generalized coordinates recursively. More... | |
virtual void | aggregateSpatialToGeneralized (Eigen::VectorXd &_generalized, const Eigen::Vector6d &_spatial) |
void | updateBodyJacobian () const |
Update body Jacobian. More... | |
void | updateWorldJacobian () const |
Update the World Jacobian. More... | |
void | updateBodyJacobianSpatialDeriv () const |
Update spatial time derivative of body Jacobian. More... | |
void | updateWorldJacobianClassicDeriv () const |
Update classic time derivative of body Jacobian. More... | |
Protected Attributes | |
int | mID |
A unique ID of this node globally. More... | |
bool | mIsColliding |
Whether the node is currently in collision with another node. More... | |
std::size_t | mIndexInSkeleton |
Index of this BodyNode in its Skeleton. More... | |
std::size_t | mIndexInTree |
Index of this BodyNode in its Tree. More... | |
std::size_t | mTreeIndex |
Index of this BodyNode's tree. More... | |
Joint * | mParentJoint |
Parent joint. More... | |
BodyNode * | mParentBodyNode |
Parent body node. More... | |
std::vector< BodyNode * > | mChildBodyNodes |
Array of child body nodes. More... | |
std::set< Entity * > | mNonBodyNodeEntities |
Array of child Entities that are not BodyNodes. More... | |
std::vector< std::size_t > | mDependentGenCoordIndices |
A increasingly sorted list of dependent dof indices. More... | |
std::vector< DegreeOfFreedom * > | mDependentDofs |
A version of mDependentGenCoordIndices that holds DegreeOfFreedom pointers instead of indices. More... | |
std::vector< const DegreeOfFreedom * > | mConstDependentDofs |
Same as mDependentDofs, but holds const pointers. More... | |
math::Jacobian | mBodyJacobian |
Body Jacobian. More... | |
math::Jacobian | mWorldJacobian |
Cached World Jacobian. More... | |
math::Jacobian | mBodyJacobianSpatialDeriv |
Spatial time derivative of body Jacobian. More... | |
math::Jacobian | mWorldJacobianClassicDeriv |
Classic time derivative of Body Jacobian. More... | |
Eigen::Vector6d | mPartialAcceleration |
Partial spatial body acceleration due to parent joint's velocity. More... | |
bool | mIsPartialAccelerationDirty |
Is the partial acceleration vector dirty. More... | |
Eigen::Vector6d | mF |
Transmitted wrench from parent to the bodynode expressed in body-fixed frame. More... | |
Eigen::Vector6d | mFgravity |
Spatial gravity force. More... | |
math::Inertia | mArtInertia |
Articulated body inertia. More... | |
math::Inertia | mArtInertiaImplicit |
Articulated body inertia for implicit joint damping and spring forces. More... | |
Eigen::Vector6d | mBiasForce |
Bias force. More... | |
Eigen::Vector6d | mCg_dV |
Cache data for combined vector of the system. More... | |
Eigen::Vector6d | mCg_F |
Eigen::Vector6d | mG_F |
Cache data for gravity force vector of the system. More... | |
Eigen::Vector6d | mFext_F |
Cache data for external force vector of the system. More... | |
Eigen::Vector6d | mM_dV |
Cache data for mass matrix of the system. More... | |
Eigen::Vector6d | mM_F |
Eigen::Vector6d | mInvM_c |
Cache data for inverse mass matrix of the system. More... | |
Eigen::Vector6d | mInvM_U |
Eigen::Vector6d | mArbitrarySpatial |
Cache data for arbitrary spatial value. More... | |
Eigen::Vector6d | mDelV |
Velocity change due to to external impulsive force exerted on bodies of the parent skeleton. More... | |
Eigen::Vector6d | mBiasImpulse |
Impulsive bias force due to external impulsive force exerted on bodies of the parent skeleton. More... | |
Eigen::Vector6d | mConstraintImpulse |
Constraint impulse: contact impulse, dynamic joint impulse. More... | |
Eigen::Vector6d | mImpF |
Generalized impulsive body force w.r.t. body frame. More... | |
ColShapeAddedSignal | mColShapeAddedSignal |
Collision shape added signal. More... | |
ColShapeRemovedSignal | mColShapeRemovedSignal |
Collision shape removed signal. More... | |
StructuralChangeSignal | mStructuralChangeSignal |
Structural change signal. More... | |
AspectState | mAspectState |
Aspect::State data, directly accessible to your derived class. More... | |
AspectProperties | mAspectProperties |
Aspect::Properties data, directly accessible to your derived class. More... | |
std::weak_ptr< Skeleton > | mSkeleton |
Weak pointer to the Skeleton this BodyNode belongs to. More... | |
std::atomic< int > | mReferenceCount |
Reference count for the number of BodyNodePtrs that are referring to this BodyNode. More... | |
std::shared_ptr< Skeleton > | mReferenceSkeleton |
If mReferenceCount is zero, then mReferenceSkeleton will hold a nullptr. More... | |
std::shared_ptr< MutexedWeakSkeletonPtr > | mLockedSkeleton |
Shared reference to a weak_ptr of this BodyNode's Skeleton, along with a mutex to ensure thread safety. More... | |
bool | mIsBodyJacobianDirty |
Dirty flag for body Jacobian. More... | |
bool | mIsWorldJacobianDirty |
Dirty flag for world Jacobian. More... | |
bool | mIsBodyJacobianSpatialDerivDirty |
Dirty flag for spatial time derivative of body Jacobian. More... | |
bool | mIsWorldJacobianClassicDerivDirty |
Dirty flag for the classic time derivative of the Jacobian. More... | |
std::shared_ptr< InverseKinematics > | mIK |
Inverse kinematics module which gets lazily created upon request. More... | |
std::unordered_set< JacobianNode * > | mChildJacobianNodes |
JacobianNode children that descend from this JacobianNode. More... | |
Eigen::Isometry3d | mWorldTransform |
World transform of this Frame. More... | |
Eigen::Vector6d | mVelocity |
Total velocity of this Frame, in the coordinates of this Frame. More... | |
Eigen::Vector6d | mAcceleration |
Total acceleration of this Frame, in the coordinates of this Frame. More... | |
std::set< Frame * > | mChildFrames |
Container of this Frame's child Frames. More... | |
std::set< Entity * > | mChildEntities |
Container of this Frame's child Entities. More... | |
Frame * | mParentFrame |
Parent frame of this Entity. More... | |
bool | mNeedTransformUpdate |
Does this Entity need a Transform update. More... | |
bool | mNeedVelocityUpdate |
Does this Entity need a Velocity update. More... | |
bool | mNeedAccelerationUpdate |
Does this Entity need an Acceleration update. More... | |
FrameChangedSignal | mFrameChangedSignal |
Frame changed signal. More... | |
NameChangedSignal | mNameChangedSignal |
Name changed signal. More... | |
EntitySignal | mTransformUpdatedSignal |
Transform changed signal. More... | |
EntitySignal | mVelocityChangedSignal |
Velocity changed signal. More... | |
EntitySignal | mAccelerationChangedSignal |
Acceleration changed signal. More... | |
std::set< Observer * > | mObservers |
List of current Observers. More... | |
std::weak_ptr< NodeDestructor > | mDestructor |
weak pointer to the destructor for this Node. More... | |
BodyNode * | mBodyNode |
Pointer to the BodyNode that this Node is attached to. More... | |
bool | mAmAttached |
bool that tracks whether this Node is attached to its BodyNode More... | |
std::size_t | mIndexInBodyNode |
The index of this Node within its vector in its BodyNode's NodeMap. More... | |
std::size_t | mVersion |
Static Protected Attributes | |
static std::size_t | msBodyNodeCount = 0 |
Counts the number of nodes globally. More... | |
Private Types | |
enum | ConstructWorldTag { ConstructWorld } |
Used when constructing the World. More... | |
Private Member Functions | |
void | incrementReferenceCount () const |
Atomically increment the reference count for this BodyNode. More... | |
void | decrementReferenceCount () const |
Atomically decrement the reference count for this BodyNode. More... | |
std::shared_ptr< NodeDestructor > | getOrCreateDestructor () |
Private Attributes | |
std::shared_ptr< NodeDestructor > | mSelfDestructor |
Hold onto a reference to this BodyNode's own Destructor to make sure that it never gets destroyed. More... | |
const bool | mAmWorld |
Contains whether or not this is the World Frame. More... | |
bool | mAmShapeFrame |
Contains whether or not this is a ShapeFrame. More... | |
VersionCounter * | mDependent |
Friends | |
class | Skeleton |
class | Joint |
class | EndEffector |
class | SoftBodyNode |
class | PointMass |
class | Node |
Slot registers | |
common::SlotRegister< FrameChangedSignal > | onFrameChanged |
Slot register for frame changed signal. More... | |
common::SlotRegister< NameChangedSignal > | onNameChanged |
Slot register for name changed signal. More... | |
common::SlotRegister< EntitySignal > | onTransformUpdated |
Slot register for transform updated signal. More... | |
common::SlotRegister< EntitySignal > | onVelocityChanged |
Slot register for velocity updated signal. More... | |
common::SlotRegister< EntitySignal > | onAccelerationChanged |
Slot register for acceleration updated signal. More... | |
const bool | mAmQuiet |
Whether or not this Entity is set to be quiet. More... | |
bool | mAmFrame |
Whether or not this Entity is a Frame. More... | |
BodyNode class represents a single node of the skeleton.
BodyNode is a basic element of the skeleton. BodyNodes are hierarchically connected and have a set of core functions for calculating derivatives.
BodyNode inherits Frame, and a parent Frame of a BodyNode is the parent BodyNode of the BodyNode.
|
inherited |
|
inherited |
|
inherited |
|
inherited |
|
inherited |
using dart::dynamics::BodyNode::ColShapeAddedSignal = common::Signal<void(const BodyNode*, ConstShapePtr _newColShape)> |
|
inherited |
|
inherited |
|
inherited |
|
inherited |
|
inherited |
Use the MakeProperties class to easily create a Properties extension from an existing class or struct.
|
inherited |
Use the MakeState class to easily create a State extension from an existing class or struct.
|
inherited |
using dart::dynamics::BodyNode::StructuralChangeSignal = common::Signal<void(const BodyNode*)> |
|
protectedinherited |
Used when constructing a pure abstract class, because calling the Frame constructor is just a formality.
Enumerator | |
---|---|
ConstructAbstract |
|
protectedinherited |
|
privateinherited |
|
delete |
|
virtual |
Destructor.
|
protected |
Constructor called by Skeleton class.
|
protected |
Delegating constructor.
|
protected |
Add a child bodynode into the bodynode.
void dart::dynamics::BodyNode::addConstraintImpulse | ( | const Eigen::Vector3d & | _constImp, |
const Eigen::Vector3d & | _offset, | ||
bool | _isImpulseLocal = false , |
||
bool | _isOffsetLocal = true |
||
) |
Add constraint impulse.
void dart::dynamics::BodyNode::addConstraintImpulse | ( | const Eigen::Vector6d & | _constImp | ) |
Add constraint impulse.
[in] | _constImp | Spatial constraint impulse w.r.t. body frame |
void dart::dynamics::BodyNode::addExtForce | ( | const Eigen::Vector3d & | _force, |
const Eigen::Vector3d & | _offset = Eigen::Vector3d::Zero() , |
||
bool | _isForceLocal = false , |
||
bool | _isOffsetLocal = true |
||
) |
Add applying linear Cartesian forces to this node.
A force is defined by a point of application and a force vector. The last two parameters specify frames of the first two parameters. Coordinate transformations are applied when needed. The point of application and the force in local coordinates are stored in mContacts. When conversion is needed, make sure the transformations are avaialble.
void dart::dynamics::BodyNode::addExtTorque | ( | const Eigen::Vector3d & | _torque, |
bool | _isLocal = false |
||
) |
Add applying Cartesian torque to the node.
The torque in local coordinates is accumulated in mExtTorqueBody.
|
protectedinherited |
Add an Observer to the list of Observers.
|
protectedvirtual |
|
protectedvirtual |
|
protectedvirtual |
|
protectedvirtual |
Aggregate the external forces mFext in the generalized coordinates recursively.
|
protectedvirtual |
|
protectedvirtual |
|
protectedvirtual |
|
protectedvirtual |
|
protectedvirtual |
|
virtualinherited |
Convert 'this' into a ShapeFrame pointer if Frame is a ShapeFrame, otherwise return nullptr.
Reimplemented in dart::dynamics::ShapeFrame.
|
virtualinherited |
Convert 'const this' into a ShapeFrame pointer if Frame is a ShapeFrame, otherwise return nullptr.
Reimplemented in dart::dynamics::ShapeFrame.
|
virtual |
Convert 'this' into a SoftBodyNode pointer if this BodyNode is a SoftBodyNode, otherwise return nullptr.
|
virtual |
Convert 'const this' into a SoftBodyNode pointer if this BodyNode is a SoftBodyNode, otherwise return nullptr.
|
overrideprotectedvirtualinherited |
Used by derived classes to change their parent frames.
Reimplemented from dart::dynamics::Entity.
JointType * dart::dynamics::BodyNode::changeParentJointType | ( | const typename JointType::Properties & | _joint = typename JointType::Properties() | ) |
|
virtual |
Clear constraint impulses and cache data used for impulse-based forward dynamics algorithm.
|
virtual |
Clean up structures that store external forces: mContacts, mFext, mExtForceBody and mExtTorqueBody.
Called by Skeleton::clearExternalForces.
|
inherited |
Wipe away the IK module for this JacobianNode, leaving it as a nullptr.
|
virtual |
This is needed in order to inherit the Node class, but it does nothing.
Implements dart::dynamics::Node.
double dart::dynamics::BodyNode::computeKineticEnergy | ( | ) | const |
Return kinetic energy.
double dart::dynamics::BodyNode::computeLagrangian | ( | const Eigen::Vector3d & | gravity | ) | const |
Return Lagrangian of this body.
double dart::dynamics::BodyNode::computePotentialEnergy | ( | const Eigen::Vector3d & | gravity | ) | const |
Return potential energy.
void dart::dynamics::BodyNode::copy | ( | const BodyNode & | otherBodyNode | ) |
Copy the Properties of another BodyNode.
void dart::dynamics::BodyNode::copy | ( | const BodyNode * | otherBodyNode | ) |
Copy the Properties of another BodyNode.
SkeletonPtr dart::dynamics::BodyNode::copyAs | ( | const std::string & | _skeletonName, |
bool | _recursive = true |
||
) | const |
Create clones of this BodyNode and all of its children (recursively) and create a new Skeleton with the specified name to attach them to.
The Skeleton::Properties of the current Skeleton will also be copied into the new Skeleton that gets created.
SkeletonPtr dart::dynamics::BodyNode::copyAs | ( | const std::string & | _skeletonName, |
const typename JointType::Properties & | _joint = typename JointType::Properties() , |
||
bool | _recursive = true |
||
) | const |
|
virtualinherited |
Copy the Properties of this Node into a unique_ptr.
By default, this simply calls getNodeProperties() and passes the result into the outputProperties, but this function can be overriden to be more performant.
|
virtualinherited |
Copy the State of this Node into a unique_ptr.
By default, this simply calls getNodeState() and passes the result into the outputState, but this function can be overriden to be more performant by avoiding allocations.
std::pair< Joint *, BodyNode * > dart::dynamics::BodyNode::copyTo | ( | BodyNode * | _newParent, |
bool | _recursive = true |
||
) |
Create clones of this BodyNode and all of its children recursively (unless _recursive is set to false) and attach the clones to the specified BodyNode.
The specified BodyNode can be in this Skeleton or a different Skeleton. Passing in nullptr will set the copy as a root node of the current Skeleton.
The return value is a pair of pointers to the root of the newly created BodyNode tree.
std::pair< Joint *, BodyNode * > dart::dynamics::BodyNode::copyTo | ( | const SkeletonPtr & | _newSkeleton, |
BodyNode * | _newParent, | ||
bool | _recursive = true |
||
) | const |
Create clones of this BodyNode and all of its children recursively (unless recursive is set to false) and attach the clones to the specified BodyNode of the specified Skeleton.
The key differences between this function and the copyTo(BodyNode*) version is that this one allows the copied BodyNode to be const and allows you to copy it as a root node of another Skeleton.
The return value is a pair of pointers to the root of the newly created BodyNode tree.
std::pair< JointType *, BodyNode * > dart::dynamics::BodyNode::copyTo | ( | const SkeletonPtr & | _newSkeleton, |
BodyNode * | _newParent, | ||
const typename JointType::Properties & | _joint = typename JointType::Properties() , |
||
bool | _recursive = true |
||
) | const |
std::pair< JointType *, NodeType * > dart::dynamics::BodyNode::createChildJointAndBodyNodePair | ( | const typename JointType::Properties & | _jointProperties = typename JointType::Properties() , |
const typename NodeType::Properties & | _bodyProperties = typename NodeType::Properties() |
||
) |
EndEffector * dart::dynamics::BodyNode::createEndEffector | ( | const char * | _name | ) |
Create an EndEffector with the specified name.
EndEffector * dart::dynamics::BodyNode::createEndEffector | ( | const EndEffector::BasicProperties & | _properties | ) |
Create an EndEffector attached to this BodyNode.
Pass an EndEffector::Properties argument into this function.
EndEffector * dart::dynamics::BodyNode::createEndEffector | ( | const std::string & | _name = "EndEffector" | ) |
Create an EndEffector with the specified name.
|
inherited |
Create a new IK module for this JacobianNode.
If an IK module already exists in this JacobianNode, it will be destroyed and replaced by a brand new one.
Marker * dart::dynamics::BodyNode::createMarker | ( | const Marker::BasicProperties & | properties | ) |
Create a Marker given its basic properties.
Marker * dart::dynamics::BodyNode::createMarker | ( | const std::string & | name = "marker" , |
const Eigen::Vector3d & | position = Eigen::Vector3d::Zero() , |
||
const Eigen::Vector4d & | color = Eigen::Vector4d::Constant(1.0) |
||
) |
Create a Marker with the given fields.
NodeType * dart::dynamics::BodyNode::createNode | ( | Args &&... | args | ) |
ShapeNode * dart::dynamics::BodyNode::createShapeNode | ( | const std::shared_ptr< ShapeType > & | shape | ) |
ShapeNode * dart::dynamics::BodyNode::createShapeNode | ( | const std::shared_ptr< ShapeType > & | shape, |
StringType && | name | ||
) |
Create a ShapeNode with the specified name.
ShapeNode * dart::dynamics::BodyNode::createShapeNode | ( | ShapeNodeProperties | properties, |
bool | automaticName = true |
||
) |
Create an ShapeNode attached to this BodyNode.
Pass a ShapeNode::Properties argument into its constructor. If automaticName is true, then the mName field of properties will be ignored, and the ShapeNode will be automatically assigned a name: <BodyNodeName>ShapeNode<#>
ShapeNode * dart::dynamics::BodyNode::createShapeNodeWith | ( | const ShapePtr & | shape, |
const std::string & | name | ||
) |
Create a ShapeNode with the specified name and Aspects.
|
privateinherited |
Atomically decrement the reference count for this BodyNode.
This should only be called by the BodyNodePtr class
|
overridevirtual |
Return true if _genCoordIndex-th generalized coordinate.
Implements dart::dynamics::JacobianNode.
|
inherited |
|
overridevirtual |
Notify the acceleration updates of this Frame and all its children are needed.
Reimplemented from dart::dynamics::Frame.
void dart::dynamics::BodyNode::dirtyArticulatedInertia | ( | ) |
void dart::dynamics::BodyNode::dirtyCoriolisForces | ( | ) |
Tell the Skeleton that the coriolis forces need to be update.
void dart::dynamics::BodyNode::dirtyExternalForces | ( | ) |
Tell the Skeleton that the external forces need to be updated.
|
inherited |
Notify this BodyNode and all its descendents that their Jacobians need to be updated.
|
inherited |
Notify this BodyNode and all its descendents that their Jacobian derivatives need to be updated.
|
overridevirtual |
Notify the transformation updates of this Frame and all its children are needed.
Reimplemented from dart::dynamics::Frame.
|
overridevirtual |
Notify the velocity updates of this Frame and all its children are needed.
Reimplemented from dart::dynamics::Frame.
void dart::dynamics::BodyNode::duplicateNodes | ( | const BodyNode * | otherBodyNode | ) |
BodyNode::AllNodeProperties dart::dynamics::BodyNode::getAllNodeProperties | ( | ) | const |
Get the Node::Properties of all Nodes attached to this BodyNode.
BodyNode::AllNodeStates dart::dynamics::BodyNode::getAllNodeStates | ( | ) | const |
Get the Node::State of all Nodes attached to this BodyNode.
|
inherited |
|
finaloverridevirtualinherited |
Return the angular Jacobian targeting the origin of this BodyNode.
You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
Return the angular Jacobian time derivative, in terms of any coordinate Frame.
Implements dart::dynamics::JacobianNode.
Eigen::Vector3d dart::dynamics::BodyNode::getAngularMomentum | ( | const Eigen::Vector3d & | _pivot = Eigen::Vector3d::Zero() | ) |
Return angular momentum.
|
inherited |
const math::Inertia & dart::dynamics::BodyNode::getArticulatedInertia | ( | ) | const |
Return the articulated body inertia.
const math::Inertia & dart::dynamics::BodyNode::getArticulatedInertiaImplicit | ( | ) | const |
Return the articulated body inertia for implicit joint damping and spring forces.
|
inlineinherited |
|
inlineinherited |
const Eigen::Vector6d & dart::dynamics::BodyNode::getBodyForce | ( | ) | const |
BodyNode::Properties dart::dynamics::BodyNode::getBodyNodeProperties | ( | ) | const |
Get the Properties of this BodyNode.
|
inherited |
|
inherited |
const Eigen::Vector6d & dart::dynamics::BodyNode::getBodyVelocityChange | ( | ) | const |
Return the velocity change due to the constraint impulse.
|
overridevirtual |
Returns a DegreeOfFreedom vector containing the dofs that form a Chain leading up to this JacobianNode from the root of the Skeleton.
Implements dart::dynamics::JacobianNode.
BodyNode * dart::dynamics::BodyNode::getChildBodyNode | ( | std::size_t | _index | ) |
const BodyNode * dart::dynamics::BodyNode::getChildBodyNode | ( | std::size_t | _index | ) | const |
|
inherited |
|
inherited |
Get a container with the Entities that are children of this Frame.
Note that this is version is slightly less efficient than the non-const version because it needs to rebuild a set where each pointer is converted to be a const pointer.
|
inherited |
|
inherited |
Get a container with the Frames that are children of this Frame.
Note that this version is less efficient than the non-const version because it needs to rebuild a set so that the entries are const.
Joint * dart::dynamics::BodyNode::getChildJoint | ( | std::size_t | _index | ) |
const Joint * dart::dynamics::BodyNode::getChildJoint | ( | std::size_t | _index | ) | const |
Eigen::Vector3d dart::dynamics::BodyNode::getCOM | ( | const Frame * | _withRespectTo = Frame::World() | ) | const |
Return the center of mass with respect to an arbitrary Frame.
Eigen::Vector3d dart::dynamics::BodyNode::getCOMLinearAcceleration | ( | const Frame * | _relativeTo = Frame::World() , |
const Frame * | _inCoordinatesOf = Frame::World() |
||
) | const |
Return the linear acceleration of the center of mass, expressed in terms of arbitary Frames.
Eigen::Vector3d dart::dynamics::BodyNode::getCOMLinearVelocity | ( | const Frame * | _relativeTo = Frame::World() , |
const Frame * | _inCoordinatesOf = Frame::World() |
||
) | const |
Return the linear velocity of the center of mass, expressed in terms of arbitrary Frames.
Eigen::Vector6d dart::dynamics::BodyNode::getCOMSpatialAcceleration | ( | ) | const |
Eigen::Vector6d dart::dynamics::BodyNode::getCOMSpatialAcceleration | ( | const Frame * | _relativeTo, |
const Frame * | _inCoordinatesOf | ||
) | const |
Return the spatial acceleration of the center of mass, expressed in terms of arbitrary Frames.
Eigen::Vector6d dart::dynamics::BodyNode::getCOMSpatialVelocity | ( | ) | const |
Eigen::Vector6d dart::dynamics::BodyNode::getCOMSpatialVelocity | ( | const Frame * | _relativeTo, |
const Frame * | _inCoordinatesOf | ||
) | const |
Return the spatial velocity of the center of mass, expressed in terms of arbitrary Frames.
const Eigen::Vector6d & dart::dynamics::BodyNode::getConstraintImpulse | ( | ) | const |
Return constraint impulse.
|
overridevirtual |
Get a pointer to the _indexth dependent DegreeOfFreedom for this BodyNode.
Implements dart::dynamics::JacobianNode.
|
overridevirtual |
Get a pointer to the _indexth dependent DegreeOfFreedom for this BodyNode.
Implements dart::dynamics::JacobianNode.
|
overridevirtual |
Return a std::vector of DegreeOfFreedom pointers that this Node depends on.
Implements dart::dynamics::JacobianNode.
|
overridevirtual |
Return a std::vector of DegreeOfFreedom pointers that this Node depends on.
Implements dart::dynamics::JacobianNode.
|
overridevirtual |
Return a generalized coordinate index from the array index (< getNumDependentDofs)
Implements dart::dynamics::JacobianNode.
|
overridevirtual |
Indices of the generalized coordinates which affect this JacobianNode.
Implements dart::dynamics::JacobianNode.
EndEffector * dart::dynamics::BodyNode::getEndEffector | ( | std::size_t | index | ) |
const EndEffector * dart::dynamics::BodyNode::getEndEffector | ( | std::size_t | index | ) | const |
Eigen::Vector6d dart::dynamics::BodyNode::getExternalForceGlobal | ( | ) | const |
const Eigen::Vector6d & dart::dynamics::BodyNode::getExternalForceLocal | ( | ) | const |
double dart::dynamics::BodyNode::getFrictionCoeff | ( | ) | const |
Return frictional coefficient.
bool dart::dynamics::BodyNode::getGravityMode | ( | ) | const |
Return true if gravity mode is enabled.
|
inherited |
Get a pointer to an IK module for this JacobianNode.
Because this is a const function, a new IK module cannot be created if one does not already exist.
|
inherited |
Get a pointer to an IK module for this JacobianNode.
If _createIfNull is true, then the IK module will be generated if one does not already exist.
std::size_t dart::dynamics::BodyNode::getIndexInSkeleton | ( | ) | const |
std::size_t dart::dynamics::BodyNode::getIndexInTree | ( | ) | const |
Return the index of this BodyNode within its tree.
const Inertia & dart::dynamics::BodyNode::getInertia | ( | ) | const |
Get the inertia data for this BodyNode.
|
finaloverridevirtual |
Return the generalized Jacobian targeting the origin of this BodyNode.
The Jacobian is expressed in the Frame of this BodyNode.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
Return the generalized Jacobian targeting an offset within the Frame of this JacobianNode.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
A version of getJacobian(const Eigen::Vector3d&) that lets you specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
A version of getJacobian() that lets you specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtual |
Return the classical time derivative of the generalized Jacobian targeting the origin of this BodyNode.
The Jacobian is expressed in the World coordinate Frame.
NOTE: Since this is a classical time derivative, it should be used with classical linear and angular vectors. If you are using spatial vectors, use getJacobianSpatialDeriv() instead.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
A version of getJacobianClassicDeriv() that can compute the Jacobian for an offset within the Frame of this BodyNode.
The offset must be expressed in the coordinates of this BodyNode Frame.
NOTE: Since this is a classical time derivative, it should be used with classical linear and angular vectors. If you are using spatial vectors, use getJacobianSpatialDeriv() instead.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
A version of getJacobianClassicDeriv() that can return the Jacobian in coordinates of any Frame.
NOTE: Since this is a classical time derivative, it should be used with classical linear and angular vectors. If you are using spatial vectors, use getJacobianSpatialDeriv() instead.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtual |
Return the spatial time derivative of the generalized Jacobian targeting the origin of this BodyNode.
The Jacobian is expressed in this BodyNode's coordinate Frame.
NOTE: Since this is a spatial time derivative, it should be used with spatial vectors. If you are using classical linear and angular acceleration vectors, then use getJacobianClassicDeriv(), getLinearJacobianDeriv(), or getAngularJacobianDeriv() instead.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
Return the spatial time derivative of the generalized Jacobian targeting an offset in the Frame of this BodyNode.
The Jacobian is expressed in this BodyNode's coordinate Frame.
NOTE: This Jacobian Derivative is only for use with spatial vectors. If you are using classic linear and angular vectors, then use getJacobianClassicDeriv(), getLinearJacobianDeriv(), or getAngularJacobianDeriv() instead.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
A version of getJacobianSpatialDeriv(const Eigen::Vector3d&) that allows an arbitrary coordinate Frame to be specified.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
A version of getJacobianSpatialDeriv() that can return the Jacobian in coordinates of any Frame.
NOTE: This Jacobian Derivative is only for use with spatial vectors. If you are using classical linear and angular vectors, then use getJacobianClassicDeriv(), getLinearJacobianDeriv(), or getAngularJacobianDeriv() instead.
Implements dart::dynamics::JacobianNode.
|
virtual |
Return kinetic energy.
|
inherited |
|
inherited |
|
finaloverridevirtualinherited |
Return the generalized Jacobian targeting an offset within the Frame of this BodyNode.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
Return the linear Jacobian targeting the origin of this BodyNode.
You can specify a coordinate Frame to express the Jacobian in.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
A version of getLinearJacobianDeriv() that can compute the Jacobian for an offset within the Frame of this BodyNode.
The offset must be expressed in coordinates of this BodyNode Frame.
NOTE: Since this is a classical time derivative, it should be used with classical linear vectors. If you are using spatial vectors, use getJacobianSpatialDeriv() instead.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
Return the linear Jacobian (classical) time derivative, in terms of any coordinate Frame.
NOTE: Since this is a classical time derivative, it should be used with classical linear vectors. If you are using spatial vectors, use getJacobianSpatialDeriv() instead.
Implements dart::dynamics::JacobianNode.
Eigen::Vector3d dart::dynamics::BodyNode::getLinearMomentum | ( | ) | const |
Return linear momentum.
|
inherited |
|
inherited |
const Eigen::Vector3d & dart::dynamics::BodyNode::getLocalCOM | ( | ) | const |
Return center of mass expressed in body frame.
Marker * dart::dynamics::BodyNode::getMarker | ( | std::size_t | index | ) |
const Marker * dart::dynamics::BodyNode::getMarker | ( | std::size_t | index | ) | const |
double dart::dynamics::BodyNode::getMass | ( | ) | const |
Return the mass of the bodynode.
void dart::dynamics::BodyNode::getMomentOfInertia | ( | double & | _Ixx, |
double & | _Iyy, | ||
double & | _Izz, | ||
double & | _Ixy, | ||
double & | _Ixz, | ||
double & | _Iyz | ||
) | const |
Return moment of inertia defined around the center of mass.
|
overridevirtual |
Get the name of this Node.
Implements dart::dynamics::Node.
|
inherited |
Get the name of this Node.
|
virtualinherited |
|
virtualinherited |
std::size_t dart::dynamics::BodyNode::getNumChildBodyNodes | ( | ) | const |
Return the number of child BodyNodes.
|
inherited |
Get the number of Entities that are currently children of this Frame.
|
inherited |
Get the number of Frames that are currently children of this Frame.
std::size_t dart::dynamics::BodyNode::getNumChildJoints | ( | ) | const |
Return the number of child Joints.
|
overridevirtual |
Same as getNumDependentGenCoords()
Implements dart::dynamics::JacobianNode.
|
overridevirtual |
The number of the generalized coordinates which affect this JacobianNode.
Implements dart::dynamics::JacobianNode.
std::size_t dart::dynamics::BodyNode::getNumEndEffectors | ( | ) | const |
std::size_t dart::dynamics::BodyNode::getNumMarkers | ( | ) | const |
std::size_t dart::dynamics::BodyNode::getNumShapeNodes | ( | ) | const |
std::size_t dart::dynamics::BodyNode::getNumShapeNodesWith |
Return the number of ShapeNodes containing given Aspect in this BodyNode.
|
privateinherited |
|
inherited |
Get a pointer to an IK module for this JacobianNode.
The IK module will be generated if one does not already exist. This function is actually the same as getIK(true).
BodyNode * dart::dynamics::BodyNode::getParentBodyNode | ( | ) |
Return the parent BodyNdoe of this BodyNode.
const BodyNode * dart::dynamics::BodyNode::getParentBodyNode | ( | ) | const |
|
inherited |
Get the parent (reference) frame of this Entity.
|
inherited |
Get the parent (reference) frame of this Entity.
const Joint * dart::dynamics::BodyNode::getParentJoint | ( | ) | const |
|
overridevirtual |
Return the partial acceleration of this body.
Implements dart::dynamics::Frame.
|
virtual |
Return potential energy.
|
overridevirtual |
The Featherstone ABI algorithm exploits a component of the spatial acceleration which we refer to as the partial acceleration, accessible by getPartialAcceleration().
We save operations during our forward kinematics by computing and storing the partial acceleration separately from the rest of the Frame's acceleration. getPrimaryRelativeAcceleration() will return the portion of the relative spatial acceleration that is not contained in the partial acceleration. To get the full spatial acceleration of this Frame relative to its parent Frame, use getRelativeSpatialAcceleration(). To get the full spatial acceleration of this Frame relative to the World Frame, use getSpatialAcceleration().
Implements dart::dynamics::Frame.
|
overridevirtual |
Get the spatial acceleration of this Frame relative to its parent Frame, in the coordinates of this Frame.
Implements dart::dynamics::Frame.
|
overridevirtual |
Get the spatial velocity of this Frame relative to its parent Frame, in its own coordinates.
Implements dart::dynamics::Frame.
|
overridevirtual |
Get the transform of this BodyNode with respect to its parent BodyNode, which is also its parent Frame.
Implements dart::dynamics::Frame.
double dart::dynamics::BodyNode::getRestitutionCoeff | ( | ) | const |
Return coefficient of restitution.
ShapeNode * dart::dynamics::BodyNode::getShapeNode | ( | std::size_t | index | ) |
const ShapeNode * dart::dynamics::BodyNode::getShapeNode | ( | std::size_t | index | ) | const |
const std::vector< ShapeNode * > dart::dynamics::BodyNode::getShapeNodes | ( | ) |
Return the list of ShapeNodes.
const std::vector< const ShapeNode * > dart::dynamics::BodyNode::getShapeNodes | ( | ) | const |
Return the list of (const) ShapeNodes.
const std::vector< ShapeNode * > dart::dynamics::BodyNode::getShapeNodesWith |
Return the list of ShapeNodes containing given Aspect.
const std::vector< const ShapeNode * > dart::dynamics::BodyNode::getShapeNodesWith |
Return the list of ShapeNodes containing given Aspect.
|
overridevirtual |
Return the Skeleton that this Node is attached to.
Reimplemented from dart::dynamics::Node.
|
overridevirtual |
Return the Skeleton that this Node is attached to.
Reimplemented from dart::dynamics::Node.
|
inherited |
|
inherited |
|
inherited |
Get the spatial acceleration of a fixed point in this Frame.
|
inherited |
const Eigen::Matrix6d & dart::dynamics::BodyNode::getSpatialInertia | ( | ) | const |
Return spatial inertia.
|
inherited |
|
inherited |
|
inherited |
Get the spatial velocity of a fixed point in this Frame.
|
inherited |
|
inherited |
std::size_t dart::dynamics::BodyNode::getTreeIndex | ( | ) | const |
Return the index of the tree that this BodyNode belongs to.
|
virtualinherited |
Get the version number of this object.
|
finaloverridevirtual |
Return the generalized Jacobian targeting the origin of this BodyNode.
The Jacobian is expressed in the World Frame.
Implements dart::dynamics::JacobianNode.
|
finaloverridevirtualinherited |
Return the generalized Jacobian targeting an offset in this JacobianNode.
The _offset is expected in coordinates of this BodyNode Frame. The Jacobian is expressed in the World Frame.
Implements dart::dynamics::JacobianNode.
|
inherited |
|
privateinherited |
Atomically increment the reference count for this BodyNode.
This should only be called by the BodyNodePtr class
|
virtualinherited |
Increment the version for this object.
Reimplemented in dart::dynamics::Shape.
|
protectedvirtual |
Initialize the vector members with proper sizes.
bool dart::dynamics::BodyNode::isCollidable | ( | ) | const |
Return true if this body can collide with others bodies.
bool dart::dynamics::BodyNode::isColliding | ( | ) |
Return whether this body node is set to be colliding with other objects.
|
inherited |
Returns true if this Entity is set to be quiet.
A quiet entity is unknown to its parent Frame. It will not be tracked by its parent; it will not receive notifications from its parent, and it will not be rendered. The advantage to a quiet Entity is that it has less overhead when constructing and deconstructing, which makes it more suitable for temporary objects.
bool dart::dynamics::BodyNode::isReactive | ( | ) | const |
Return true if the body can react to force or constraint impulse.
A body node is reactive if the skeleton is mobile and the number of dependent generalized coordinates is non zero.
|
inherited |
Returns true if this Node has been staged for removal from its BodyNode.
It will be deleted once all strong references to it expire. If it is an AccessoryNode, you can call reattach() to prevent that from happening.
|
inherited |
Returns true if this Frame is a ShapeFrame.
|
inherited |
void dart::dynamics::BodyNode::matchNodes | ( | const BodyNode * | otherBodyNode | ) |
bool dart::dynamics::BodyNode::moveTo | ( | BodyNode * | _newParent | ) |
Remove this BodyNode and all of its children (recursively) from their current parent BodyNode, and move them to another parent BodyNode.
The new parent BodyNode can either be in a new Skeleton or the current one. If you pass in a nullptr, this BodyNode will become a new root BodyNode for its current Skeleton.
Using this function will result in changes to the indexing of (potentially) all BodyNodes and Joints in the current Skeleton, even if the BodyNodes are kept within the same Skeleton.
JointType * dart::dynamics::BodyNode::moveTo | ( | BodyNode * | _newParent, |
const typename JointType::Properties & | _joint = typename JointType::Properties() |
||
) |
A version of moveTo(BodyNode*) that also changes the Joint type of the parent Joint of this BodyNode.
This function returns the pointer to the newly created Joint. The original parent Joint will be deleted.
This function can be used to change the Joint type of the parent Joint of this BodyNode, but note that the indexing of the BodyNodes and Joints in this Skeleton will still be changed, even if only the Joint type is changed.
bool dart::dynamics::BodyNode::moveTo | ( | const SkeletonPtr & | _newSkeleton, |
BodyNode * | _newParent | ||
) |
This is a version of moveTo(BodyNode*) that allows you to explicitly move this BodyNode into a different Skeleton.
The key difference for this version of the function is that you can make this BodyNode a root node in a different Skeleton, which is not something that can be done by the other version.
JointType * dart::dynamics::BodyNode::moveTo | ( | const SkeletonPtr & | _newSkeleton, |
BodyNode * | _newParent, | ||
const typename JointType::Properties & | _joint = typename JointType::Properties() |
||
) |
|
inherited |
Returns true iff an acceleration update is needed for this Entity.
|
inherited |
Returns true iff a transform update is needed for this Entity.
|
inherited |
Returns true iff a velocity update is needed for this Entity.
|
virtualinherited |
void dart::dynamics::BodyNode::notifyArticulatedInertiaUpdate | ( | ) |
void dart::dynamics::BodyNode::notifyCoriolisUpdate | ( | ) |
Tell the Skeleton that the coriolis forces need to be update.
void dart::dynamics::BodyNode::notifyExternalForcesUpdate | ( | ) |
Tell the Skeleton that the external forces need to be updated.
|
inherited |
Notify this BodyNode and all its descendents that their Jacobian derivatives need to be updated.
|
inherited |
Notify this BodyNode and all its descendents that their Jacobians need to be updated.
|
virtualinherited |
|
virtualinherited |
Same as copy(const BodyNode&)
|
overrideprotectedvirtual |
Separate generic child Entities from child BodyNodes for more efficient update notices.
Reimplemented from dart::dynamics::Frame.
|
overrideprotectedvirtual |
Remove this Entity from mChildBodyNodes or mNonBodyNodeEntities.
Reimplemented from dart::dynamics::Frame.
|
protectedinherited |
SkeletonPtr dart::dynamics::BodyNode::remove | ( | const std::string & | _name = "temporary" | ) |
Remove this BodyNode and all of its children (recursively) from their Skeleton.
If a BodyNodePtr that references this BodyNode (or any of its children) still exists, the subtree will be moved into a new Skeleton with the given name. If the returned SkeletonPtr goes unused and no relevant BodyNodePtrs are held anywhere, then this BodyNode and all its children will be deleted.
Note that this function is actually the same as split(), but given a different name for semantic reasons.
void dart::dynamics::BodyNode::removeAllShapeNodes | ( | ) |
Remove all ShapeNodes from this BodyNode.
void dart::dynamics::BodyNode::removeAllShapeNodesWith |
Remove all ShapeNodes containing given Aspect from this BodyNode.
|
protectedinherited |
Remove an Observer from the list of Observers.
|
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.
void dart::dynamics::BodyNode::setAllNodeProperties | ( | const AllNodeProperties & | properties | ) |
Set the Node::Properties of all Nodes attached to this BodyNode.
void dart::dynamics::BodyNode::setAllNodeStates | ( | const AllNodeStates & | states | ) |
Set the Node::State of all Nodes attached to this BodyNode.
void dart::dynamics::BodyNode::setAspectProperties | ( | const AspectProperties & | properties | ) |
Set the AspectProperties of this BodyNode.
void dart::dynamics::BodyNode::setAspectState | ( | const AspectState & | state | ) |
Set the AspectState of this BodyNode.
void dart::dynamics::BodyNode::setCollidable | ( | bool | _isCollidable | ) |
Set whether this body node will collide with others in the world.
[in] | _isCollidable | True to enable collisions |
void dart::dynamics::BodyNode::setColliding | ( | bool | _isColliding | ) |
Set whether this body node is colliding with other objects.
Note that this status is set by the constraint solver during dynamics simulation but not by collision detector.
[in] | True | if this body node is colliding. |
void dart::dynamics::BodyNode::setConstraintImpulse | ( | const Eigen::Vector6d & | _constImp | ) |
Set constraint impulse.
[in] | _constImp | Spatial constraint impulse w.r.t. body frame |
void dart::dynamics::BodyNode::setExtForce | ( | const Eigen::Vector3d & | _force, |
const Eigen::Vector3d & | _offset = Eigen::Vector3d::Zero() , |
||
bool | _isForceLocal = false , |
||
bool | _isOffsetLocal = true |
||
) |
Set Applying linear Cartesian forces to this node.
void dart::dynamics::BodyNode::setExtTorque | ( | const Eigen::Vector3d & | _torque, |
bool | _isLocal = false |
||
) |
Set applying Cartesian torque to the node.
The torque in local coordinates is accumulated in mExtTorqueBody.
void dart::dynamics::BodyNode::setFrictionCoeff | ( | double | _coeff | ) |
Set coefficient of friction in range of [0, ~].
void dart::dynamics::BodyNode::setGravityMode | ( | bool | _gravityMode | ) |
Set whether gravity affects this body.
[in] | _gravityMode | True to enable gravity |
void dart::dynamics::BodyNode::setInertia | ( | const Inertia & | inertia | ) |
Set the inertia data for this BodyNode.
void dart::dynamics::BodyNode::setLocalCOM | ( | const Eigen::Vector3d & | _com | ) |
Set center of mass expressed in body frame.
void dart::dynamics::BodyNode::setMass | ( | double | mass | ) |
Set the mass of the bodynode.
void dart::dynamics::BodyNode::setMomentOfInertia | ( | double | _Ixx, |
double | _Iyy, | ||
double | _Izz, | ||
double | _Ixy = 0.0 , |
||
double | _Ixz = 0.0 , |
||
double | _Iyz = 0.0 |
||
) |
Set moment of inertia defined around the center of mass.
Principal moments of inertia (_Ixx, _Iyy, _Izz) must be positive or zero values.
|
overridevirtual |
Set name.
If the name is already taken, this will return an altered version which will be used by the Skeleton
Implements dart::dynamics::Node.
|
inherited |
Set the name of this Node.
|
virtualinherited |
Set the Properties of this Node. By default, this does nothing.
|
virtualinherited |
Set the State of this Node. By default, this does nothing.
void dart::dynamics::BodyNode::setProperties | ( | const AspectProperties & | _properties | ) |
Set the UniqueProperties of this BodyNode.
void dart::dynamics::BodyNode::setProperties | ( | const CompositeProperties & | _properties | ) |
Same as setCompositeProperties()
void dart::dynamics::BodyNode::setRestitutionCoeff | ( | double | _coeff | ) |
Set coefficient of restitution in range of [0, 1].
|
protectedinherited |
SkeletonPtr dart::dynamics::BodyNode::split | ( | const std::string & | _skeletonName | ) |
Remove this BodyNode and all of its children (recursively) from their current Skeleton and move them into a newly created Skeleton.
The newly created Skeleton will have the same Skeleton::Properties as the current Skeleton, except it will use the specified name. The return value is a shared_ptr to the newly created Skeleton.
Note that the parent Joint of this BodyNode will remain the same. If you want to change the Joint type of this BodyNode's parent Joint (for example, make it a FreeJoint), then use the templated split<JointType>() function.
SkeletonPtr dart::dynamics::BodyNode::split | ( | const std::string & | _skeletonName, |
const typename JointType::Properties & | _joint = typename JointType::Properties() |
||
) |
A version of split(const std::string&) that also changes the Joint type of the parent Joint of this BodyNode.
|
protectedinherited |
When all external references to the Node disappear, it will be deleted.
|
protectedvirtual |
Update spatial body acceleration for forward dynamics.
|
protectedvirtual |
Update spatial body acceleration with the partial spatial body acceleration for inverse dynamics.
|
protectedvirtual |
Update articulated body inertia for forward dynamics.
[in] | _timeStep | Rquired for implicit joint stiffness and damping. |
|
protectedvirtual |
Update bias force associated with the articulated body inertia for forward dynamics.
[in] | _gravity | Vector of gravitational acceleration |
[in] | _timeStep | Rquired for implicit joint stiffness and damping. |
|
protectedvirtual |
Update bias impulse associated with the articulated body inertia for impulse-based forward dynamics.
|
protected |
Update body Jacobian.
getJacobian() calls this function if mIsBodyJacobianDirty is true.
|
protected |
Update spatial time derivative of body Jacobian.
getJacobianSpatialTimeDeriv() calls this function if mIsBodyJacobianSpatialDerivDirty is true.
|
protectedvirtual |
|
protectedvirtual |
Update constrained terms due to the constraint impulses for foward dynamics.
|
protectedvirtual |
|
protectedvirtual |
|
protectedvirtual |
Update the joint force for forward dynamics.
|
protectedvirtual |
Update the joint force for inverse dynamics.
|
protectedvirtual |
Update the joint impulse for forward dynamics.
|
protectedvirtual |
|
protectedvirtual |
Update partial spatial body acceleration due to parent joint's velocity.
|
protectedvirtual |
Update transformation.
|
protectedvirtual |
|
protectedvirtual |
|
protectedvirtual |
|
protectedvirtual |
Update spatial body velocity.
|
protectedvirtual |
Update spatical body velocity change for impluse-based forward dynamics.
|
protected |
Update the World Jacobian.
The commonality of using the World Jacobian makes it worth caching.
|
protected |
Update classic time derivative of body Jacobian.
getJacobianClassicDeriv() calls this function if mIsWorldJacobianClassicDerivDirty is true.
|
staticinherited |
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
mutableprotectedinherited |
Total acceleration of this Frame, in the coordinates of this Frame.
Do not use directly! Use getSpatialAcceleration() to access this quantity
|
protectedinherited |
Acceleration changed signal.
|
protectedinherited |
|
privateinherited |
Whether or not this Entity is set to be quiet.
|
privateinherited |
Contains whether or not this is a ShapeFrame.
|
privateinherited |
Contains whether or not this is the World Frame.
|
protected |
Cache data for arbitrary spatial value.
|
mutableprotected |
Articulated body inertia.
Do not use directly! Use getArticulatedInertia() to access this quantity
|
mutableprotected |
Articulated body inertia for implicit joint damping and spring forces.
DO not use directly! Use getArticulatedInertiaImplicit() to access this
|
protectedinherited |
Aspect::Properties data, directly accessible to your derived class.
|
protectedinherited |
Aspect::State data, directly accessible to your derived class.
|
protected |
Bias force.
|
protected |
Impulsive bias force due to external impulsive force exerted on bodies of the parent skeleton.
|
mutableprotected |
Body Jacobian.
Do not use directly! Use getJacobian() to access this quantity
|
mutableprotected |
Spatial time derivative of body Jacobian.
Do not use directly! Use getJacobianSpatialDeriv() to access this quantity
|
protectedinherited |
|
protected |
Cache data for combined vector of the system.
|
protected |
|
protected |
Array of child body nodes.
|
protectedinherited |
Container of this Frame's child Entities.
|
protectedinherited |
Container of this Frame's child Frames.
|
protectedinherited |
JacobianNode children that descend from this JacobianNode.
|
protected |
Collision shape added signal.
|
protected |
Collision shape removed signal.
|
protected |
Same as mDependentDofs, but holds const pointers.
|
protected |
Constraint impulse: contact impulse, dynamic joint impulse.
|
protected |
Velocity change due to to external impulsive force exerted on bodies of the parent skeleton.
|
privateinherited |
|
protected |
A version of mDependentGenCoordIndices that holds DegreeOfFreedom pointers instead of indices.
|
protected |
A increasingly sorted list of dependent dof indices.
|
protectedinherited |
weak pointer to the destructor for this Node.
We use a shared_ptr "destructor" class instead of managing Nodes directly with shared_ptrs because this scheme allows BodyNodes to circumvent the shared_ptr management by setting the mNode member of the Destructor to a nullptr. That way the BodyNode can never be deleted by its Destructor.
|
protected |
Transmitted wrench from parent to the bodynode expressed in body-fixed frame.
|
protected |
Cache data for external force vector of the system.
|
protected |
Spatial gravity force.
|
protectedinherited |
Frame changed signal.
|
protected |
Cache data for gravity force vector of the system.
|
protected |
A unique ID of this node globally.
|
protectedinherited |
Inverse kinematics module which gets lazily created upon request.
|
protected |
Generalized impulsive body force w.r.t. body frame.
|
protectedinherited |
|
protected |
|
protected |
Index of this BodyNode in its Tree.
|
protected |
Cache data for inverse mass matrix of the system.
|
protected |
|
mutableprotectedinherited |
Dirty flag for body Jacobian.
|
mutableprotectedinherited |
Dirty flag for spatial time derivative of body Jacobian.
|
protected |
Whether the node is currently in collision with another node.
|
mutableprotected |
Is the partial acceleration vector dirty.
|
mutableprotectedinherited |
Dirty flag for the classic time derivative of the Jacobian.
|
mutableprotectedinherited |
Dirty flag for world Jacobian.
|
protectedinherited |
|
protected |
Cache data for mass matrix of the system.
|
protected |
|
protectedinherited |
Name changed signal.
|
mutableprotectedinherited |
Does this Entity need an Acceleration update.
|
mutableprotectedinherited |
Does this Entity need a Transform update.
|
mutableprotectedinherited |
Does this Entity need a Velocity update.
|
protected |
Array of child Entities that are not BodyNodes.
Organizing them separately allows some performance optimizations.
|
mutableprotectedinherited |
List of current Observers.
|
protected |
Parent body node.
|
protected |
Parent joint.
|
mutableprotected |
Partial spatial body acceleration due to parent joint's velocity.
Do not use directly! Use getPartialAcceleration() to access this quantity
|
mutableprotectedinherited |
Reference count for the number of BodyNodePtrs that are referring to this BodyNode.
|
mutableprotectedinherited |
|
staticprotected |
Counts the number of nodes globally.
|
private |
Hold onto a reference to this BodyNode's own Destructor to make sure that it never gets destroyed.
|
protectedinherited |
|
protected |
Structural change signal.
|
protectedinherited |
Transform changed signal.
|
protected |
Index of this BodyNode's tree.
|
mutableprotectedinherited |
Total velocity of this Frame, in the coordinates of this Frame.
Do not use directly! Use getSpatialVelocity() to access this quantity
|
protectedinherited |
Velocity changed signal.
|
protectedinherited |
|
mutableprotected |
Cached World Jacobian.
Do not use directly! Use getJacobian() to access this quantity
|
mutableprotected |
Classic time derivative of Body Jacobian.
Do not use directly! Use getJacobianClassicDeriv() to access this quantity
|
mutableprotectedinherited |
World transform of this Frame.
This object is mutable to enable auto-updating to happen in the const member getWorldTransform() function
Do not use directly! Use getWorldTransform() to access this quantity
|
inherited |
Slot register for acceleration updated signal.
common::SlotRegister<ColShapeAddedSignal> dart::dynamics::BodyNode::onColShapeAdded |
Slot register for collision shape added signal.
common::SlotRegister<ColShapeRemovedSignal> dart::dynamics::BodyNode::onColShapeRemoved |
Slot register for collision shape removed signal.
|
inherited |
Slot register for frame changed signal.
|
inherited |
Slot register for name changed signal.
|
mutable |
|
inherited |
Slot register for transform updated signal.
|
inherited |
Slot register for velocity updated signal.