TAO: taoABNode.h Source File

00001 /* Copyright (c) 2005 Arachi, Inc. and Stanford University.
00002  *
00003  * Permission is hereby granted, free of charge, to any person obtaining
00004  * a copy of this software and associated documentation files (the
00005  * "Software"), to deal in the Software without restriction, including
00006  * without limitation the rights to use, copy, modify, merge, publish,
00007  * distribute, sublicense, and/or sell copies of the Software, and to
00008  * permit persons to whom the Software is furnished to do so, subject
00009  * to the following conditions:
00010  *
00011  * The above copyright notice and this permission notice shall be included
00012  * in all copies or substantial portions of the Software.
00013  *
00014  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00015  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00016  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00017  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
00018  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
00019  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
00020  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00021  */
00022 
00023 #ifndef _taoABNode_h
00024 #define _taoABNode_h
00025 
00026 #include "taoABJoint.h"
00027 #include "deMath.h"
00028 
00029 #ifndef DOXYGEN_SHOULD_SKIP_THIS
00030 
00037 class taoABNode
00038 {
00039 public:
00040         taoABNode() { _V.zero(); _A.zero(); _H.zero(), _Omega.zero(); }
00041 
00042         virtual ~taoABNode() {}
00043 
00044         virtual void setFlag(deInt v) = 0;
00045         virtual const deInt getFlag() const = 0;
00046 
00047         virtual deVector6* Pa() = 0;
00048 
00049         virtual const deMatrix6* I() const = 0;
00050 
00051         virtual deMatrix6* Omega() { return &_Omega; }
00052         virtual deVector6* H() { return &_H; }
00053         virtual deVector6* V() { return &_V; }
00054         virtual deVector6* A() { return &_A; }
00055 
00056         // Ia = I
00057         virtual void abInertiaInit(deMatrix6& Ia) = 0;
00058 
00059         // Ya = -Xc Yc
00060         //    = -[R , 0; dxR , R] [ y, 0]
00061         //    = [-Ry ; -dxRy]
00062         virtual void impulseInit(const deVector3& point, const deVector3& impulse) = 0;
00063 
00064 
00065         // Ii = Xc Ic Xtc
00066         //    = [ RMRt, -RMRt rx; rx RMRt, RIRt - rx RMRt rx]
00067         //    = [ M, -Mrx; rxM, RIRt - m rx rx]
00068         virtual void inertia(const deFloat* mass, const deVector3* centerOfMass, const deMatrix3* inertiaTensor) = 0;
00069 
00070         // Pi = Xc (Wc X Ic Vc) - Ii (Wi X Vi)  
00071         //    = Xc (Wc X [mi vc; Ic wc]) -Ii (Wi X Vi)
00072         virtual void biasForce(deVector6& P, const deVector6& V, const deVector3& WxV) = 0;
00073 
00074         // Dinv = inv(St Ia S)
00075         // SbarT = Ia S Dinv
00076         // hLi = hXi [ 1 - Si Sbari ]^T = hXi [1 - Sbari^T Si^T] = X - X SbarT St
00077         // Lt = [1 - S Sbar] Xt
00078         // Iah = Ih + sum [ Li Iai Lti ]
00079         virtual void _abInertia(deMatrix6& Iah, const deMatrix6& L, const deMatrix6& Ia, const deTransform& X) = 0;
00080 
00081         // Pah = Ph - Fexth + sum [ Li (Iai Ci + Pai) + X SbarTi taui ]
00082         virtual void _abBiasForce(deVector6& Pah, const deMatrix6& L, const deMatrix6& Ia, const deVector6& C, const deVector6& Pa) = 0;
00083 
00084         virtual void netForce(deVector6& F, const deVector6& A, const deVector6& P) = 0;
00085         // Pa -= Fext
00086         virtual void externalForce(deVector6& Pa, const deVector6& G, const deVector6& Fext) = 0;
00087 
00088         virtual void updateLocalX(const deFrame& homeFrame, const deFrame& localFrame) = 0;
00089 
00090         virtual void getFrameLocal(deFrame& localFrame) = 0;
00091 
00092         virtual void abImpulse(deVector6& Yah, deInt propagate) = 0;
00093         // 0Je = Je = iXe^T Si = 0Xi^(-T) Si
00094         // where 0Xi^(-T) = [ R rxR; 0 R ]
00095         // since 0Re = identity
00096         // and  iXe^T = [(inv 0Xi) 0Xe]^T = (0Xe)^T * (0Xi)^(-T) = 0Xi ^(-T)
00097         // since  0Xe = identity matrix    <--  {e} = {0}
00098         virtual void globalJacobian(const deFrame& globalFrame) = 0;
00099         // A += J * ddQ
00100         virtual void plusEq_Jg_ddQ(deVector6& Ag) = 0;
00101         // Tau += Jt * F
00102         virtual void add2Tau_JgT_F(const deVector6& Fg) = 0;
00103         // Gi = Xc Gc
00104         // Gc = [ mi Rtci gi ; 0]
00105         // gi = Rt gh
00106         virtual void gravityForce(deVector6& Fext, deVector3& g, const deVector3& gh) = 0;
00107 
00108         // Vi = hXi^T Vh + Si dqi;
00109         // xform = [R 0; dxR R]
00110         // xformT = [ Rt -Rtdx; 0 Rt ]
00111         // Ci = Wi X Vi - Xt (Wh X Vh) + Vi X Si dqi
00112         // V X = [v0 ; v1] X = [ v1x , v0x ; 0 , v1x]
00113         // WxV = [ 0 ; v1 ] x [ v0 ; v1 ]
00114         //     = [ v1x , 0 ; 0 , v1x ] [ v0 ; v1 ] = [v1 x v0 ;v1 x v1] = [ v1 x v0 ; 0 ]
00115         //     = [ wxv ; 0 ]
00116         // Xt * WxV = [ Rt -Rtdx; 0 Rt ] [ wxv ; 0 ] = [ Rt WxV ; 0 ]
00117         virtual void velocity(deVector6& V, deVector3& WxV, const deVector6& Vh, const deVector3& WhxVh) = 0;
00118         virtual void velocityOnly(deVector6& V, const deVector6& Vh) = 0;
00119         // Hi = hXi^T Hh + Ci
00120         virtual void biasAcceleration(deVector6& H, const deVector6& Hh) = 0;
00121         // Vi = hXi^T Vh + Si dqi;
00122         // E = 0.5 Vt I V
00123         virtual deFloat kineticEnergy(deVector6& V, const deVector6& Vh) = 0;
00124         // E = mgh
00125         virtual deFloat potentialEnergy(const deVector3& gh, const deFrame& globalFrame, const deFloat mass, const deVector3& centerOfMass) = 0;
00126 
00127 
00128         // ddQ = Dinv*(tau - St*Pa) - Sbar*(X Ah + Ci)
00129         // Ai = (hXi^T Ah + Ci) + Si ddqi;
00130         virtual void acceleration(deVector6& A, const deVector6& Ah) = 0;
00131         virtual void accelerationOnly(deVector6& A, const deVector6& Ah) = 0;
00132         // ddQ = Dinv*(tau - St*Pa) - Sbar*(X Ah)
00133         // Ai = (hXi^T Ah) + Si ddqi;
00134         // d dQ = Dinv*(- St*Pa) - Sbar*(X dVh)
00135         // dVi = (hXi^T dVh) + Si d dqi;
00136         virtual void velocityDelta(deVector6& dV, const deVector6& dVh, const deInt dist) = 0;
00137 
00138         virtual void force(deVector6& Fh, deInt propagate) = 0;
00139 
00140         // C = Ph = 0
00141         // Pah = - Fexth + sum [ Li (Pai) + X SbarTi taui ]
00142         virtual void abBiasForceConfig(deVector6& Pah, deInt propagate) = 0;
00143         virtual void abInertiaDepend(deMatrix6& Iah, deVector6& Pah, deMatrix6& Ia, deInt propagate) = 0;
00144         // O = S Dinv St + Lt Oh L
00145         virtual void osInertiaInv(deMatrix6& Oa, const deMatrix6& Oah) = 0;
00146 
00147         virtual void setABJoint(taoABJoint* joint, deInt i = 0) = 0;
00148         virtual taoABJoint* getABJoint(deInt i = 0) = 0;
00149 
00150         virtual void setNOJ(deInt n) = 0;
00151         virtual const deInt getNOJ() const = 0;
00152 
00153 private:
00154         deVector6 _V;
00155         deVector6 _A;
00156         deVector6 _H;           
00157         deMatrix6 _Omega;       // Op Sp Matrix inverse : diagonal terms
00158 };
00159 
00160 class taoABNodeRoot : public taoABNode
00161 {
00162 public:
00163         virtual const deInt getFlag() const { return 0; }
00164         virtual void setFlag(deInt v) {}
00165 
00166         virtual deVector6* Pa() { return NULL; }
00167         virtual const deMatrix6* I() const { return NULL; }
00168 
00169         virtual void abInertiaInit(deMatrix6& Ia) {}
00170         virtual void impulseInit(const deVector3& point, const deVector3& impulse) {}
00171 
00172         virtual void inertia(const deFloat* mass, const deVector3* centerOfMass, const deMatrix3* inertiaTensor) {}
00173         virtual void biasForce(deVector6& P, const deVector6& V, const deVector3& WxV) {}
00174         virtual void _abInertia(deMatrix6& Iah, const deMatrix6& L, const deMatrix6& Ia, const deTransform& X) {}
00175         virtual void _abBiasForce(deVector6& Pah, const deMatrix6& L, const deMatrix6& Ia, const deVector6& C, const deVector6& Pa) {}
00176         virtual void netForce(deVector6& F, const deVector6& A, const deVector6& P) {}
00177         virtual void externalForce(deVector6& Pa, const deVector6& G, const deVector6& Fext) {}
00178 
00179         virtual void updateLocalX(const deFrame& homeFrame, const deFrame& localFrame) {}
00180         virtual void getFrameLocal(deFrame& localFrame) {}
00181 
00182         virtual void abImpulse(deVector6& Yah, deInt propagate) {}
00183         virtual void globalJacobian(const deFrame& globalFrame) {}
00184         virtual void plusEq_Jg_ddQ(deVector6& Ag) {}
00185         virtual void add2Tau_JgT_F(const deVector6& Fg) {}
00186 
00187         virtual void gravityForce(deVector6& Fext, deVector3& g, const deVector3& gh) { g = gh; }
00188         virtual void velocity(deVector6& V, deVector3& WxV, const deVector6& Vh, const deVector3& WhxVh) { WxV.crossMultiply(V[1], V[0]); }
00189         virtual void velocityOnly(deVector6& V, const deVector6& Vh) {}
00190         virtual void biasAcceleration(deVector6& H, const deVector6& Hh) { H.zero(); }
00191         virtual deFloat kineticEnergy(deVector6& V, const deVector6& Vh) { return 0; }
00192         virtual deFloat potentialEnergy(const deVector3& gh, const deFrame& globalFrame, const deFloat mass, const deVector3& centerOfMass) { return 0; }
00193         virtual void acceleration(deVector6& A, const deVector6& Ah) {}
00194         virtual void accelerationOnly(deVector6& A, const deVector6& Ah) {}
00195         virtual void velocityDelta(deVector6& dV, const deVector6& dVh, const deInt dist) { dV.zero(); }
00196 
00197         virtual void force(deVector6& Fh, deInt propagate) {}
00198 
00199         virtual void abBiasForceConfig(deVector6& Pah, deInt propagate) {}
00200         virtual void abInertiaDepend(deMatrix6& Iah, deVector6& Pah, deMatrix6& Ia, deInt propagate) {}
00201         virtual void osInertiaInv(deMatrix6& Oa, const deMatrix6& Oah) {}
00202 
00203         virtual void setABJoint(taoABJoint* joint, deInt i = 0) {}
00204         virtual taoABJoint* getABJoint(deInt i = 0) { return NULL; }
00205 
00206         virtual void setNOJ(deInt n) {}
00207         virtual const deInt getNOJ() const { return 0; }
00208 };
00209 
00210 class taoABNodeNOJ : public taoABNode
00211 {
00212 public:
00213         taoABNodeNOJ()
00214         {
00215                 _flag = 0;
00216                 _Ic.zero();
00217                 _I.zero();
00218         }
00219         virtual const deInt getFlag() const { return _flag; }
00220         virtual void setFlag(deInt v) { _flag = v; }
00221 
00222         virtual const deMatrix3* Ic() const { return &_Ic; }
00223         virtual const deMatrix6* I() const { return &_I;}
00224 
00225         virtual deVector6* Pa() { return &(getABJoint(getNOJ() - 1)->Pa()); }
00226 
00227         virtual void abInertiaInit(deMatrix6& Ia);
00228         virtual void impulseInit(const deVector3& point, const deVector3& impulse);
00229 
00230         virtual void inertia(const deFloat* mass, const deVector3* centerOfMass, const deMatrix3* inertiaTensor);
00231         virtual void biasForce(deVector6& P, const deVector6& V, const deVector3& WxV);
00232         virtual void _abInertia(deMatrix6& Iah, const deMatrix6& L, const deMatrix6& Ia, const deTransform& X);
00233         virtual void _abBiasForce(deVector6& Pah, const deMatrix6& L, const deMatrix6& Ia, const deVector6& C, const deVector6& Pa);
00234         virtual void externalForce(deVector6& Pa, const deVector6& G, const deVector6& Fext);
00235         virtual deFloat kineticEnergy(deVector6& V, const deVector6& Vh);
00236         virtual deFloat potentialEnergy(const deVector3& gh, const deFrame& globalFrame, const deFloat mass, const deVector3& centerOfMass);
00237 
00238         virtual void setABJoint(taoABJoint* joint, deInt i = 0) = 0;
00239         virtual taoABJoint* getABJoint(deInt i = 0) = 0;
00240 
00241         virtual void setNOJ(deInt n) = 0;
00242         virtual const deInt getNOJ() const = 0;
00243 
00244 private:
00245         deInt _flag;
00246         deMatrix3 _Ic;
00247         deMatrix6 _I;
00248 };
00249 
00250 class taoABNodeNOJ1 : public taoABNodeNOJ
00251 {
00252 public:
00253         taoABNodeNOJ1() : _joint(NULL) {}
00254 
00255         virtual void updateLocalX(const deFrame& homeFrame, const deFrame& localFrame);
00256         virtual void getFrameLocal(deFrame& localFrame);
00257         virtual void abImpulse(deVector6& Yah, deInt propagate);
00258         virtual void globalJacobian(const deFrame& globalFrame);
00259         virtual void plusEq_Jg_ddQ(deVector6& Ag);
00260         virtual void add2Tau_JgT_F(const deVector6& Fg);
00261 
00262         virtual void gravityForce(deVector6& Fext, deVector3& g, const deVector3& gh);
00263         virtual void velocity(deVector6& V, deVector3& WxV, const deVector6& Vh, const deVector3& WhxVh);
00264         virtual void velocityOnly(deVector6& V, const deVector6& Vh);
00265 
00266         virtual void biasAcceleration(deVector6& H, const deVector6& Hh);
00267         virtual void acceleration(deVector6& A, const deVector6& Ah);
00268         virtual void accelerationOnly(deVector6& A, const deVector6& Ah);
00269         virtual void velocityDelta(deVector6& dV, const deVector6& dVh, const deInt dist);
00270 
00271         virtual void netForce(deVector6& F, const deVector6& A, const deVector6& P);
00272         virtual void force(deVector6& Fh, deInt propagate);
00273 
00274         virtual void abBiasForceConfig(deVector6& Pah, deInt propagate);
00275         virtual void abInertiaDepend(deMatrix6& Iah, deVector6& Pah, deMatrix6& Ia, deInt propagate);
00276         virtual void osInertiaInv(deMatrix6& Oa, const deMatrix6& Oah);
00277 
00278         virtual void setABJoint(taoABJoint* joint, deInt i = 0) { _joint = joint; }
00279         virtual taoABJoint* getABJoint(deInt i = 0) { return _joint; }
00280 
00281         virtual void setNOJ(deInt n) {}
00282         virtual const deInt getNOJ() const { return 1; }
00283 
00284         virtual ~taoABNodeNOJ1() { delete _joint; }
00285 
00286 private:
00287         taoABJoint* _joint;
00288 };
00289 
00290 class taoABNodeNOJn : public taoABNodeNOJ
00291 {
00292 public:
00293         taoABNodeNOJn() : _noj(0), _joint(NULL) {}
00294 
00295         virtual void updateLocalX(const deFrame& homeFrame, const deFrame& localFrame);
00296         virtual void getFrameLocal(deFrame& localFrame);
00297         virtual void abImpulse(deVector6& Yah, deInt propagate);
00298         virtual void globalJacobian(const deFrame& globalFrame);
00299         virtual void plusEq_Jg_ddQ(deVector6& Ag);
00300         virtual void add2Tau_JgT_F(const deVector6& Fg);
00301 
00302         virtual void gravityForce(deVector6& Fext, deVector3& g, const deVector3& gh);
00303         virtual void velocity(deVector6& V, deVector3& WxV, const deVector6& Vh, const deVector3& WhxVh);
00304         virtual void velocityOnly(deVector6& V, const deVector6& Vh);
00305 
00306         virtual void biasAcceleration(deVector6& H, const deVector6& Hh);
00307         virtual void acceleration(deVector6& A, const deVector6& Ah);
00308         virtual void accelerationOnly(deVector6& A, const deVector6& Ah);
00309         virtual void velocityDelta(deVector6& dV, const deVector6& dVh, const deInt dist);
00310 
00311         virtual void netForce(deVector6& F, const deVector6& A, const deVector6& P);
00312         virtual void force(deVector6& Fh, deInt propagate);
00313 
00314         virtual void abBiasForceConfig(deVector6& Pah, deInt propagate);
00315         virtual void abInertiaDepend(deMatrix6& Iah, deVector6& Pah, deMatrix6& Ia, deInt propagate);
00316         virtual void osInertiaInv(deMatrix6& Oa, const deMatrix6& Oah);
00317 
00318         virtual void setABJoint(taoABJoint* joint, deInt i = 0) { _joint[i] = joint; }
00319         virtual taoABJoint* getABJoint(deInt i = 0) { return _joint[i]; }
00320 
00321         virtual void setNOJ(deInt n) { _noj = n; _joint = new (taoABJoint*[_noj]); }
00322         virtual const deInt getNOJ() const { return _noj; }
00323 
00324         virtual ~taoABNodeNOJn() 
00325         { 
00326                 for (int i = 0; i < _noj; i++)
00327                         delete _joint[i];
00328                 delete[] _joint; 
00329         }
00330 
00331 private:
00332         deInt _noj;
00333         taoABJoint** _joint;
00334 };
00335 
00336 #endif // DOXYGEN_SHOULD_SKIP_THIS
00337 
00338 #endif // _taoABNode_h
00339