suPreTrack.h Source File

00001 // suPreTrack.h
00002 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00003 // AUTHOR: Frank C. Anderson, Saryn R. Goldberg
00004 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00005 #ifndef __suPreTrack_h__
00006 #define __suPreTrack_h__
00007 
00008 
00009 //=============================================================================
00010 // INCLUDES
00011 //=============================================================================
00012 #include <suTrackControllerDLL.h>
00013 #include <rdUTWalking8.h>
00014 #include <rdFSQP.h>
00015 class suContactTargetRight;
00016 
00017 //=============================================================================
00018 //=============================================================================
00023 class SUTRACKCONTROLLER_API suPreTrack 
00024 {
00025 //=============================================================================
00026 // DATA
00027 //=============================================================================
00028 public:
00029 
00030 protected:
00032    double _wLarge;
00034    double _wSmall;
00035 private:
00037    int _counter;
00039    rdUTWalking8 *_model;
00041    suContactTargetRight *_target;
00043    double _sposOrig[rdUTWalking8_NS][3];
00045    double _spos[rdUTWalking8_NS][3];
00047    double _svel[rdUTWalking8_NS][3];
00049    double _sfrc[rdUTWalking8_NS][3];
00051    double *_x;
00053    rdStorage *_sposOrigStore;
00055    rdStorage *_qOrigStore;
00057    double *_qOrigRad;
00059    double *_qOrig;
00061    double *_q;
00063    double *_u;
00065    rdStorage *_qNew;
00067    double *_springInContact;
00069    double _time;
00071    rdFSQP *_sqp;
00073    double **_dcdxLast;
00075    double *_dpdxLast;
00077    int _status;
00078 
00079    // ONLY TEMPORARY FOR OPTIMAL WALKING SOLUTION
00081    rdStorage *_springStore;
00082 
00083    // ONLY USED FOR DRAWING MODEL
00084    double *_controls;
00085    double *_states;
00086 
00087 //=============================================================================
00088 // METHODS
00089 //=============================================================================
00090 public:
00091    suPreTrack(rdUTWalking8 *aModel,rdStorage *aQ,rdStorage *aSpringPositions);
00092    virtual ~suPreTrack();
00093 private:
00094    void setNull();
00095 
00096 public:
00097    //--------------------------------------------------------------------------
00098    // GET AND SET
00099    //--------------------------------------------------------------------------
00100    rdStorage* getQNewStorage();
00101 
00102    //--------------------------------------------------------------------------
00103    // CONTACT CONSISTENCY METHODS
00104    //--------------------------------------------------------------------------
00105 public:
00106    void compute(double aT, int aTIndex);
00107    int suComputePerformance(double *x,double *p);
00108    int suComputePerformanceGradient(double *x,double *dpdx);
00109    int suComputeConstraint(double *x,int ic,double *c);
00110    int suComputeConstraintGradient(double *x,int ic,double *dcdx);
00111 private:
00112    int initializeControls();
00113    int updateQ(double *x);
00114    int updateModel();
00115    double getSprForce(int direction, int spring);
00116    double getSprPos(int direction, int spring);
00117    double getSumSprForce(int direction, int startSpring, int endSpring);
00118    double getSprPosRelToAnkle(int direction, int spring);
00119    int getNumSpringsInContact(int startSpring, int endSpring);
00120    int appendQNew();
00121 //=============================================================================
00122 }; // END of class suPreTrack
00123 //=============================================================================
00124 //=============================================================================
00125 
00126 
00127 #endif // #ifndef __suPreTrack_h__