00001 /* 00002 * This file is part of ACADO Toolkit. 00003 * 00004 * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization. 00005 * Copyright (C) 2008-2009 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven. 00006 * Developed within the Optimization in Engineering Center (OPTEC) under 00007 * supervision of Moritz Diehl. All rights reserved. 00008 * 00009 * ACADO Toolkit is free software; you can redistribute it and/or 00010 * modify it under the terms of the GNU Lesser General Public 00011 * License as published by the Free Software Foundation; either 00012 * version 3 of the License, or (at your option) any later version. 00013 * 00014 * ACADO Toolkit is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00017 * Lesser General Public License for more details. 00018 * 00019 * You should have received a copy of the GNU Lesser General Public 00020 * License along with ACADO Toolkit; if not, write to the Free Software 00021 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 00022 * 00023 */ 00024 00025 00026 00034 void threedof_robot( DifferentialEquation *f ){ 00035 00036 // DEFINE THE RIGHT-HAND-SIDE 00037 // --------------------------- 00038 00039 DifferentialState q1; // 00040 DifferentialState q2; // 00041 DifferentialState q3; // 00042 DifferentialState dq1; // 00043 DifferentialState dq2; // 00044 DifferentialState dq3; // 00045 00046 DifferentialStateDerivative d_q1; // the 1st derivative of q1 w.r.t t 00047 DifferentialStateDerivative d_q2; // the 1st derivative of q2 w.r.t t 00048 DifferentialStateDerivative d_q3; // the 1st derivative of q3 w.r.t t 00049 DifferentialStateDerivative d_dq1; // the 1st derivative of dq1 w.r.t t 00050 DifferentialStateDerivative d_dq2; // the 1st derivative of dq2 w.r.t t 00051 DifferentialStateDerivative d_dq3; // the 1st derivative of dq3 w.r.t t 00052 00053 Control tau1; 00054 Control tau2; 00055 Control tau3; 00056 00057 Disturbance w1; 00058 Disturbance w2; 00059 Disturbance w3; 00060 Disturbance w4; 00061 Disturbance w5; 00062 Disturbance w6; 00063 00064 Parameter y0; // 00065 Parameter y1; // 00066 Parameter y2; // 00067 Parameter y3; // 00068 Parameter y4; // 00069 Parameter y5; // 00070 Parameter y6; // 00071 Parameter y7; // 00072 Parameter y8; // 00073 Parameter y9; // 00074 Parameter y10; // 00075 Parameter y11; // 00076 Parameter y12; // 00077 Parameter y13; // 00078 Parameter y14; // 00079 Parameter y15; // 00080 Parameter y16; // 00081 Parameter y17; // 00082 Parameter y18; // 00083 Parameter y19; // 00084 Parameter y20; // 00085 Parameter y21; // 00086 Parameter y22; // 00087 Parameter y23; // 00088 Parameter y24; // 00089 Parameter y25; // 00090 00091 const double D13 = 0.48; // 00092 const double g1 = 9.81; // gravitational constant 00093 const double r3 = 51.44118; // 00094 const double l = 0.488; // 00095 const double r = 0.1; // 00096 const double dqm = 0.001; // 00097 00098 IntermediateState t1; 00099 IntermediateState t2; 00100 IntermediateState t3; 00101 IntermediateState t4; 00102 IntermediateState t5; 00103 IntermediateState t6; 00104 IntermediateState t7; 00105 IntermediateState t8; 00106 IntermediateState t9; 00107 IntermediateState t10; 00108 IntermediateState t11; 00109 IntermediateState t12; 00110 IntermediateState t13; 00111 00112 IntermediateState t20; 00113 IntermediateState t21; 00114 IntermediateState t22; 00115 IntermediateState t23; 00116 00117 IntermediateState t24; 00118 00119 IntermediateState t26; 00120 IntermediateState t27; 00121 00122 IntermediateState t29; 00123 IntermediateState t30; 00124 IntermediateState t34; 00125 IntermediateState t35; 00126 IntermediateState t36; 00127 IntermediateState t37; 00128 IntermediateState t38; 00129 IntermediateState t39; 00130 IntermediateState t40; 00131 00132 IntermediateState t41; 00133 00134 IntermediateState t42; 00135 IntermediateState t45; 00136 IntermediateState t50; 00137 IntermediateState t58; 00138 IntermediateState t73; 00139 IntermediateState t80; 00140 IntermediateState t87; 00141 IntermediateState t88; 00142 IntermediateState t89; 00143 IntermediateState t96; 00144 IntermediateState t101; 00145 IntermediateState t111; 00146 IntermediateState t112; 00147 IntermediateState t116; 00148 IntermediateState t125; 00149 IntermediateState t126; 00150 IntermediateState t129; 00151 IntermediateState t131; 00152 00153 IntermediateState t136; 00154 IntermediateState t141; 00155 00156 IntermediateState t145; 00157 IntermediateState t146; 00158 IntermediateState t163; 00159 00160 IntermediateState sdq1; 00161 IntermediateState sdq2; 00162 IntermediateState sdq3; 00163 00164 IntermediateState M44; 00165 IntermediateState M45; 00166 IntermediateState M46; 00167 00168 IntermediateState M55; 00169 IntermediateState M56; 00170 00171 IntermediateState M66; 00172 00173 IntermediateState F4; 00174 IntermediateState F5; 00175 IntermediateState F6; 00176 00177 t1 = cos(q3); 00178 t2 = D13 * t1; 00179 t3 = sin(q3); 00180 t4 = D13 * t3; 00181 t5 = q2 + q3; 00182 t6 = sin(t5); 00183 t7 = cos(t5); 00184 t8 = t6 * t7; 00185 00186 t11 = t6 * t6; 00187 t12 = t7 * t7; 00188 t13 = t11 - t12; 00189 00190 t20 = cos(q2); 00191 t21 = t20 * t20; 00192 t22 = sin(q2); 00193 t23 = t22 * t22; 00194 t34 = t6 * y9; 00195 t35 = t7 * y10; 00196 t40 = t2 * y6; 00197 t41 = t4 * y5; 00198 00199 M44 = (t2 - 2 * t4 * t8 + t2 * t13) * y6 + t13 * y7 + 2 * t8 * y8 + y0 + (t21 - t23) * y1 - 2 * t20 * t22 * y2 + (t4 - t4 * t13 - 2 * t2 * t8) * y5; 00200 M45 = t34 + t35 + t20 * y3 - t22 * y4; 00201 M46 = t34 + t35; 00202 00203 M55 = 2 * t41 + 2 * t40 + y13 + y14 + y15 / 1000; 00204 M56 = t41 + t40 + y14 + r3 * y15 / 1000; 00205 00206 M66 = y14 + r3 * r3 * y15 / 1000; 00207 00208 sdq1 = (2/(1+exp(-3*dq1/dqm))-1); 00209 sdq2 = (2/(1+exp(-3*dq2/dqm))-1); 00210 sdq3 = (2/(1+exp(-3*dq3/dqm))-1); 00211 00212 t9 = y2 * dq1; 00213 t10 = y8 * dq1; 00214 00215 t24 = dq3 * t6; 00216 00217 t26 = y5 * dq1; 00218 t27 = t26 * dq2; 00219 00220 t29 = D13 * t22; 00221 t30 = t29 * t6; 00222 t36 = y6 * dq1; 00223 t37 = t36 * dq2; 00224 t38 = D13 * t20; 00225 t39 = t38 * t6; 00226 t42 = t29 * t10; 00227 t45 = y7 * dq1; 00228 t50 = t38 * t10; 00229 t58 = -4 * t9 * dq2 * t21 + 4 * t10 * dq2 * t12 + 4 * t10 * dq3 * t12 + 2 * y9 * dq2 * dq3 * t10 - 2 * y10 * dq2 * t24 + 2 * t27 * t30 + 2 * t26 * dq3 * t30 + 2 * t37 * t39 + 2 * t37 * t42 + 4 * t45 * dq2 * t6 * t10 + y23 - 2 * t27 * t50 - 4 * y1 * dq1 * dq2 * t20 * t22; 00230 t73 = dq2 * dq2; 00231 t80 = dq3 * dq3; 00232 t87 = dq1 * y17 + sdq1 * y18 + 2 * t36 * dq3 * t42 + 4 * t45 * t24 * t10 + 2 * t9 * dq2 - 2 * t10 * dq2 - 2 * t10 * dq3 - y3 * t73 * t22 - y4 * t73 * t20 + y9 * t73 * t10 + y9 * t80 * t10 - y10 * t73 * t6 - y10 * t80 * t6; 00233 00234 t88 = dq1 * dq1; 00235 t89 = t88 * y8; 00236 00237 t96 = sqrt(l * l + r * r - 2 * l * r * t20); 00238 t101 = t88 * y2; 00239 t111 = t88 * t6; 00240 t112 = t10 * y7; 00241 t116 = g1 * t20; 00242 t125 = y5 * t96; 00243 t126 = t125 * t88; 00244 t129 = -t89 * t96 + 2 * t89 * t96 * t12 + t101 * t96 - 2 * t101 * t96 * t21 - 2 * t88 * t20 * t22 * y1 * t96 + 2 * t111 * t112 * t96 + t116 * y12 * t96 - dq2 * y19 * t96 - sdq2 * y20 * t96 - t22 * y11 * t96 - t126 * t50 + t126 * t30; 00245 t131 = dq3 * D13; 00246 00247 t136 = t80 * D13; 00248 t141 = g1 * t22; 00249 00250 t145 = y6 * t96; 00251 t146 = t145 * t88; 00252 t163 = -2 * t125 * dq2 * t131 * t9 - t125 * t136 * t9 - t125 * t116 * t9 + t125 * t141 * t21 + t146 * t39 + t146 * t42 + 2 * t145 * dq2 * t131 * t21 + t145 * t136 * t21 + t145 * t116 * t21 + t145 * t141 * t9 + 1000 * l * r * t22 * y16 - y24 * t96; 00253 00254 F4 = tau1 - (t58 + t87); 00255 F5 = tau2 +(t129 + t163) / t96; 00256 F6 = tau3 - (y6 * t73 * D13 * t21 - y5 * t73 * D13 * t9 - 2 * t89 * t12 + sdq3 * y22 - 2 * t111 * t112 - y5 * t88 * t30 + t89 - y6 * g1 * t6 + y5 * g1 * t10 - y6 * t88 * t42 + dq3 * y21 + y25); 00257 00258 00259 *f << dot(q1) == d_q1 - dq1 + w1; 00260 *f << dot(q2) == d_q2 - dq2 + w2; 00261 *f << dot(q3) == d_q3 - dq3 + w3; 00262 *f << dot(dq1) == M44 * d_dq1 + M45 * d_dq2 + M46 * d_dq3 - F4 + w4; 00263 *f << dot(dq2) == M45 * d_dq1 + M55 * d_dq2 + M56 * d_dq3 - F5 + w5; 00264 *f << dot(dq3) == M46 * d_dq1 + M56 * d_dq2 + M66 * d_dq3 - F6 + w6; 00265 00266 } 00267