SCL  1.0
Standard Control Library : Control, dynamics, physics, and simulation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Macros Groups Pages
Public Member Functions | Private Member Functions | Private Attributes | List of all members
taoNode Class Reference

node class for articulated body dynamicsThis implements taoDNode for articulated body dynamics. More...

#include <taoNode.h>

Inheritance diagram for taoNode:
Inheritance graph
[legend]
Collaboration diagram for taoNode:
Collaboration graph
[legend]

Public Member Functions

 taoNode (taoDNode *parent, deFrame const *home)
 
virtual void sync (deFrame *local)
 
virtual taoJointgetJointList ()
 
virtual taoJoint const * getJointList () const
 
virtual deVector6velocity ()
 
virtual deVector6acceleration ()
 
virtual void getFrameGraphics (deFrame *Tog)
 global frame for graphics display More...
 
virtual deFrameframeHome ()
 
virtual deFrameframeLocal ()
 
virtual deFrameframeGlobal ()
 global frame for the dynamics computation More...
 
virtual deFrame const * frameGlobal () const
 
virtual deFloat * mass ()
 
virtual deVector3center ()
 
virtual deVector3 const * center () const
 
virtual deMatrix3inertia ()
 
virtual deVector6force ()
 
virtual void zeroForce ()
 
virtual void addForce (const deVector6 *f)
 
virtual void updateFrame ()
 
virtual void integrate (deFloat dt)
 
virtual void setDParent (taoDNode *n)
 
virtual taoDNodegetDParent ()
 
virtual taoDNode const * getDParent () const
 
virtual void setDChild (taoDNode *n)
 
virtual taoDNodegetDChild ()
 
virtual taoDNode const * getDChild () const
 
virtual void setDSibling (taoDNode *n)
 
virtual taoDNodegetDSibling ()
 
virtual taoDNode const * getDSibling () const
 
virtual void addJoint (taoJoint *joint)
 
virtual void addABNode ()
 
virtual void unlink ()
 
virtual void link (taoDNode *parent, deFrame const *home)
 
virtual void deleteJointABNode ()
 
virtual void linearVelocity (deVector3 *Vie, const deVector3 *Pie)
 
virtual void linearAcceleration (deVector3 *Aie, const deVector3 *Pie)
 
virtual void impulse (const deVector3 *Pie, const deVector3 *Yie)
 computes new velocity given impulse More...
 
virtual void impulseDist (const deVector3 *Pie, const deVector3 *Yie)
 computes new position given pseudo impulse More...
 
virtual void force (const deVector3 *Pie, const deVector3 *Fie)
 computes new force given force More...
 
virtual deInt isRoot ()
 indicates if this node is root More...
 
virtual deInt isParentRoot ()
 indicates if parent of this node is root More...
 
virtual taoArticulatedBodyLinkgetABNode ()
 gets _abNode More...
 
virtual void setABNode (taoArticulatedBodyLink *node)
 sets _abNode More...
 
virtual deInt getPropagate ()
 indicates if dynamics of node should be included for control More...
 
virtual void setPropagate (deInt p)
 
virtual void setID (const deInt id)
 
virtual deInt getID () const
 
virtual void setIsFixed (const deInt f)
 
virtual deInt getIsFixed ()
 
virtual deFloat getCOR ()
 
virtual deFloat getCOF_grip ()
 
virtual deFloat getCOF_viscous ()
 
virtual deFloat getCOF_static ()
 
virtual deFloat getCOF_dynamic ()
 
virtual void setCOR (deFloat c)
 coefficient of restitution More...
 

Private Member Functions

void _Initialize ()
 

Private Attributes

deFrame _frameHome
 
deFrame _frameLocal
 
deFrame _frameGlobal
 
deVector3 _center
 
deFloat _mass
 
deMatrix3 _inertia
 
deVector6 _Fext
 
taoDNode_parent
 
taoNode_child
 
taoNode_sibling
 
taoJoint_jointList
 

Coefficients of friction

where v is velocity, a is acceleration, y is impulse, and f is force at the contact point.

Note
: (*)_n and (*)_t indicate the normal and tangential components of (*) respectively.
virtual void setCOF_grip (deFloat c)
 cofg : coefficient of grip friction when v != 0 and y != 0 : f_g = - sign(v_t) * cofg * y_n More...
 
virtual void setCOF_viscous (deFloat c)
 cofv : coefficient of viscous friction when v != 0 : f_v = - v_t * cofv More...
 
