00001 00002 #include "acado_common.h" 00003 00004 00005 void rhs( const real_t *x, real_t *f ){ 00006 const real_t *xd = x; 00007 const real_t *xa = x + 6; 00008 const real_t *u = x + 11; 00009 const real_t *dx = x + 12; 00010 00011 /* COMPUTE INTERMEDIATE STATES: */ 00012 /* ---------------------------- */ 00013 00014 /* COMPUTE OUTPUT: */ 00015 /* --------------- */ 00016 f[0] = (dx[0]-xd[3]); 00017 f[1] = (dx[1]-xd[4]); 00018 f[2] = (((real_t)(0.)-xd[5])+dx[2]); 00019 f[3] = (((real_t)(0.)-xa[0])+dx[3]); 00020 f[4] = (((real_t)(0.)-xa[1])+dx[4]); 00021 f[5] = (((real_t)(0.)-xa[2])+dx[5]); 00022 f[6] = ((((real_t)(2.)*xa[0])-xa[3])-u[0]); 00023 f[7] = ((((real_t)(19.62)+((real_t)(2.)*xa[1]))-xa[4])-u[0]); 00024 f[8] = ((((((real_t)(0.)-(xa[3]+u[0]))*xd[1])+((xa[4]+u[0])*xd[0]))+((real_t)(0.1)*xa[2]))-(real_t)(3.5)); 00025 f[9] = (((xd[5]*xd[4])+(xa[2]*xd[1]))+xa[0]); 00026 f[10] = (((((real_t)(0.)-xd[5])*xd[3])-(xa[2]*xd[0]))+xa[1]); 00027 } 00028 00029 00030 00031 void rhs_jac( const real_t *x, real_t *f ){ 00032 const real_t *xd = x; 00033 const real_t *xa = x + 6; 00034 const real_t *u = x + 11; 00035 const real_t *dx = x + 12; 00036 00037 /* COMPUTE INTERMEDIATE STATES: */ 00038 /* ---------------------------- */ 00039 00040 /* COMPUTE OUTPUT: */ 00041 /* --------------- */ 00042 f[0] = (real_t)(0.); 00043 f[1] = (real_t)(0.); 00044 f[2] = (real_t)(0.); 00045 f[3] = ((real_t)(0.)-(real_t)(1.)); 00046 f[4] = (real_t)(0.); 00047 f[5] = (real_t)(0.); 00048 f[6] = (real_t)(0.); 00049 f[7] = (real_t)(0.); 00050 f[8] = (real_t)(0.); 00051 f[9] = (real_t)(0.); 00052 f[10] = (real_t)(0.); 00053 f[11] = (real_t)(0.); 00054 f[12] = (real_t)(1.); 00055 f[13] = (real_t)(0.); 00056 f[14] = (real_t)(0.); 00057 f[15] = (real_t)(0.); 00058 f[16] = (real_t)(0.); 00059 f[17] = (real_t)(0.); 00060 f[18] = (real_t)(0.); 00061 f[19] = (real_t)(0.); 00062 f[20] = (real_t)(0.); 00063 f[21] = (real_t)(0.); 00064 f[22] = ((real_t)(0.)-(real_t)(1.)); 00065 f[23] = (real_t)(0.); 00066 f[24] = (real_t)(0.); 00067 f[25] = (real_t)(0.); 00068 f[26] = (real_t)(0.); 00069 f[27] = (real_t)(0.); 00070 f[28] = (real_t)(0.); 00071 f[29] = (real_t)(0.); 00072 f[30] = (real_t)(0.); 00073 f[31] = (real_t)(1.); 00074 f[32] = (real_t)(0.); 00075 f[33] = (real_t)(0.); 00076 f[34] = (real_t)(0.); 00077 f[35] = (real_t)(0.); 00078 f[36] = (real_t)(0.); 00079 f[37] = (real_t)(0.); 00080 f[38] = (real_t)(0.); 00081 f[39] = (real_t)(0.); 00082 f[40] = (real_t)(0.); 00083 f[41] = ((real_t)(0.)-(real_t)(1.)); 00084 f[42] = (real_t)(0.); 00085 f[43] = (real_t)(0.); 00086 f[44] = (real_t)(0.); 00087 f[45] = (real_t)(0.); 00088 f[46] = (real_t)(0.); 00089 f[47] = (real_t)(0.); 00090 f[48] = (real_t)(0.); 00091 f[49] = (real_t)(0.); 00092 f[50] = (real_t)(1.); 00093 f[51] = (real_t)(0.); 00094 f[52] = (real_t)(0.); 00095 f[53] = (real_t)(0.); 00096 f[54] = (real_t)(0.); 00097 f[55] = (real_t)(0.); 00098 f[56] = (real_t)(0.); 00099 f[57] = (real_t)(0.); 00100 f[58] = (real_t)(0.); 00101 f[59] = (real_t)(0.); 00102 f[60] = ((real_t)(0.)-(real_t)(1.)); 00103 f[61] = (real_t)(0.); 00104 f[62] = (real_t)(0.); 00105 f[63] = (real_t)(0.); 00106 f[64] = (real_t)(0.); 00107 f[65] = (real_t)(0.); 00108 f[66] = (real_t)(0.); 00109 f[67] = (real_t)(0.); 00110 f[68] = (real_t)(0.); 00111 f[69] = (real_t)(1.); 00112 f[70] = (real_t)(0.); 00113 f[71] = (real_t)(0.); 00114 f[72] = (real_t)(0.); 00115 f[73] = (real_t)(0.); 00116 f[74] = (real_t)(0.); 00117 f[75] = (real_t)(0.); 00118 f[76] = (real_t)(0.); 00119 f[77] = (real_t)(0.); 00120 f[78] = (real_t)(0.); 00121 f[79] = ((real_t)(0.)-(real_t)(1.)); 00122 f[80] = (real_t)(0.); 00123 f[81] = (real_t)(0.); 00124 f[82] = (real_t)(0.); 00125 f[83] = (real_t)(0.); 00126 f[84] = (real_t)(0.); 00127 f[85] = (real_t)(0.); 00128 f[86] = (real_t)(0.); 00129 f[87] = (real_t)(0.); 00130 f[88] = (real_t)(1.); 00131 f[89] = (real_t)(0.); 00132 f[90] = (real_t)(0.); 00133 f[91] = (real_t)(0.); 00134 f[92] = (real_t)(0.); 00135 f[93] = (real_t)(0.); 00136 f[94] = (real_t)(0.); 00137 f[95] = (real_t)(0.); 00138 f[96] = (real_t)(0.); 00139 f[97] = (real_t)(0.); 00140 f[98] = ((real_t)(0.)-(real_t)(1.)); 00141 f[99] = (real_t)(0.); 00142 f[100] = (real_t)(0.); 00143 f[101] = (real_t)(0.); 00144 f[102] = (real_t)(0.); 00145 f[103] = (real_t)(0.); 00146 f[104] = (real_t)(0.); 00147 f[105] = (real_t)(0.); 00148 f[106] = (real_t)(0.); 00149 f[107] = (real_t)(1.); 00150 f[108] = (real_t)(0.); 00151 f[109] = (real_t)(0.); 00152 f[110] = (real_t)(0.); 00153 f[111] = (real_t)(0.); 00154 f[112] = (real_t)(0.); 00155 f[113] = (real_t)(0.); 00156 f[114] = (real_t)(2.); 00157 f[115] = (real_t)(0.); 00158 f[116] = (real_t)(0.); 00159 f[117] = ((real_t)(0.)-(real_t)(1.)); 00160 f[118] = (real_t)(0.); 00161 f[119] = ((real_t)(0.)-(real_t)(1.)); 00162 f[120] = (real_t)(0.); 00163 f[121] = (real_t)(0.); 00164 f[122] = (real_t)(0.); 00165 f[123] = (real_t)(0.); 00166 f[124] = (real_t)(0.); 00167 f[125] = (real_t)(0.); 00168 f[126] = (real_t)(0.); 00169 f[127] = (real_t)(0.); 00170 f[128] = (real_t)(0.); 00171 f[129] = (real_t)(0.); 00172 f[130] = (real_t)(0.); 00173 f[131] = (real_t)(0.); 00174 f[132] = (real_t)(0.); 00175 f[133] = (real_t)(2.); 00176 f[134] = (real_t)(0.); 00177 f[135] = (real_t)(0.); 00178 f[136] = ((real_t)(0.)-(real_t)(1.)); 00179 f[137] = ((real_t)(0.)-(real_t)(1.)); 00180 f[138] = (real_t)(0.); 00181 f[139] = (real_t)(0.); 00182 f[140] = (real_t)(0.); 00183 f[141] = (real_t)(0.); 00184 f[142] = (real_t)(0.); 00185 f[143] = (real_t)(0.); 00186 f[144] = (xa[4]+u[0]); 00187 f[145] = ((real_t)(0.)-(xa[3]+u[0])); 00188 f[146] = (real_t)(0.); 00189 f[147] = (real_t)(0.); 00190 f[148] = (real_t)(0.); 00191 f[149] = (real_t)(0.); 00192 f[150] = (real_t)(0.); 00193 f[151] = (real_t)(0.); 00194 f[152] = (real_t)(0.1); 00195 f[153] = (xd[1]*((real_t)(0.)-(real_t)(1.))); 00196 f[154] = xd[0]; 00197 f[155] = ((xd[1]*((real_t)(0.)-(real_t)(1.)))+xd[0]); 00198 f[156] = (real_t)(0.); 00199 f[157] = (real_t)(0.); 00200 f[158] = (real_t)(0.); 00201 f[159] = (real_t)(0.); 00202 f[160] = (real_t)(0.); 00203 f[161] = (real_t)(0.); 00204 f[162] = (real_t)(0.); 00205 f[163] = xa[2]; 00206 f[164] = (real_t)(0.); 00207 f[165] = (real_t)(0.); 00208 f[166] = xd[5]; 00209 f[167] = xd[4]; 00210 f[168] = (real_t)(1.); 00211 f[169] = (real_t)(0.); 00212 f[170] = xd[1]; 00213 f[171] = (real_t)(0.); 00214 f[172] = (real_t)(0.); 00215 f[173] = (real_t)(0.); 00216 f[174] = (real_t)(0.); 00217 f[175] = (real_t)(0.); 00218 f[176] = (real_t)(0.); 00219 f[177] = (real_t)(0.); 00220 f[178] = (real_t)(0.); 00221 f[179] = (real_t)(0.); 00222 f[180] = ((real_t)(0.)-xa[2]); 00223 f[181] = (real_t)(0.); 00224 f[182] = (real_t)(0.); 00225 f[183] = ((real_t)(0.)-xd[5]); 00226 f[184] = (real_t)(0.); 00227 f[185] = (xd[3]*((real_t)(0.)-(real_t)(1.))); 00228 f[186] = (real_t)(0.); 00229 f[187] = (real_t)(1.); 00230 f[188] = ((real_t)(0.)-xd[0]); 00231 f[189] = (real_t)(0.); 00232 f[190] = (real_t)(0.); 00233 f[191] = (real_t)(0.); 00234 f[192] = (real_t)(0.); 00235 f[193] = (real_t)(0.); 00236 f[194] = (real_t)(0.); 00237 f[195] = (real_t)(0.); 00238 f[196] = (real_t)(0.); 00239 f[197] = (real_t)(0.); 00240 } 00241 00242 00243 00244 void out( const real_t *x, real_t *f ){ 00245 const real_t *xd = x; 00246 const real_t *xa = x + 6; 00247 const real_t *dx = x + 12; 00248 00249 /* COMPUTE INTERMEDIATE STATES: */ 00250 /* ---------------------------- */ 00251 00252 /* COMPUTE OUTPUT: */ 00253 /* --------------- */ 00254 f[0] = xa[3]; 00255 f[1] = xa[4]; 00256 } 00257 00258 00259 00260 void out_jac( const real_t *x, real_t *f ){ 00261 const real_t *xd = x; 00262 const real_t *xa = x + 6; 00263 const real_t *dx = x + 12; 00264 00265 /* COMPUTE INTERMEDIATE STATES: */ 00266 /* ---------------------------- */ 00267 00268 /* COMPUTE OUTPUT: */ 00269 /* --------------- */ 00270 f[0] = (real_t)(0.); 00271 f[1] = (real_t)(0.); 00272 f[2] = (real_t)(0.); 00273 f[3] = (real_t)(0.); 00274 f[4] = (real_t)(0.); 00275 f[5] = (real_t)(0.); 00276 f[6] = (real_t)(0.); 00277 f[7] = (real_t)(0.); 00278 f[8] = (real_t)(0.); 00279 f[9] = (real_t)(1.); 00280 f[10] = (real_t)(0.); 00281 f[11] = (real_t)(0.); 00282 f[12] = (real_t)(0.); 00283 f[13] = (real_t)(0.); 00284 f[14] = (real_t)(0.); 00285 f[15] = (real_t)(0.); 00286 f[16] = (real_t)(0.); 00287 f[17] = (real_t)(0.); 00288 f[18] = (real_t)(0.); 00289 f[19] = (real_t)(0.); 00290 f[20] = (real_t)(0.); 00291 f[21] = (real_t)(0.); 00292 f[22] = (real_t)(0.); 00293 f[23] = (real_t)(0.); 00294 f[24] = (real_t)(0.); 00295 f[25] = (real_t)(0.); 00296 f[26] = (real_t)(0.); 00297 f[27] = (real_t)(0.); 00298 f[28] = (real_t)(1.); 00299 f[29] = (real_t)(0.); 00300 f[30] = (real_t)(0.); 00301 f[31] = (real_t)(0.); 00302 f[32] = (real_t)(0.); 00303 f[33] = (real_t)(0.); 00304 f[34] = (real_t)(0.); 00305 f[35] = (real_t)(0.); 00306 }