|
A sparse MPC solver for walking motion generation (old version).
|
00001 00009 /**************************************** 00010 * INCLUDES 00011 ****************************************/ 00012 00013 #include "matrix_E.h" 00014 00015 00016 00017 /**************************************** 00018 * FUNCTIONS 00019 ****************************************/ 00020 00028 void matrix_E::form_Ex (const problem_parameters& ppar, const double *x, double *result) 00029 { 00030 int i; 00031 state_parameters stp; 00032 00033 00034 const double *control = &x[ppar.N*SMPC_NUM_STATE_VAR]; 00035 // a pointer to 6 current elements of result 00036 double *res = result; 00037 00038 for (i = 0; i < ppar.N; i++) 00039 { 00040 stp = ppar.spar[i]; 00041 00042 // a pointer to 6 current state variables 00043 const double *xc = &x[i*SMPC_NUM_STATE_VAR]; 00044 00045 00046 // result = -R * x + B * u 00047 res[0] = -(stp.cos * xc[0] - stp.sin * xc[3]) + stp.B[0] * control[0]; 00048 res[1] = -xc[1] + stp.B[1] * control[0]; 00049 res[2] = -xc[2] + stp.B[2] * control[0]; 00050 res[3] = -(stp.sin * xc[0] + stp.cos * xc[3]) + stp.B[0] * control[1]; 00051 res[4] = -xc[4] + stp.B[1] * control[1]; 00052 res[5] = -xc[5] + stp.B[2] * control[1]; 00053 00054 00055 if (i != 0) // no multiplication by A on the first iteration 00056 { 00057 double cosA = ppar.spar[i-1].cos; 00058 double sinA = ppar.spar[i-1].sin; 00059 00060 xc = &x[(i-1)*SMPC_NUM_STATE_VAR]; 00061 00062 00063 // result += A*R*x 00064 res[0] += cosA * xc[0] + stp.A3 * xc[1] + stp.A6 * xc[2] - sinA * xc[3]; 00065 res[1] += xc[1] + stp.A3 * xc[2]; 00066 res[2] += xc[2]; 00067 res[3] += cosA * xc[3] + stp.A3 * xc[4] + stp.A6 * xc[5] + sinA * xc[0]; 00068 res[4] += xc[4] + stp.A3 * xc[5]; 00069 res[5] += xc[5]; 00070 } 00071 00072 // next control variables 00073 control = &control[SMPC_NUM_CONTROL_VAR]; 00074 res = &res[SMPC_NUM_STATE_VAR]; 00075 } 00076 } 00077 00078 00086 void matrix_E::form_ETx (const problem_parameters& ppar, const double *x, double *result) 00087 { 00088 int i; 00089 state_parameters stp; 00090 00091 00092 00093 double *res = result; 00094 double *control_res = &result[ppar.N*SMPC_NUM_STATE_VAR]; 00095 00096 for (i = 0; i < ppar.N; i++) 00097 { 00098 stp = ppar.spar[i]; 00099 00100 00101 // a pointer to 6 current elements of result 00102 // a pointer to 6 current elements of nu 00103 const double *xc = &x[i*SMPC_NUM_STATE_VAR]; 00104 00105 00106 // result = -R' * nu 00107 res[0] = -(stp.cos * xc[0] + stp.sin * xc[3]); 00108 res[1] = -xc[1]; 00109 res[2] = -xc[2]; 00110 res[3] = -(- stp.sin * xc[0] + stp.cos * xc[3]); 00111 res[4] = -xc[4]; 00112 res[5] = -xc[5]; 00113 00114 00115 if (i != ppar.N-1) // no multiplication by A on the last iteration 00116 { 00117 double A3 = ppar.spar[i+1].A3; 00118 double A6 = ppar.spar[i+1].A6; 00119 00120 xc = &x[i*SMPC_NUM_STATE_VAR + SMPC_NUM_STATE_VAR]; 00121 00122 // result += R' * A' * x 00123 res[0] += stp.cos * xc[0] + stp.sin * xc[3]; 00124 res[1] += A3 * xc[0] + xc[1]; 00125 res[2] += A6 * xc[0] + A3 * xc[1] + xc[2]; 00126 00127 res[3] += - stp.sin * xc[0] + stp.cos * xc[3]; 00128 res[4] += A3 * xc[3] + xc[4]; 00129 res[5] += A6 * xc[3] + A3 * xc[4] + xc[5]; 00130 } 00131 00132 00133 xc = &x[i*SMPC_NUM_STATE_VAR]; 00134 00135 // result = B' * x 00136 control_res[0] = stp.B[0] * xc[0] + stp.B[1] * xc[1] + stp.B[2] * xc[2]; 00137 control_res[1] = stp.B[0] * xc[3] + stp.B[1] * xc[4] + stp.B[2] * xc[5]; 00138 00139 00140 res = &res[SMPC_NUM_STATE_VAR]; 00141 control_res = &control_res[SMPC_NUM_CONTROL_VAR]; 00142 } 00143 }
1.8.0