suPreTrackContact.h Source File

00001 // suPreTrackContact.h
00002 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00003 // AUTHOR: Frank C. Anderson, Saryn R. Goldberg
00004 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00005 #ifndef __suPreTrackContact_h__
00006 #define __suPreTrackContact_h__
00007 
00008 
00009 //=============================================================================
00010 // INCLUDES
00011 //=============================================================================
00012 #include <suTrackControllerDLL.h>
00013 #include <rdUTWalking8.h>
00014 #include <rdFSQP.h>
00015 class suContactTargetVertical;
00016 class suContactTargetHorizontal;
00017 
00018 //=============================================================================
00019 //=============================================================================
00025 class SUTRACKCONTROLLER_API suPreTrackContact 
00026 {
00027 //=============================================================================
00028 // DATA
00029 //=============================================================================
00030 public:
00031 
00032 protected:
00034    double _wXS;
00036    double _wExpTight;
00038    double _wExpLoose;
00039 private:
00041    int _counter;
00043    rdUTWalking8 *_model;
00045    int _nvControls;
00047    int _nhControls;
00049    int _nvConstraints;
00051    int _nhConstraints;
00053    double _spos[rdUTWalking8_NS][3];
00055    double _svel[rdUTWalking8_NS][3];
00057    double _sfrc[rdUTWalking8_NS][3];
00059    double *_xVertLast;
00061    double *_xVert;
00063    rdStorage *_expKineticsStore;
00065    rdStorage *_qOrigStore;
00067    rdStorage *_ypOrigStore;
00069    double *_expKinetics;
00071    double *_qOrigRad;
00073    double *_qOrig;
00075    double *_q;
00077    double *_u;
00079    double *_yp;
00081    rdStorage *_newGenCrds;
00083    rdStorage *_newPsdStates;
00085    double *_springInContact;
00087    double _time;
00089    suContactTargetVertical *_vertContTarg;
00091    suContactTargetHorizontal *_horizContTarg;
00093    rdFSQP *_sqpVert;
00095    rdFSQP *_sqpHoriz;
00097    double **_dcdxVertPast;
00099    double *_dpdxVertPast;
00101    int _vStatus;
00102 
00103 // ONLY TEMPORARY FOR OPTIMAL WALKING SOLUTION
00105    rdStorage *_springStore;
00106 
00107 // ONLY USED FOR DRAWING MODEL
00108    double *_controls;
00109    double *_states;
00110 
00111 //=============================================================================
00112 // METHODS
00113 //=============================================================================
00114 public:
00115    suPreTrackContact(rdUTWalking8 *aModel,rdStorage *aGenCrds,
00116          rdStorage *psdStates, rdStorage *aKinetics, rdStorage *aSprings);
00117    virtual ~suPreTrackContact();
00118 private:
00119    void setNull();
00120 
00121 public:
00122    //--------------------------------------------------------------------------
00123    // GET AND SET
00124    //--------------------------------------------------------------------------
00125    rdStorage* getGenCrdStorage();
00126    rdStorage* getPsdStateStorage();
00127 
00128    //--------------------------------------------------------------------------
00129    // CONTACT CONSISTENCY METHODS
00130    //--------------------------------------------------------------------------
00131 public:
00132    void compute(double aT, int aTIndex);
00133    int suComputeVertPerformance(double *x,double *p);
00134    int suComputeVertPerformanceGradient(double *x,double *dpdx);
00135    int suComputeVertConstraint(double *x,int ic,double *c);
00136    int suComputeVertConstraintGradient(double *x,int ic,double *dcdx);
00137    int suComputeHorizPerformance(double *x,double *p);
00138    int suComputeHorizPerformanceGradient(double *x,double *dpdx);
00139    int suComputeHorizConstraint(double *x,int ic,double *c);
00140 protected:
00141 private:
00142    int initializeVertControls();
00143    int updateGenCoords(double *x);
00144    int appendGenCoords();
00145    int updatePsdStates(double *x);
00146    int updateModel();
00147    double getSprForce(int direction, int spring);
00148    double getSprPos(int direction, int spring);
00149    double getSumSprForce(int direction, int startSpring, int endSpring);
00150    double getSprPosRelToAnkle(int direction, int spring);
00151    int getNumSpringsInContact(int startSpring, int endSpring);
00152 //=============================================================================
00153 }; // END of class suPreTrackContact
00154 //=============================================================================
00155 //=============================================================================
00156 
00157 
00158 #endif // #ifndef __suPreTrackContact_h__