virtual void setCOF_static (deFloat c)
 cofs : coefficient of static friction when v == 0 and a != 0 : f_s = - sign(a_t) * cofs * f_n More...
 
virtual void setCOF_dynamic (deFloat c)
 cofd : coefficient of dynamic friction when v != 0 : f_d = - sign(v_t) * cofd * f_n More...
 
virtual deFloat effectiveMass (const deVector3 *Pie, const deVector3 *Ui)
 
virtual deInt impact1 (const deVector3 *Pie, const deVector3 *Ui, const deFloat cor2, const deFloat cofg2)
 computes new velocity and force given pos, vel, acc More...
 
virtual deInt impact2 (taoDNode *ni, const deVector3 *Pie, const deVector3 *Ui, taoDNode *nj, const deVector3 *Pje, const deVector3 *Uj)
 This method computes impulse/force and changes velocity/friction between two colliding nodes, ni and nj. More...
 
virtual deInt penetration1 (const deVector3 *Pie, const deVector3 *Ui, const deVector3 *pdist, const deFloat dt)
 This method computes impulse and changes position/orientation using impulseDist(). More...
 
std::string name_
 

Detailed Description

node class for articulated body dynamics

This implements taoDNode for articulated body dynamics.

See Also
taoCNode, taoDNode, taoDynamics

Member Function Documentation

deVector6 * taoNode::acceleration ( )
virtual
Remarks
expressed in local frame

Implements taoDNode.

virtual void taoNode::addForce ( const deVector6 f)
inlinevirtual
Remarks
expressed in local frame

Implements taoDNode.

virtual deVector3* taoNode::center ( )
inlinevirtual
Returns
center of gravity in local frame

Implements taoDNode.

virtual deVector6* taoNode::force ( )
inlinevirtual
Remarks
expressed in local frame

Implements taoDNode.

void taoNode::force ( const deVector3 Pie,
const deVector3 Fie 
)
virtual

computes new force given force

This method replaces the accumulated force with the given force.

Remarks
replaces the internal force with a new force
Precondition
A, F
Postcondition
F
  • Pie - contact point expressed in local frame.
  • Fie - force at the contact point expressed in local frame.
    Note
    this method replaces the internal force in the node.

Reimplemented from taoDNode.

virtual deFrame* taoNode::frameGlobal ( )
inlinevirtual

global frame for the dynamics computation

Note
for taoNodeRB, this is the center of mass frame.
for taoNode, this is the same frame getFrameGraphics()

Implements taoDNode.

virtual deFrame* taoNode::frameHome ( )
inlinevirtual
Returns
home frame

Implements taoDNode.

virtual deFrame* taoNode::frameLocal ( )
inlinevirtual
Returns
local frame

Implements taoDNode.

virtual taoArticulatedBodyLink* taoDNode::getABNode ( )
inlinevirtualinherited

gets _abNode

virtual taoDNode* taoNode::getDChild ( )
inlinevirtual
Returns
first child node

Implements taoDNode.

virtual taoDNode* taoNode::getDParent ( )
inlinevirtual
Returns
parent node

Implements taoDNode.

virtual taoDNode* taoNode::getDSibling ( )
inlinevirtual
Returns
next sibling node

Implements taoDNode.

virtual void taoNode::getFrameGraphics ( deFrame Fg)
inlinevirtual

global frame for graphics display

Note
for taoNodeRB, this is the graphics origin frame without the offset.
for taoNodeRB, setFrameGraphics() should be used to set this frame.
for taoNode, this is the same frame as frameGlobal()
Return values
Fgis filled with the frame info for graphics sync.

Implements taoDNode.

virtual deInt taoDNode::getPropagate ( )
inlinevirtualinherited

indicates if dynamics of node should be included for control

Return values
1to propagate dynamics of node for control
0not to propagate dynamics of node for control
deInt taoDNode::impact1 ( const deVector3 Pie,
const deVector3 Ui,
const deFloat  cor2,
const deFloat  cofg2 
)
virtualinherited

computes new velocity and force given pos, vel, acc

Remarks
do not use ddQ after this call, replaces the internal force to the new force
Precondition
T, Q, V, dQ, A
Postcondition
V, dQ, F
  • Pie - contact point expressed in local frame.
  • Ui - unit contact direction vector expressed in local frame (outward normal vector)
  • cor2 - coefficient of restitution of the other collision node.
  • cof2 - coefficient of grip friction of the other collision node.
    Note
    this method is assuming a node is colliding with a non-moving node.
deInt taoDNode::impact2 ( taoDNode ni,
const deVector3 Pie,
const deVector3 Ui,
taoDNode nj,
const deVector3 Pje,
const deVector3 Uj 
)
virtualinherited

