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 + 9; 00009 00010 f[0] = xd[3]; 00011 f[1] = xd[4]; 00012 f[2] = xd[5]; 00013 f[3] = (xa[1]+u[0]); 00014 f[4] = ((real_t)(-9.81)+xa[2]); 00015 f[5] = xa[0]; 00016 f[6] = ((sin(xd[2]))-xd[0]); 00017 f[7] = (((real_t)(0.)-(cos(xd[2])))-xd[1]); 00018 f[8] = (((((real_t)(0.7)*xd[5])+(xa[1]*(cos(xd[2]))))+(xa[2]*(sin(xd[2]))))+xa[0]); 00019 } 00020 00021 00022 00023 void rhs_jac( const real_t *x, real_t *f ){ 00024 const real_t* xd = x; 00025 const real_t* xa = x + 6; 00026 00027 f[0] = (real_t)(0.); 00028 f[1] = (real_t)(0.); 00029 f[2] = (real_t)(0.); 00030 f[3] = (real_t)(1.); 00031 f[4] = (real_t)(0.); 00032 f[5] = (real_t)(0.); 00033 f[6] = (real_t)(0.); 00034 f[7] = (real_t)(0.); 00035 f[8] = (real_t)(0.); 00036 f[9] = (real_t)(0.); 00037 f[10] = (real_t)(0.); 00038 f[11] = (real_t)(0.); 00039 f[12] = (real_t)(0.); 00040 f[13] = (real_t)(0.); 00041 f[14] = (real_t)(1.); 00042 f[15] = (real_t)(0.); 00043 f[16] = (real_t)(0.); 00044 f[17] = (real_t)(0.); 00045 f[18] = (real_t)(0.); 00046 f[19] = (real_t)(0.); 00047 f[20] = (real_t)(0.); 00048 f[21] = (real_t)(0.); 00049 f[22] = (real_t)(0.); 00050 f[23] = (real_t)(0.); 00051 f[24] = (real_t)(0.); 00052 f[25] = (real_t)(1.); 00053 f[26] = (real_t)(0.); 00054 f[27] = (real_t)(0.); 00055 f[28] = (real_t)(0.); 00056 f[29] = (real_t)(0.); 00057 f[30] = (real_t)(0.); 00058 f[31] = (real_t)(0.); 00059 f[32] = (real_t)(0.); 00060 f[33] = (real_t)(0.); 00061 f[34] = (real_t)(0.); 00062 f[35] = (real_t)(0.); 00063 f[36] = (real_t)(0.); 00064 f[37] = (real_t)(1.); 00065 f[38] = (real_t)(0.); 00066 f[39] = (real_t)(1.); 00067 f[40] = (real_t)(0.); 00068 f[41] = (real_t)(0.); 00069 f[42] = (real_t)(0.); 00070 f[43] = (real_t)(0.); 00071 f[44] = (real_t)(0.); 00072 f[45] = (real_t)(0.); 00073 f[46] = (real_t)(0.); 00074 f[47] = (real_t)(0.); 00075 f[48] = (real_t)(1.); 00076 f[49] = (real_t)(0.); 00077 f[50] = (real_t)(0.); 00078 f[51] = (real_t)(0.); 00079 f[52] = (real_t)(0.); 00080 f[53] = (real_t)(0.); 00081 f[54] = (real_t)(0.); 00082 f[55] = (real_t)(0.); 00083 f[56] = (real_t)(1.); 00084 f[57] = (real_t)(0.); 00085 f[58] = (real_t)(0.); 00086 f[59] = (real_t)(0.); 00087 f[60] = ((real_t)(0.)-(real_t)(1.)); 00088 f[61] = (real_t)(0.); 00089 f[62] = (cos(xd[2])); 00090 f[63] = (real_t)(0.); 00091 f[64] = (real_t)(0.); 00092 f[65] = (real_t)(0.); 00093 f[66] = (real_t)(0.); 00094 f[67] = (real_t)(0.); 00095 f[68] = (real_t)(0.); 00096 f[69] = (real_t)(0.); 00097 f[70] = (real_t)(0.); 00098 f[71] = ((real_t)(0.)-(real_t)(1.)); 00099 f[72] = ((real_t)(0.)-((real_t)(-1.)*(sin(xd[2])))); 00100 f[73] = (real_t)(0.); 00101 f[74] = (real_t)(0.); 00102 f[75] = (real_t)(0.); 00103 f[76] = (real_t)(0.); 00104 f[77] = (real_t)(0.); 00105 f[78] = (real_t)(0.); 00106 f[79] = (real_t)(0.); 00107 f[80] = (real_t)(0.); 00108 f[81] = (real_t)(0.); 00109 f[82] = ((xa[1]*((real_t)(-1.)*(sin(xd[2]))))+(xa[2]*(cos(xd[2])))); 00110 f[83] = (real_t)(0.); 00111 f[84] = (real_t)(0.); 00112 f[85] = (real_t)(0.7); 00113 f[86] = (real_t)(1.); 00114 f[87] = (cos(xd[2])); 00115 f[88] = (sin(xd[2])); 00116 f[89] = (real_t)(0.); 00117 } 00118 00119 00120 00121 void out( const real_t *x, real_t *f ){ 00122 const real_t* xd = x; 00123 const real_t* xa = x + 6; 00124 00125 f[0] = xa[1]; 00126 f[1] = xa[2]; 00127 } 00128 00129 00130 00131 void out_jac( const real_t *x, real_t *f ){ 00132 const real_t* xd = x; 00133 const real_t* xa = x + 6; 00134 00135 f[0] = (real_t)(0.); 00136 f[1] = (real_t)(0.); 00137 f[2] = (real_t)(0.); 00138 f[3] = (real_t)(0.); 00139 f[4] = (real_t)(0.); 00140 f[5] = (real_t)(0.); 00141 f[6] = (real_t)(0.); 00142 f[7] = (real_t)(1.); 00143 f[8] = (real_t)(0.); 00144 f[9] = (real_t)(0.); 00145 f[10] = (real_t)(0.); 00146 f[11] = (real_t)(0.); 00147 f[12] = (real_t)(0.); 00148 f[13] = (real_t)(0.); 00149 f[14] = (real_t)(0.); 00150 f[15] = (real_t)(0.); 00151 f[16] = (real_t)(0.); 00152 f[17] = (real_t)(0.); 00153 f[18] = (real_t)(1.); 00154 f[19] = (real_t)(0.); 00155 }