|
A sparse MPC solver for walking motion generation (old version).
|
00001 00008 #ifndef WMG_H 00009 #define WMG_H 00010 00011 00012 /**************************************** 00013 * INCLUDES 00014 ****************************************/ 00015 00016 #include <string> 00017 #include <vector> 00018 00019 #include "smpc_solver.h" 00020 00021 /**************************************** 00022 * DEFINES 00023 ****************************************/ 00024 00025 00026 00027 /**************************************** 00028 * TYPEDEFS 00029 ****************************************/ 00030 class footstep; 00031 00033 00034 00035 enum WMGret 00036 { 00037 WMG_OK, 00038 WMG_HALT 00039 }; 00040 00041 enum fs_type 00042 { 00043 FS_TYPE_AUTO, // let the library decide 00044 FS_TYPE_SS_L, // left single support 00045 FS_TYPE_SS_R, // right single support 00046 FS_TYPE_DS // double support 00047 }; 00048 00049 00050 00054 class smpc_parameters 00055 { 00056 public: 00064 smpc_parameters ( 00065 const unsigned int, 00066 const double, 00067 const double gravity_ = 9.81); 00068 00072 ~smpc_parameters(); 00073 00074 00075 00076 // variables 00077 double hCoM; 00078 00079 double gravity; 00080 00081 double *T; 00082 00083 double *h; 00084 double h0; 00085 00087 double *angle; 00088 00090 00091 double *fp_x; 00092 double *fp_y; 00094 00096 00097 double *lb; 00098 double *ub; 00100 00102 00103 double *zref_x; 00104 double *zref_y; 00106 00107 00109 smpc::state_orig init_state; 00110 00112 double *X; 00113 }; 00114 00115 00116 00120 class WMG 00121 { 00122 public: 00123 // methods 00124 WMG ( const unsigned int, 00125 const unsigned int, 00126 const double step_height_ = 0.0135, 00127 double bezier_weight_1_ = 1.0, 00128 double bezier_weight_2_ = 2.0, 00129 double bezier_inclination_1_ = 0.01, 00130 double bezier_inclination_2_ = 0.008); 00131 ~WMG(); 00132 00133 00134 void setFootstepDefaults( 00135 const unsigned int, 00136 const unsigned int, 00137 const double * constraints = NULL); 00138 void setFootstepDefaults( 00139 const unsigned int, 00140 const unsigned int, 00141 const unsigned int, 00142 const double * constraints = NULL); 00143 00144 void addFootstep( 00145 const double, 00146 const double, 00147 const double, 00148 fs_type type = FS_TYPE_AUTO); 00149 00150 00151 bool isSupportSwitchNeeded (); 00152 00153 WMGret formPreviewWindow (smpc_parameters &); 00154 00155 void FS2file(const std::string, const bool plot_ds = true); 00156 00157 void getFeetPositions (const unsigned int, double *, double *); 00158 00159 void getFootsteps( 00160 std::vector<double> &, 00161 std::vector<double> &, 00162 std::vector<double> &); 00163 00164 void changeNextSSPosition (const double *, const bool); 00165 00166 00167 // variables 00169 std::vector<footstep> FS; 00170 00171 00173 unsigned int N; 00174 00175 unsigned int *T_ms; 00176 unsigned int sampling_period; 00177 00178 00180 int current_step_number; 00181 00183 int first_preview_step; 00184 00186 double step_height; 00187 00188 double def_ss_constraint[4]; 00189 double def_auto_ds_constraint[4]; 00190 00192 00209 double bezier_weight_1; 00210 double bezier_weight_2; 00211 double bezier_inclination_1; 00212 double bezier_inclination_2; 00214 00215 private: 00216 void getDSFeetPositions (const int, double *, double *); 00217 void getSSFeetPositions (const int, const double, double *, double *); 00218 void getSSFeetPositionsBezier (const int, const double, double *, double *); 00219 int getNextSS (const int, const fs_type type = FS_TYPE_AUTO); 00220 int getPrevSS (const int, const fs_type type = FS_TYPE_AUTO); 00221 00222 00223 double addstep_constraint[4]; 00224 00225 unsigned int def_time_ms; 00226 unsigned int ds_time_ms; 00227 unsigned int ds_num; 00228 00229 unsigned int last_time_decrement; 00230 }; 00231 00232 00233 00237 class IPM 00238 { 00239 public: 00240 IPM (const double); 00241 ~IPM (); 00242 void calculateNextState (smpc::control&, smpc::state_orig&); 00243 00244 00246 00247 00248 double *A; 00249 double *B; 00251 00252 smpc::control control_vector; 00253 smpc::state_orig state_vector; 00254 }; 00255 00257 00258 #endif /*WMG_H*/
1.8.0