This method computes impulse/force and changes velocity/friction between two colliding nodes, ni and nj.

  • Pie - contact point expressed in frame i : Pie = ^iP_e - ^iP_i
  • Pje - contact point expressed in frame j : Pje = ^jP_e - ^jP_j
  • Ui - unit contact direction vector expressed in frame i : Ui = (^iP_j - ^iP_i)/|^iP_j - ^iP_i|
  • Uj - Ui expressed in frame j
    Note
    this method should be used for two moving nodes.
    Edited 2013-08-21 : Samir Menon smeno.nosp@m.n@st.nosp@m.anfor.nosp@m.d.ed.nosp@m.u
void taoNode::impulse ( const deVector3 Pie,
const deVector3 Yie 
)
virtual

computes new velocity given impulse

This method changes velocity instantaneously by applying the given impulse.

Remarks
do not use ddQ after this call
Precondition
T, Q, V, dQ, Y
Postcondition
V, dQ
  • Pie - contact point expressed in local frame.
  • Yie - impulse at the contact point expressed in local frame.

Reimplemented from taoDNode.

void taoNode::impulseDist ( const deVector3 Pie,
const deVector3 Yie 
)
virtual

computes new position given pseudo impulse

This method changes position and orientation instantaneously by applying the given pseudo impulse.

  • Pie - contact point expressed in local frame.
  • Yie - pseudo impulse at the contact point expressed in local frame.

Reimplemented from taoDNode.

virtual deMatrix3* taoNode::inertia ( )
inlinevirtual
Returns
inertia tensor at the origin of local frame

Implements taoDNode.

virtual deInt taoDNode::isParentRoot ( )
inlinevirtualinherited

indicates if parent of this node is root

Return values
1parent this node is root
0parent this node in not root
virtual deInt taoDNode::isRoot ( )
inlinevirtualinherited

indicates if this node is root

Return values
1this node is root
0this node in not root
void taoNode::linearAcceleration ( deVector3 Aie,
const deVector3 Pie 
)
virtual
Returns
Aie
Remarks
expressed in local frame

Reimplemented from taoDNode.

void taoNode::linearVelocity ( deVector3 Vie,
const deVector3 Pie 
)
virtual
Returns
Vie
Remarks
expressed in local frame

Reimplemented from taoDNode.

virtual deFloat* taoNode::mass ( )
inlinevirtual
Returns
global frame
mass

Implements taoDNode.

deInt taoDNode::penetration1 ( const deVector3 Pie,
const deVector3 Ui,
const deVector3 pdist,
const deFloat  dt 
)
virtualinherited

This method computes impulse and changes position/orientation using impulseDist().

  • Pie - contact point expressed in local frame.
  • Ui - unit contact direction vector expressed in local frame (outward normal vector).
  • pdist - penetration distance vector : pdist = P_o - P_i
  • dt - integration time step.
    Note
    this method is assuming a node is colliding with a non-moving node.
virtual void taoDNode::setABNode ( taoArticulatedBodyLink node)
inlinevirtualinherited

sets _abNode

virtual void taoDNode::setCOF_dynamic ( deFloat  c)
inlinevirtualinherited

cofd : coefficient of dynamic friction when v != 0 : f_d = - sign(v_t) * cofd * f_n

virtual void taoDNode::setCOF_grip ( deFloat  c)
inlinevirtualinherited

cofg : coefficient of grip friction when v != 0 and y != 0 : f_g = - sign(v_t) * cofg * y_n

virtual void taoDNode::setCOF_static ( deFloat  c)
inlinevirtualinherited

cofs : coefficient of static friction when v == 0 and a != 0 : f_s = - sign(a_t) * cofs * f_n

virtual void taoDNode::setCOF_viscous ( deFloat  c)
inlinevirtualinherited

cofv : coefficient of viscous friction when v != 0 : f_v = - v_t * cofv

virtual void taoDNode::setCOR ( deFloat  c)
inlinevirtualinherited

coefficient of restitution

void taoNode::updateFrame ( )
virtual

This function updates two frames. parent_T_node (_frameLocal), and origin_T_node (_frameGlobal)

Implements taoDNode.

deVector6 * taoNode::velocity ( )
virtual
Remarks
expressed in local frame

Edited 2013-08-21 : Samir Menon smeno.nosp@m.n@st.nosp@m.anfor.nosp@m.d.ed.nosp@m.u

Implements taoDNode.


The documentation for this class was generated from the following files: