00001
00002 #include "kuka361FwDynnf.hpp"
00003
00004 #define FSIGN(a) ( a<-1E-15 ? -1.0 : ( a>1E-15 ? 1.0 : 0.0))
00005 #define SSSIGN(a,b) (2.0/(1.0+exp(-3*a/b))-1.0)
00006
00007
00008
00009 using namespace std;
00010
00011 #define Y_LS {29.0130, -10.9085, -0.2629, 2.6403, -0.0421, 0.0509, 3.0221, 2.4426, 0.0531, -0.2577, -0.2627, 346.7819, -0.2651, 35.4666, 5.6321, 0.9935, 3.2192, 10.7823, 34.2604, 16.9902, 39.1594, 5.4501, 16.1392, -12.7243, -6.1094, 1.9590}
00012 #define Y_LS2 {3.4102, 3.2555, 2.2401, 29.5178, 35.0201, 28.1370, 16.3857, 15.5666, 14.5318, -0.5734, -2.1976, 5.8708}
00013
00014 kuka361FwDynnf::kuka361FwDynnf():_ddq(6){
00015 dqm = 0.001;
00016 D13 = 0.48;
00017 g1 = 9.81;
00018 double y_ls[26] = Y_LS;
00019 for(int i=0; i<26;i++){
00020 _y_ls[i]=y_ls[i];
00021 }
00022 r3 = 51.44118;
00023 l = 0.488;
00024 r = 0.1;
00025 double y_ls2[12] = Y_LS2;
00026 for(int i=0; i<12;i++){
00027 _y_ls2[i]=y_ls2[i];
00028 }
00029 }
00030
00031 vector<double> kuka361FwDynnf::fwdyn361(vector<double> &tau, vector<double> &q, vector<double> &dq){
00032
00033 t1 = sin(q[2]);
00034 t2 = D13 * t1;
00035 t3 = r3 * r3;
00036 t5 = _y_ls[5] * t3 * _y_ls[15];
00037 t6 = t2 * t5;
00038 t8 = cos(q[2]);
00039 t9 = D13 * t8;
00040 t11 = _y_ls[6] * t3 * _y_ls[15];
00041 t12 = t9 * t11;
00042 t15 = _y_ls[13] * t3 * _y_ls[15];
00043 t16 = (_y_ls[13] * _y_ls[14]);
00044 t19 = _y_ls[14] * t3 * _y_ls[15];
00045 t20 = _y_ls[15] * _y_ls[14];
00046 t21 = D13 * D13;
00047 t22 = t1 * t1;
00048 t23 = t21 * t22;
00049 t24 = _y_ls[5] * _y_ls[5];
00050 t25 = t23 * t24;
00051 t26 = 0.1000e4 * t25;
00052 t27 = t21 * t1;
00053 t28 = _y_ls[5] * t8;
00054 t29 = t28 * _y_ls[6];
00055 t30 = t27 * t29;
00056 t31 = 0.2000e4 * t30;
00057 t33 = _y_ls[5] * r3 * _y_ls[15];
00058 t34 = t2 * t33;
00059 t36 = t8 * t8;
00060 t37 = t21 * t36;
00061 t38 = _y_ls[6] * _y_ls[6];
00062 t39 = t37 * t38;
00063 t40 = 0.1000e4 * t39;
00064 t42 = _y_ls[6] * r3 * _y_ls[15];
00065 t43 = t9 * t42;
00066 t46 = _y_ls[14] * r3 * _y_ls[15];
00067 t48 = 0.2e1 * t6 + 0.2e1 * t12 + t15 + (1000 * t16) + t19 + t20 - t26 - t31 - 0.2e1 * t34 - t40 - 0.2e1 * t43 - 0.2e1 * t46;
00068 t49 = t21 * D13;
00069 t50 = t24 * t49;
00070 t51 = t50 * t22;
00071 t52 = q[1] + q[2];
00072 t53 = sin(t52);
00073 t54 = t53 * t53;
00074 t55 = t54 * t8;
00075 t59 = _y_ls[6] * D13;
00076 t60 = t1 * t53;
00077 t61 = t59 * t60;
00078 t62 = cos(t52);
00079 t63 = t62 * _y_ls[14];
00080 t64 = t3 * _y_ls[15];
00081 t65 = t63 * t64;
00082 t68 = cos(q[1]);
00083 t69 = sin(q[1]);
00084 t70 = t68 * t69;
00085 t71 = t70 * _y_ls[2];
00086 t74 = t53 * _y_ls[9];
00087 t75 = t74 * t69;
00088 t77 = _y_ls[4] * t3 * _y_ls[15];
00089 t80 = t74 * t68;
00090 t81 = _y_ls[3] * D13;
00091 t82 = t1 * _y_ls[5];
00092 t83 = t81 * t82;
00093 t86 = _y_ls[0] * _y_ls[13];
00094 t89 = _y_ls[9] * _y_ls[9];
00095 t90 = t54 * t89;
00096 t92 = t62 * t62;
00097 t93 = _y_ls[10] * _y_ls[10];
00098 t94 = t92 * t93;
00099 t96 = t68 * t68;
00100 t97 = _y_ls[3] * _y_ls[3];
00101 t98 = t96 * t97;
00102 t101 = t69 * t69;
00103 t102 = _y_ls[4] * _y_ls[4];
00104 t103 = t101 * t102;
00105 t106 = t36 * t8;
00106 t108 = t38 * _y_ls[6];
00107 t111 = t22 * t1;
00108 t113 = t24 * _y_ls[5];
00109 t116 = 0.1000e4 * t51 * t55 * _y_ls[6] - 0.2e1 * t61 * t65 + 0.4e1 * t71 * t46 + 0.2e1 * t75 * t77 + 0.2000e4 * t80 * t83 + 0.1000e4 * t86 * _y_ls[14] - t90 * _y_ls[15] - t94 * _y_ls[15] - 0.1000e4 * t98 * _y_ls[14] - 0.1000e4 * t103 * _y_ls[14] - 0.1000e4 * t49 * t106 * t108 - 0.1000e4 * t49 * t111 * t113;
00110 t121 = _y_ls[7] * t54;
00111 t123 = t8 * _y_ls[6];
00112 t124 = t82 * t123;
00113 t127 = t62 * _y_ls[10];
00114 t128 = t127 * _y_ls[13];
00115 t131 = _y_ls[5] * t49;
00116 t133 = t53 * t62;
00117 t137 = t127 * _y_ls[15];
00118 t140 = t127 * t69;
00119 t143 = t59 * t8;
00120 t144 = t54 * _y_ls[13];
00121 t145 = t144 * _y_ls[14];
00122 t148 = _y_ls[4] * D13;
00123 t149 = t148 * t82;
00124 t152 = t148 * t123;
00125 t155 = _y_ls[5] * t21;
00126 t157 = t155 * t36 * t53;
00127 t158 = t62 * _y_ls[6];
00128 t159 = r3 * _y_ls[15];
00129 t164 = t92 * r3 * _y_ls[15];
00130 t167 = -0.1000e4 * t94 * _y_ls[13] - 0.1000e4 * t90 * _y_ls[13] - 0.2000e4 * t121 * t21 * t124 - 0.2000e4 * t74 * t128 + 0.2000e4 * t131 * t106 * t133 * t38 - 0.2e1 * t74 * t137 + 0.2e1 * t140 * t77 + 0.1000e4 * t143 * t145 - 0.2000e4 * t140 * t149 - 0.2000e4 * t140 * t152 + 0.4e1 * t157 * t158 * t159 + 0.2e1 * t39 * t164;
00131 t169 = t121 * D13;
00132 t170 = t123 * t159;
00133 t173 = _y_ls[5] * D13;
00134 t174 = t8 * t53;
00135 t175 = t173 * t174;
00136 t178 = t173 * t8;
00137 t179 = t133 * t20;
00138 t182 = t133 * _y_ls[8];
00139 t186 = t133 * _y_ls[8] * D13;
00140 t187 = t82 * t64;
00141 t190 = t68 * _y_ls[3];
00142 t195 = _y_ls[3] * t3 * _y_ls[15];
00143 t200 = t81 * t123;
00144 t204 = _y_ls[3] * r3 * _y_ls[15];
00145 t207 = _y_ls[7] * t92;
00146 t208 = t207 * D13;
00147 t213 = -0.2e1 * t169 * t170 - 0.2e1 * t175 * t65 - 0.2e1 * t178 * t179 + 0.2e1 * t182 * t15 + 0.4e1 * t186 * t187 + 0.2e1 * t190 * t69 * t77 - 0.2e1 * t80 * t195 - 0.4e1 * t182 * t46 + 0.2000e4 * t80 * t200 + 0.2e1 * t80 * t204 - 0.2e1 * t208 * t187 + 0.1000e4 * t207 * t25;
00148 t217 = _y_ls[6] * _y_ls[13];
00149 t221 = t123 * t64;
00150 t225 = t54 * r3 * _y_ls[15];
00151 t237 = t82 * t159;
00152 t240 = t27 * _y_ls[5];
00153 t247 = t113 * t49;
00154 t252 = 0.2000e4 * t207 * t21 * t124 + 0.1000e4 * t9 * t217 * _y_ls[14] + 0.4e1 * t186 * t221 + 0.2e1 * t25 * t225 + 0.2e1 * t23 * t24 * t3 * _y_ls[15] - 0.2e1 * t121 * t46 + 0.2e1 * t169 * t221 + t121 * t15 + 0.2e1 * t208 * t237 + 0.4e1 * t240 * t221 - 0.2000e4 * t75 * t149 - 0.2000e4 * t75 * t152 + 0.2000e4 * t247 * t8 * t133 * t22;
00155 t258 = t92 * t3 * _y_ls[15];
00156 t268 = _y_ls[4] * r3 * _y_ls[15];
00157 t273 = _y_ls[1] * t101;
00158 t276 = _y_ls[0] * _y_ls[14];
00159 t279 = t74 * t62;
00160 t284 = t69 * _y_ls[4];
00161 t285 = t284 * _y_ls[14];
00162 t292 = -0.2e1 * t25 * t164 + 0.2e1 * t25 * t258 - 0.1000e4 * t121 * t39 - 0.2000e4 * t182 * t25 - 0.2e1 * t208 * t221 - 0.2e1 * t75 * t268 + 0.2e1 * t94 * t159 - 0.1000e4 * t273 * t16 - 0.2e1 * t276 * t159 - 0.2e1 * t279 * _y_ls[10] * t3 * _y_ls[15] + 0.2000e4 * t190 * t285 + 0.4e1 * t279 * _y_ls[10] * r3 * _y_ls[15];
00163 t293 = t127 * t68;
00164 t296 = t2 * _y_ls[5];
00165 t298 = _y_ls[5] * _y_ls[13];
00166 t310 = _y_ls[6] * _y_ls[15];
00167 t314 = _y_ls[5] * _y_ls[15];
00168 t321 = t63 * t159;
00169 t324 = t131 * t1;
00170 t329 = 0.2000e4 * t293 * t83 + t296 * t15 + 0.1000e4 * t2 * t298 * _y_ls[14] + 0.2000e4 * t293 * t200 - 0.2e1 * t23 * t24 * r3 * _y_ls[15] + t276 * t64 + t86 * t64 + t9 * t310 * _y_ls[14] + t296 * t19 + t2 * t314 * _y_ls[14] - 0.2e1 * t37 * t38 * r3 * _y_ls[15] + 0.4e1 * t175 * t321 + 0.1000e4 * t324 * t92 * t36 * t38;
00171 t331 = t92 * _y_ls[14];
00172 t332 = t331 * t159;
00173 t335 = t24 * t21;
00174 t336 = t335 * t174;
00175 t337 = t62 * t1;
00176 t350 = t92 * _y_ls[13];
00177 t351 = t350 * t64;
00178 t353 = t54 * _y_ls[14];
00179 t354 = t353 * t159;
00180 t357 = _y_ls[0] * D13;
00181 t358 = t357 * t1;
00182 t361 = t357 * t8;
00183 t366 = _y_ls[8] * _y_ls[13];
00184 t370 = -0.2e1 * t296 * t332 - 0.4e1 * t336 * t337 * t64 + t121 * t19 - 0.1000e4 * t324 * t54 * t36 * t38 + 0.2e1 * t37 * t38 * t3 * _y_ls[15] + t296 * t351 + 0.2e1 * t296 * t354 - 0.2e1 * t358 * t33 - 0.2e1 * t361 * t42 + t143 * t15 + t143 * t19 + 0.2000e4 * t133 * t366 * _y_ls[14];
00185 t371 = _y_ls[1] * t96;
00186 t372 = t371 * D13;
00187 t378 = t59 * t1;
00188 t379 = t133 * t16;
00189 t384 = _y_ls[8] * _y_ls[15];
00190 t391 = t70 * _y_ls[2] * D13;
00191 t398 = _y_ls[0] * t21;
00192 t404 = 0.2e1 * t372 * t187 + 0.2e1 * t372 * t221 + t371 * t15 - 0.2000e4 * t378 * t379 + 0.2e1 * t182 * t19 + 0.2e1 * t133 * t384 * _y_ls[14] + 0.2000e4 * t71 * t39 + 0.4e1 * t391 * t170 + t371 * t19 - t207 * t15 + 0.2e1 * t293 * t204 - 0.2000e4 * t398 * t1 * t29 - 0.1000e4 * t371 * t25;
00193 t414 = t62 * _y_ls[13] * t64;
00194 t433 = t273 * D13;
00195 t436 = 0.4000e4 * t50 * t36 * t133 * t1 * _y_ls[6] - 0.2e1 * t175 * t414 - t273 * t15 + 0.2e1 * t361 * t11 - 0.4e1 * t186 * t237 - 0.2e1 * t293 * t195 - t90 * t64 - t273 * t19 - 0.2e1 * t143 * t46 + 0.1000e4 * t273 * t25 + 0.2000e4 * t273 * t21 * t124 + 0.2e1 * t433 * t237;
00196 t460 = -0.2e1 * t372 * t237 - 0.2000e4 * t371 * t21 * t124 - 0.4e1 * t186 * t170 + 0.2e1 * t358 * t5 + 0.4e1 * t336 * t337 * t159 - 0.1000e4 * t371 * t39 - 0.2e1 * t372 * t170 - 0.2e1 * t433 * t187 - t98 * t64 + 0.1000e4 * t371 * t16 + t371 * t20 - t273 * t20;
00197 t465 = t108 * t49;
00198 t476 = t22 * t24;
00199 t479 = t36 * t38;
00200 t489 = -0.1000e4 * t247 * t111 * t92 - 0.1000e4 * t465 * t106 * t54 + 0.1000e4 * t465 * t106 * t92 + 0.1000e4 * t247 * t111 * t54 - t207 * t20 - 0.1000e4 * t398 * t476 - 0.1000e4 * t398 * t479 + 0.1000e4 * t121 * t16 + t121 * t20 - t103 * t64 - t94 * t64 - 0.1000e4 * t207 * t16;
00201 t492 = _y_ls[0] * _y_ls[15];
00202 t511 = _y_ls[6] * t21;
00203 t513 = t511 * t22 * t53;
00204 t514 = t62 * _y_ls[5];
00205 t520 = 0.2e1 * t90 * t159 + t492 * _y_ls[14] - 0.2e1 * t371 * t46 - t207 * t19 - 0.2e1 * t433 * t221 + 0.1000e4 * t207 * t39 + 0.2e1 * t208 * t170 - 0.2e1 * t169 * t237 - 0.1000e4 * t121 * t25 + 0.2e1 * t207 * t46 + 0.4e1 * t61 * t321 - 0.4e1 * t513 * t514 * t64 + 0.2e1 * t273 * t46;
00206 t530 = t54 * _y_ls[15] * _y_ls[14];
00207 t532 = t38 * t21;
00208 t533 = t532 * t60;
00209 t534 = t62 * t8;
00210 t540 = t350 * _y_ls[14];
00211 t543 = t331 * t64;
00212 t546 = t92 * _y_ls[15] * _y_ls[14];
00213 t549 = t353 * t64;
00214 t553 = -0.4e1 * t391 * t187 + 0.2e1 * t433 * t170 + 0.1000e4 * t273 * t39 + t143 * t530 - 0.4e1 * t533 * t534 * t64 - 0.2e1 * t61 * t414 + 0.1000e4 * t296 * t540 + t296 * t543 + t296 * t546 - t296 * t530 - t296 * t549 - 0.4e1 * t391 * t221;
00215 t588 = _y_ls[2] * _y_ls[15];
00216 t592 = _y_ls[2] * _y_ls[13];
00217 t598 = t143 * t549 - 0.2e1 * t378 * t179 + 0.2000e4 * _y_ls[6] * t49 * t111 * t133 * t24 - 0.2000e4 * t178 * t379 - 0.2e1 * t71 * t15 - 0.4000e4 * t133 * _y_ls[8] * t21 * t124 - 0.3000e4 * t49 * t1 * _y_ls[5] * t36 * t38 + 0.4e1 * t513 * t514 * t159 + 0.4000e4 * t38 * t49 * t22 * t133 * t28 - 0.3000e4 * t49 * t22 * t24 * t8 * _y_ls[6] - 0.2e1 * t70 * t588 * _y_ls[14] - 0.2000e4 * t70 * t592 * _y_ls[14] - 0.2e1 * t71 * t19;
00218 t627 = -0.4e1 * t157 * t158 * t64 - 0.2e1 * t140 * t268 - 0.1000e4 * t296 * t145 + 0.4000e4 * t70 * _y_ls[2] * t21 * t124 + 0.4e1 * t391 * t237 + 0.2000e4 * t71 * t25 - 0.2e1 * t39 * t225 - 0.2e1 * t143 * t354 - t143 * t351 - 0.1000e4 * t143 * t540 - 0.2e1 * t39 * t258 + 0.4e1 * t533 * t534 * t159;
00219 t638 = t54 * t3 * _y_ls[15];
00220 t643 = t92 * t8;
00221 t647 = t144 * t64;
00222 t657 = 0.2000e4 * t465 * t1 * t133 * t36 - t143 * t543 - 0.4e1 * t240 * t170 - 0.2e1 * t296 * t46 + 0.2e1 * t39 * t638 + 0.2e1 * t143 * t332 - 0.1000e4 * t51 * t643 * _y_ls[6] + t143 * t647 - 0.2000e4 * t182 * t39 + 0.2e1 * t169 * t187 - t296 * t647 - t143 * t546 - 0.2e1 * t25 * t638;
00223 t662 = 0.1e1 / (t116 + t167 + t213 + t252 + t292 + t329 + t370 + t404 + t436 + t460 + t489 + t520 + t553 + t598 + t627 + t657);
00224 t664 = dq[1] * dq[1];
00225 t667 = _y_ls[6] * dq[0];
00226 t668 = t667 * dq[1];
00227 t669 = D13 * t68;
00228 t670 = t669 * t53;
00229 t673 = D13 * t69;
00230 t674 = t673 * t62;
00231 t680 = _y_ls[7] * dq[0];
00232 t685 = dq[2] * t53;
00233 t693 = _y_ls[2] * dq[0];
00234 t700 = _y_ls[8] * dq[0];
00235 t706 = _y_ls[3] * t664 * t69 - 0.2e1 * t668 * t670 + tau[0] - 0.2e1 * t668 * t674 - 0.2e1 * t667 * dq[2] * t674 - 0.4e1 * t680 * dq[1] * t53 * t62 - _y_ls[23] - 0.4e1 * t680 * t685 * t62 - 0.2e1 * _y_ls[9] * dq[1] * dq[2] * t62 + 0.4e1 * t693 * dq[1] * t96 + 0.2e1 * _y_ls[10] * dq[1] * t685 - 0.4e1 * t700 * dq[2] * t92 - _y_ls[9] * t664 * t62;
00236 t707 = dq[2] * dq[2];
00237 t730 = _y_ls[5] * dq[0];
00238 t731 = t730 * dq[1];
00239 t732 = t669 * t62;
00240 t735 = t673 * t53;
00241 t743 = -_y_ls[9] * t707 * t62 + _y_ls[10] * t664 * t53 + _y_ls[10] * t707 * t53 - 0.2e1 * t693 * dq[1] + 0.2e1 * t700 * dq[2] + 0.2e1 * t700 * dq[1] + _y_ls[4] * t664 * t68 - 0.4e1 * t700 * dq[1] * t92 + 0.4e1 * _y_ls[1] * dq[0] * dq[1] * t68 * t69 + 0.2e1 * t731 * t732 - 0.2e1 * t731 * t735 - 0.2e1 * t730 * dq[2] * t735 - SSSIGN(dq[0],dqm) * _y_ls[18] - dq[0] * _y_ls[17];
00242 t744 = t706 + t743;
00243 t748 = 0.1000e4 * t190 * _y_ls[14];
00244 t750 = 0.1000e4 * t285;
00245 t754 = 0.1000e4 * t74 * t296;
00246 t756 = 0.1000e4 * t74 * t143;
00247 t757 = t74 * t159;
00248 t759 = 0.1000e4 * t127 * t296;
00249 t761 = 0.1000e4 * t127 * t143;
00250 t762 = t127 * t159;
00251 t763 = t190 * t64 + t748 - t284 * t64 - t750 + t74 * t64 + t127 * t64 - t754 - t756 - t757 - t759 - t761 - t762;
00252 t764 = t763 * t662;
00253 t765 = g1 * t68;
00254 t766 = l * l;
00255 t767 = r * r;
00256 t768 = l * r;
00257 t772 = sqrt(t766 + t767 - 0.2e1 * t768 * t68);
00258 t775 = dq[0] * dq[0];
00259 t776 = t775 * _y_ls[8];
00260 t781 = t775 * _y_ls[2];
00261 t794 = t775 * t53;
00262 t795 = t62 * _y_ls[7];
00263 t799 = _y_ls[5] * t772;
00264 t800 = t799 * t775;
00265 t804 = dq[2] * D13;
00266 t808 = t707 * D13;
00267 t811 = t765 * _y_ls[12] * t772 - t776 * t772 + 0.2e1 * t776 * t772 * t92 + t781 * t772 - 0.2e1 * t781 * t772 * t96 + 0.1000e4 * t768 * t69 * _y_ls[16] - 0.2e1 * t775 * t68 * t69 * _y_ls[1] * t772 + 0.2e1 * t794 * t795 * t772 - t800 * t732 + t800 * t735 - 0.2e1 * t799 * dq[1] * t804 * t8 - t799 * t808 * t8;
00268 t814 = g1 * t69;
00269 t817 = _y_ls[6] * t772;
00270 t818 = t817 * t775;
00271 t838 = -t799 * t765 * t8 + t799 * t814 * t1 + t818 * t670 + t818 * t674 + 0.2e1 * t817 * dq[1] * t804 * t1 + t817 * t808 * t1 + t817 * t765 * t1 + t817 * t814 * t8 - t69 * _y_ls[11] * t772 - dq[1] * _y_ls[19] * t772 - SSSIGN(dq[1],dqm) * _y_ls[20] * t772 - _y_ls[24] * t772;
00272 t842 = tau[1] + (t811 + t838) / t772;
00273 t858 = t754 + t756 + t759 + t761 - t190 * t159 + t284 * t159 - 0.1000e4 * t190 * t296 - 0.1000e4 * t190 * t143 + 0.1000e4 * t284 * t296 + 0.1000e4 * t284 * t143 + t137 + 0.1000e4 * t74 * _y_ls[13] + t74 * _y_ls[15] + 0.1000e4 * t128 - t748 + t750 - t757 - t762;
00274 t859 = t858 * t662;
00275 t878 = tau[2] + _y_ls[5] * t775 * t735 - dq[2] * _y_ls[21] - SSSIGN(dq[2],dqm) * _y_ls[22] + 0.2e1 * t776 * t92 + _y_ls[5] * t664 * t9 - _y_ls[25] + 0.2e1 * t794 * t795 + _y_ls[6] * t775 * t674 - _y_ls[6] * t664 * t2 - t776 - _y_ls[5] * g1 * t62 + _y_ls[6] * g1 * t53;
00276 _ddq[0] = t48 * t662 * t744 - t764 * t842 - t859 * t878;
00277 t881 = 0.1000e4 * t90;
00278 t887 = 0.1000e4 * t9 * _y_ls[6] * _y_ls[14];
00279 t890 = 0.1000e4 * t2 * _y_ls[5] * _y_ls[14];
00280 t893 = 0.2000e4 * t70 * _y_ls[2] * _y_ls[14];
00281 t895 = 0.2000e4 * t74 * t127;
00282 t899 = 0.2000e4 * t133 * _y_ls[8] * _y_ls[14];
00283 t900 = 0.1000e4 * t276;
00284 t904 = 0.1000e4 * t273 * _y_ls[14];
00285 t906 = 0.1000e4 * t371 * _y_ls[14];
00286 t907 = t6 + t12 - t881 - t207 * t64 + t371 * t64 - t273 * t64 + t887 + t890 - t893 - t895 + t121 * t64 + t899 + t900 + _y_ls[0] * t3 * _y_ls[15] - t904 + t906;
00287 t909 = 0.1000e4 * t121 * _y_ls[14];
00288 t911 = 0.1000e4 * t207 * _y_ls[14];
00289 t912 = t133 * t64;
00290 t917 = 0.1000e4 * t59 * t55 * _y_ls[14];
00291 t918 = t1 * t54;
00292 t921 = 0.1000e4 * t173 * t918 * _y_ls[14];
00293 t927 = 0.1000e4 * t59 * t643 * _y_ls[14];
00294 t929 = 0.1000e4 * t94;
00295 t930 = t1 * t92;
00296 t933 = 0.1000e4 * t173 * t930 * _y_ls[14];
00297 t940 = t133 * _y_ls[14];
00298 t942 = 0.2000e4 * t178 * t940;
00299 t944 = 0.2000e4 * t378 * t940;
00300 t949 = t909 - t911 - 0.2e1 * t378 * t912 + t917 - t921 + t143 * t638 - t296 * t638 - t143 * t258 - t927 + t296 * t258 - t929 + t933 - 0.2e1 * t178 * t912 + 0.2e1 * t133 * _y_ls[8] * t3 * _y_ls[15] - t942 - t944 - 0.2e1 * t70 * _y_ls[2] * t3 * _y_ls[15];
00301 t953 = t207 * t143;
00302 t957 = t182 * t296;
00303 t959 = t182 * t143;
00304 t962 = t273 * t296;
00305 t964 = t371 * t296;
00306 t967 = t133 * t159;
00307 t970 = t207 * t296;
00308 t972 = t371 * t143;
00309 t974 = t92 * t21;
00310 t975 = t974 * t479;
00311 t977 = t74 * t284;
00312 t979 = 0.1000e4 * t953 - _y_ls[0] * r3 * _y_ls[15] - 0.2000e4 * t957 - 0.2000e4 * t959 - t143 * t225 + 0.1000e4 * t962 - 0.1000e4 * t964 + t296 * t225 - t40 + 0.2e1 * t378 * t967 + 0.1000e4 * t970 - t43 - 0.1000e4 * t972 + 0.1000e4 * t975 - 0.1000e4 * t977;
00313 t980 = t273 * t143;
00314 t982 = t71 * t296;
00315 t984 = t357 * t82;
00316 t988 = t357 * t123;
00317 t990 = t54 * t21;
00318 t991 = t990 * t479;
00319 t993 = t990 * t476;
00320 t996 = t127 * t284;
00321 t998 = t74 * t190;
00322 t1000 = t974 * t476;
00323 t1004 = t155 * t36 * t133 * _y_ls[6];
00324 t1009 = 0.1000e4 * t980 + 0.2000e4 * t982 - 0.1000e4 * t984 + t273 * t159 - t371 * t159 - 0.1000e4 * t988 - 0.1000e4 * t991 + 0.1000e4 * t993 - t121 * t159 - 0.1000e4 * t996 + 0.1000e4 * t998 - 0.1000e4 * t1000 + 0.2000e4 * t1004 + t207 * t159 - t296 * t164 + t143 * t164;
00325 t1017 = t511 * t22 * t133 * _y_ls[5];
00326 t1027 = t532 * t1 * t133 * t8;
00327 t1029 = t71 * t143;
00328 t1031 = t121 * t296;
00329 t1033 = t121 * t143;
00330 t1035 = t127 * t190;
00331 t1039 = t335 * t8 * t133 * t1;
00332 t1041 = 0.2e1 * t70 * _y_ls[2] * r3 * _y_ls[15] + 0.2000e4 * t1017 + 0.2e1 * t178 * t967 - 0.2e1 * t133 * _y_ls[8] * r3 * _y_ls[15] + 0.2000e4 * t1027 + 0.2000e4 * t1029 - 0.1000e4 * t1031 + t881 - 0.1000e4 * t1033 + 0.1000e4 * t1035 + 0.2000e4 * t1039 - t887 - t890 + t893 + t895;
00333 t1042 = -t899 - t900 - t31 - t34 - t26 + t904 - t906 - t909 + t911 - t917 + t921 + t927 + t929 - t933 + t942 + t944;
00334 t1045 = (t979 + t1009 + t1041 + t1042) * t662;
00335 _ddq[1] = -t764 * t744 + (t907 + t949) * t662 * t842 + t1045 * t878;
00336 t1059 = t133 * _y_ls[15];
00337 t1074 = 0.1000e4 * t173 * t930 * _y_ls[13] - 0.2000e4 * t953 + 0.4000e4 * t957 + 0.4000e4 * t959 - 0.2000e4 * t962 + 0.2000e4 * t964 + 0.2000e4 * t39 - 0.2000e4 * t970 - 0.2e1 * t378 * t1059 + 0.2000e4 * t972 - 0.2000e4 * t975 + 0.2000e4 * t977 + t59 * t55 * _y_ls[15] - 0.2000e4 * t980 - 0.4000e4 * t982 + 0.2000e4 * t984 + 0.2000e4 * t988 + 0.2000e4 * t991 - 0.2000e4 * t993 + 0.2000e4 * t996;
00338 t1100 = t133 * _y_ls[13];
00339 t1109 = -0.2000e4 * t998 + 0.2000e4 * t1000 - 0.4000e4 * t1004 + t492 - 0.1000e4 * t59 * t643 * _y_ls[13] - 0.1000e4 * t173 * t918 * _y_ls[13] + t2 * t314 + 0.2000e4 * t133 * t366 + 0.1000e4 * t9 * t217 + 0.2e1 * t133 * t384 + t9 * t310 - 0.2000e4 * t70 * t592 + 0.1000e4 * t2 * t298 - 0.2e1 * t70 * t588 - 0.1000e4 * t103 - 0.4000e4 * t1017 - 0.2000e4 * t378 * t1100 + 0.1000e4 * t59 * t55 * _y_ls[13] - t59 * t643 * _y_ls[15] + t371 * _y_ls[15];
00340 t1138 = -t207 * _y_ls[15] - 0.1000e4 * t273 * _y_ls[13] + 0.1000e4 * t121 * _y_ls[13] - t273 * _y_ls[15] + t121 * _y_ls[15] + 0.1000e4 * t371 * _y_ls[13] - 0.1000e4 * t207 * _y_ls[13] - 0.4000e4 * t1027 - 0.4000e4 * t1029 - t173 * t918 * _y_ls[15] - 0.2000e4 * t178 * t1100 + 0.2000e4 * t1031 - 0.1000e4 * t98 - 0.2e1 * t178 * t1059 + t173 * t930 * _y_ls[15] - t881 + 0.1000e4 * t86 + 0.2000e4 * t1033 - 0.2000e4 * t1035 - 0.4000e4 * t1039;
00341 t1143 = t887 + t890 - t893 - t895 + t899 + t900 + 0.4000e4 * t30 + 0.2000e4 * t190 * t284 + 0.2000e4 * t25 - t904 + t906 + t909 - t911 + t917 - t921 - t927 - t929 + t933 - t942 - t944;
00342 _ddq[2] = -t859 * t744 + t1045 * t842 + (t1074 + t1109 + t1138 + t1143) * t662 * t878;
00343
00344
00345 tau4 = (_y_ls2[1*3+0]*dq[3] + _y_ls2[2*3+0]*SSSIGN(dq[3],dqm) + _y_ls2[3*3+0]);
00346 tau5 = (_y_ls2[1*3+1]*dq[4] + _y_ls2[2*3+1]*SSSIGN(dq[4],dqm) + _y_ls2[3*3+1]);
00347 tau6 = (_y_ls2[1*3+2]*dq[5] + _y_ls2[2*3+2]*SSSIGN(dq[5],dqm) + _y_ls2[3*3+2]);
00348
00349 taut[0] = tau[3] - tau4;
00350 taut[1] = tau[4] - tau5;
00351 taut[2] = tau[5] - tau6;
00352
00353 _ddq[3] = taut[0]/_y_ls2[0*3+0];
00354 _ddq[4] = taut[1]/_y_ls2[0*3+1];
00355 _ddq[5] = taut[2]/_y_ls2[0*3+2];
00356
00357 return _ddq;
00358 }
00359
00360
