00001 // rdDynamicFilter.h 00002 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00003 // Copyright 2000 Realistic Dynamics, Inc. 00004 // All rights reserved. 00005 // 00006 // Please do not read, copy, distribute, or use without permission. 00007 // Contact: Frank C. Anderson, fca@RealisticDynamics.com 00008 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00009 #include "rdDynamicFilterDLL.h" 00010 #include <math.h> 00011 #include <rdFSQP.h> 00012 #include <rdModel.h> 00013 #include <rdController.h> 00014 #include "rdKinematics.h" 00015 //============================================================================= 00016 #ifndef __rdDynamicFilter_h__ 00017 #define __rdDynamicFilter_h__ 00018 00019 00020 //============================================================================= 00021 //============================================================================= 00036 class RDDYNAMICFILTER_API rdDynamicFilter 00037 : public rdController, public rdOptimizationTarget 00038 { 00039 //============================================================================= 00040 // DATA 00041 //============================================================================= 00042 private: 00043 rdKinematics *_kin; 00044 rdFSQP *_fsqp; 00045 int _maxIter; 00046 int _ny,_nq,_nu; 00047 double _dt,_t,*_y,*_ytmp,*_q,*_u,*_qerr,*_uerr; 00048 double *_dqdt,*_dudt_d,*_dudt_c,*_dudt; 00049 double *_kq,*_ku; 00050 double _kAcc,_kX; 00051 double _lookAhead; 00052 bool *_track; 00053 00054 //============================================================================= 00055 // METHODS 00056 //============================================================================= 00057 public: 00058 //-------------------------------------------------------------------------- 00059 // CONSTRUCTION 00060 //-------------------------------------------------------------------------- 00061 virtual ~rdDynamicFilter(); 00062 rdDynamicFilter(rdModel *aModel,rdKinematics *aKin); 00063 00064 //-------------------------------------------------------------------------- 00065 // SET AND GET 00066 //-------------------------------------------------------------------------- 00067 rdFSQP* getFSQP(); 00068 void setQGains(double aQGain); 00069 void setQGains(int aI,double aQGain); 00070 double* getQGains(); 00071 void setUGains(double aUGain); 00072 void setUGains(int aI,double aUGain); 00073 double* getUGains(); 00074 void setTrackingPerformanceWeight(double aW); 00075 double getTrackingPerformanceWeight(); 00076 void setControlsPerformanceWeight(double aW); 00077 double getControlsPerformanceWeight(); 00078 void setLookAhead(double aTimeDelta); 00079 double getLookAhead(); 00080 void setTrack(bool aTrueFalse); 00081 void setTrack(int i,bool aTrueFalse); 00082 bool* getTrack(); 00083 double getDT(); 00084 double* getCorrectedAccelerations(); 00085 00086 //========================================================================== 00087 // REQUIRED OPTIMIZATION TARGET METHODS 00088 //========================================================================== 00089 //-------------------------------------------------------------------------- 00090 // PERFORMANCE AND CONSTRAINTS 00091 //-------------------------------------------------------------------------- 00092 int compute(double *x,double *p,double *c); 00093 int computeGradients(double *dx,double *x,double *dpdx,double *dcdx); 00094 //-------------------------------------------------------------------------- 00095 // PERFORMANCE 00096 //-------------------------------------------------------------------------- 00097 int computePerformance(double *x,double *p); 00098 int computePerformanceGradient(double *x,double *dpdx); 00099 //-------------------------------------------------------------------------- 00100 // CONSTRAINT 00101 //-------------------------------------------------------------------------- 00102 int computeConstraint(double *x,int i,double *c); 00103 int computeConstraintGradient(double *x,int i,double *dcdx); 00104 00105 //-------------------------------------------------------------------------- 00106 // TRAJECTORY 00107 //-------------------------------------------------------------------------- 00108 void 00109 computeTrajectoryErrors(double t,double *q,double *u, 00110 double *qerr,double *uerr,double *dudt); 00111 void 00112 computeCorrectedAccelerations(double *qerr,double *uerr,double *dudt_d); 00113 void 00114 computeAccelerations(double *x); 00115 00116 //-------------------------------------------------------------------------- 00117 // OPTIMAL CONTROLS 00118 //-------------------------------------------------------------------------- 00119 virtual void 00120 computeControls(double dt,double t,const double *y,double *x); 00121 00122 //-------------------------------------------------------------------------- 00123 // UTILITY 00124 //-------------------------------------------------------------------------- 00125 void printTrajectoryErrors(); 00126 void printAccelerations(); 00127 00128 //============================================================================= 00129 }; // END class rdDynamicFilter 00130 //============================================================================= 00131 //============================================================================= 00132 00133 #endif // __rdDynamicFilter_h__ 00134 00135 00136 00137 //============================================================================= 00138 // INCLUDES OF THE VARIOUS COMPONENTS OF THE DYNAMIC FILTER 00139 //============================================================================= 00140 #ifndef RDDYNAMICFILTER_EXPORTS 00141 #include <rdKinematics.h> 00142 #endif
1.3