A sparse MPC solver for walking motion generation (old version).
|
00001 00009 /**************************************** 00010 * INCLUDES 00011 ****************************************/ 00012 00013 #include "problem_param.h" 00014 00015 #include <cmath> //cos,sin 00016 00017 /**************************************** 00018 * FUNCTIONS 00019 ****************************************/ 00020 00021 problem_parameters::problem_parameters ( 00022 const int N_, 00023 const double Alpha, 00024 const double Beta, 00025 const double Gamma, 00026 const double regularization) 00027 { 00028 N = N_; 00029 00030 i2Q[0] = 1/(2*(Beta/2)); 00031 i2Q[1] = 1/(2*(Alpha/2)); 00032 i2Q[2] = 1/(2*regularization); 00033 00034 i2P = 1/(2 * (Gamma/2)); 00035 00036 spar = new state_parameters[N]; 00037 } 00038 00039 00040 00041 problem_parameters::~problem_parameters() 00042 { 00043 if (spar != NULL) 00044 delete [] spar; 00045 } 00046 00047 00048 00055 void problem_parameters::set_state_parameters ( 00056 const double* T_, 00057 const double* h_, 00058 const double h_initial, 00059 const double* angle) 00060 { 00061 for (int i = 0; i < N; i++) 00062 { 00063 spar[i].cos = cos(angle[i]); 00064 spar[i].sin = sin(angle[i]); 00065 00066 if (i == 0) 00067 { 00068 spar[i].A6 = T_[i]*T_[i]/2 - (h_[0] - h_initial); 00069 } 00070 else 00071 { 00072 spar[i].A6 = T_[i]*T_[i]/2 - (h_[i] - h_[i-1]); 00073 } 00074 00075 spar[i].T = T_[i]; 00076 spar[i].h = h_[i]; 00077 00078 spar[i].B[2] = T_[i]; 00079 spar[i].B[1] = T_[i]*T_[i]/2; 00080 spar[i].B[0] = spar[i].B[1]*T_[i]/3 - h_[i]*T_[i]; 00081 00082 spar[i].A3 = T_[i]; 00083 } 00084 }