threedof_robot.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2009 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.
6  * Developed within the Optimization in Engineering Center (OPTEC) under
7  * supervision of Moritz Diehl. All rights reserved.
8  *
9  * ACADO Toolkit is free software; you can redistribute it and/or
10  * modify it under the terms of the GNU Lesser General Public
11  * License as published by the Free Software Foundation; either
12  * version 3 of the License, or (at your option) any later version.
13  *
14  * ACADO Toolkit is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17  * Lesser General Public License for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public
20  * License along with ACADO Toolkit; if not, write to the Free Software
21  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
22  *
23  */
24 
25 
26 
34 void threedof_robot( DifferentialEquation *f ){
35 
36  // DEFINE THE RIGHT-HAND-SIDE
37  // ---------------------------
38 
39  DifferentialState q1; //
40  DifferentialState q2; //
41  DifferentialState q3; //
42  DifferentialState dq1; //
43  DifferentialState dq2; //
44  DifferentialState dq3; //
45 
46  DifferentialStateDerivative d_q1; // the 1st derivative of q1 w.r.t t
47  DifferentialStateDerivative d_q2; // the 1st derivative of q2 w.r.t t
48  DifferentialStateDerivative d_q3; // the 1st derivative of q3 w.r.t t
49  DifferentialStateDerivative d_dq1; // the 1st derivative of dq1 w.r.t t
50  DifferentialStateDerivative d_dq2; // the 1st derivative of dq2 w.r.t t
51  DifferentialStateDerivative d_dq3; // the 1st derivative of dq3 w.r.t t
52 
53  Control tau1;
54  Control tau2;
55  Control tau3;
56 
57  Disturbance w1;
58  Disturbance w2;
59  Disturbance w3;
60  Disturbance w4;
61  Disturbance w5;
62  Disturbance w6;
63 
64  Parameter y0; //
65  Parameter y1; //
66  Parameter y2; //
67  Parameter y3; //
68  Parameter y4; //
69  Parameter y5; //
70  Parameter y6; //
71  Parameter y7; //
72  Parameter y8; //
73  Parameter y9; //
74  Parameter y10; //
75  Parameter y11; //
76  Parameter y12; //
77  Parameter y13; //
78  Parameter y14; //
79  Parameter y15; //
80  Parameter y16; //
81  Parameter y17; //
82  Parameter y18; //
83  Parameter y19; //
84  Parameter y20; //
85  Parameter y21; //
86  Parameter y22; //
87  Parameter y23; //
88  Parameter y24; //
89  Parameter y25; //
90 
91  const double D13 = 0.48; //
92  const double g1 = 9.81; // gravitational constant
93  const double r3 = 51.44118; //
94  const double l = 0.488; //
95  const double r = 0.1; //
96  const double dqm = 0.001; //
97 
98  IntermediateState t1;
99  IntermediateState t2;
100  IntermediateState t3;
101  IntermediateState t4;
102  IntermediateState t5;
103  IntermediateState t6;
104  IntermediateState t7;
105  IntermediateState t8;
106  IntermediateState t9;
107  IntermediateState t10;
108  IntermediateState t11;
109  IntermediateState t12;
110  IntermediateState t13;
111 
112  IntermediateState t20;
113  IntermediateState t21;
114  IntermediateState t22;
115  IntermediateState t23;
116 
117  IntermediateState t24;
118 
119  IntermediateState t26;
120  IntermediateState t27;
121 
122  IntermediateState t29;
123  IntermediateState t30;
124  IntermediateState t34;
125  IntermediateState t35;
126  IntermediateState t36;
127  IntermediateState t37;
128  IntermediateState t38;
129  IntermediateState t39;
130  IntermediateState t40;
131 
132  IntermediateState t41;
133 
134  IntermediateState t42;
135  IntermediateState t45;
136  IntermediateState t50;
137  IntermediateState t58;
138  IntermediateState t73;
139  IntermediateState t80;
140  IntermediateState t87;
141  IntermediateState t88;
142  IntermediateState t89;
143  IntermediateState t96;
144  IntermediateState t101;
145  IntermediateState t111;
146  IntermediateState t112;
147  IntermediateState t116;
148  IntermediateState t125;
149  IntermediateState t126;
150  IntermediateState t129;
151  IntermediateState t131;
152 
153  IntermediateState t136;
154  IntermediateState t141;
155 
156  IntermediateState t145;
157  IntermediateState t146;
158  IntermediateState t163;
159 
160  IntermediateState sdq1;
161  IntermediateState sdq2;
162  IntermediateState sdq3;
163 
164  IntermediateState M44;
165  IntermediateState M45;
166  IntermediateState M46;
167 
168  IntermediateState M55;
169  IntermediateState M56;
170 
171  IntermediateState M66;
172 
173  IntermediateState F4;
174  IntermediateState F5;
175  IntermediateState F6;
176 
177  t1 = cos(q3);
178  t2 = D13 * t1;
179  t3 = sin(q3);
180  t4 = D13 * t3;
181  t5 = q2 + q3;
182  t6 = sin(t5);
183  t7 = cos(t5);
184  t8 = t6 * t7;
185 
186  t11 = t6 * t6;
187  t12 = t7 * t7;
188  t13 = t11 - t12;
189 
190  t20 = cos(q2);
191  t21 = t20 * t20;
192  t22 = sin(q2);
193  t23 = t22 * t22;
194  t34 = t6 * y9;
195  t35 = t7 * y10;
196  t40 = t2 * y6;
197  t41 = t4 * y5;
198 
199  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;
200  M45 = t34 + t35 + t20 * y3 - t22 * y4;
201  M46 = t34 + t35;
202 
203  M55 = 2 * t41 + 2 * t40 + y13 + y14 + y15 / 1000;
204  M56 = t41 + t40 + y14 + r3 * y15 / 1000;
205 
206  M66 = y14 + r3 * r3 * y15 / 1000;
207 
208  sdq1 = (2/(1+exp(-3*dq1/dqm))-1);
209  sdq2 = (2/(1+exp(-3*dq2/dqm))-1);
210  sdq3 = (2/(1+exp(-3*dq3/dqm))-1);
211 
212  t9 = y2 * dq1;
213  t10 = y8 * dq1;
214 
215  t24 = dq3 * t6;
216 
217  t26 = y5 * dq1;
218  t27 = t26 * dq2;
219 
220  t29 = D13 * t22;
221  t30 = t29 * t6;
222  t36 = y6 * dq1;
223  t37 = t36 * dq2;
224  t38 = D13 * t20;
225  t39 = t38 * t6;
226  t42 = t29 * t10;
227  t45 = y7 * dq1;
228  t50 = t38 * t10;
229  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;
230  t73 = dq2 * dq2;
231  t80 = dq3 * dq3;
232  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;
233 
234  t88 = dq1 * dq1;
235  t89 = t88 * y8;
236 
237  t96 = sqrt(l * l + r * r - 2 * l * r * t20);
238  t101 = t88 * y2;
239  t111 = t88 * t6;
240  t112 = t10 * y7;
241  t116 = g1 * t20;
242  t125 = y5 * t96;
243  t126 = t125 * t88;
244  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;
245  t131 = dq3 * D13;
246 
247  t136 = t80 * D13;
248  t141 = g1 * t22;
249 
250  t145 = y6 * t96;
251  t146 = t145 * t88;
252  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;
253 
254  F4 = tau1 - (t58 + t87);
255  F5 = tau2 +(t129 + t163) / t96;
256  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);
257 
258 
259  *f << dot(q1) == d_q1 - dq1 + w1;
260  *f << dot(q2) == d_q2 - dq2 + w2;
261  *f << dot(q3) == d_q3 - dq3 + w3;
262  *f << dot(dq1) == M44 * d_dq1 + M45 * d_dq2 + M46 * d_dq3 - F4 + w4;
263  *f << dot(dq2) == M45 * d_dq1 + M55 * d_dq2 + M56 * d_dq3 - F5 + w5;
264  *f << dot(dq3) == M46 * d_dq1 + M56 * d_dq2 + M66 * d_dq3 - F6 + w6;
265 
266 }
267 
sin
USING_NAMESPACE_ACADO IntermediateState sin(const Expression &arg)
Definition: acado_syntax.cpp:43
sqrt
IntermediateState sqrt(const Expression &arg)
Definition: acado_syntax.cpp:50
threedof_robot
void threedof_robot(DifferentialEquation *f)
Definition: threedof_robot.cpp:34
cos
IntermediateState cos(const Expression &arg)
Definition: acado_syntax.cpp:44
dot
Expression dot(const Expression &arg)
Definition: acado_syntax.cpp:98
exp
IntermediateState exp(const Expression &arg)
Definition: acado_syntax.cpp:49
DifferentialEquation
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
Definition: differential_equation.hpp:55

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:35:12