PRISMA Lab
prima prova bracctio
Dependencies: Eigen MX64_senzaCorrente
Embed:
(wiki syntax)
Show/hide line numbers
ARAP180.cpp
00001 #include "ARAP180.h" 00002 #include "ARM_parameters.h" 00003 00004 //***************************************************************************** 00005 00006 ARAP180::ARAP180(){ 00007 00008 q_sat_max(0) = M_PI/2; 00009 q_sat_max(1) = M_PI; 00010 q_sat_max(2) = M_PI; 00011 q_sat_max(3) = M_PI/4; 00012 q_sat_max(4) = M_PI; 00013 q_sat_max(5) = M_PI/4; 00014 00015 q_sat_min(0) = -M_PI/2; 00016 q_sat_min(1) = -M_PI/6; 00017 q_sat_min(2) = -0.1; 00018 q_sat_min(3) = -M_PI; 00019 q_sat_min(4) = -M_PI; 00020 q_sat_min(5) = -M_PI/4; 00021 00022 ID[0] = 1; 00023 ID[1] = 2; 00024 ID[2] = 3; 00025 ID[3] = 4; 00026 ID[4] = 5; 00027 00028 operatingMode[0] = 3; 00029 operatingMode[1] = 3; 00030 operatingMode[2] = 3; 00031 operatingMode[3] = 3; 00032 operatingMode[4] = 3; 00033 00034 enableVal[0] = 1; 00035 enableVal[1] = 1; 00036 enableVal[2] = 1; 00037 enableVal[3] = 1; 00038 enableVal[4] = 1; 00039 00040 motorSign(0) = 1; 00041 motorSign(1) = -1; 00042 motorSign(2) = 1; 00043 motorSign(3) = 1; 00044 motorSign(4) = -1; 00045 motorSign(5) = 1; 00046 00047 offsets(0) = 0.0; 00048 offsets(1) = 0.0; 00049 offsets(2) = 0.0; 00050 offsets(3) = 0.0; 00051 offsets(4) = 0.0; 00052 offsets(5) = 0.0; 00053 00054 dxl_port = new UARTSerial_half(PB_9,PB_8,PF_13, 1000000); 00055 mx_MotorChain = new MX(ID, sizeof(ID)/sizeof(int), 1000000, dxl_port); 00056 } 00057 00058 00059 void ARAP180::initArmMotors(){ 00060 mx_MotorChain->SyncOperatingMode(operatingMode, -1); 00061 00062 //Accelleration init 00063 printf("\n Inizializzo i profili di Accelerazione "); 00064 setMaxAcc(100); 00065 00066 00067 00068 printf("\n Inizializzo i profili di Velocita' "); 00069 setMaxVel(20); 00070 00071 mx_MotorChain->SyncTorqueEnable(enableVal, -1); 00072 printf("Motori inizializzati!!\r\n"); 00073 00074 00075 } 00076 00077 00078 void ARAP180::setMaxVel(float maxVel){ 00079 00080 float Vel_profile[5] = {maxVel,maxVel,maxVel,maxVel,maxVel}; 00081 mx_MotorChain->SyncProfileVel(Vel_profile); 00082 00083 } 00084 00085 void ARAP180::setMaxAcc(float maxAcc){ 00086 00087 float Acc_profile[5] = {maxAcc,maxAcc,maxAcc,maxAcc,maxAcc}; 00088 mx_MotorChain->SyncProfileAccel(Acc_profile); 00089 00090 00091 } 00092 00093 00094 void ARAP180::setJointPos(Vector6f q){ 00095 00096 00097 float Goal_position[5]; 00098 00099 Goal_position[0] = utilities::rad2deg(q(0)) * motorSign(0) + offsets(0); 00100 Goal_position[1] = utilities::rad2deg(q(1)) * motorSign(1) + offsets(1); 00101 Goal_position[2] = utilities::rad2deg(q(2)) * motorSign(2) + offsets(2); 00102 Goal_position[3] = utilities::rad2deg(q(3)) * motorSign(3) + offsets(3); 00103 Goal_position[4] = utilities::rad2deg(q(4)) * motorSign(4) + offsets(4); 00104 00105 00106 mx_MotorChain->SyncSetGoal(Goal_position); 00107 00108 00109 } 00110 00111 00112 Vector6f ARAP180::getJointPos(){ 00113 00114 Vector6f getPosConverted = Vector6f::Zero(); 00115 float getPos[5]; 00116 00117 mx_MotorChain->SyncGetPosition(getPos); 00118 00119 00120 getPosConverted(0) = utilities::deg2rad(getPos[0] - offsets(0)) * motorSign(0); 00121 getPosConverted(1) = utilities::deg2rad(getPos[1] - offsets(1)) * motorSign(1); 00122 getPosConverted(2) = utilities::deg2rad(getPos[2] - offsets(2)) * motorSign(2); 00123 getPosConverted(3) = utilities::deg2rad(getPos[3] - offsets(3)) * motorSign(3); 00124 getPosConverted(4) = utilities::deg2rad(getPos[4] - offsets(4)) * motorSign(4); 00125 getPosConverted(5) = (0.0 - offsets(4)) * motorSign(5); 00126 00127 return getPosConverted; 00128 00129 } 00130 00131 00132 00133 00134 00135 00136 //Dyrect Kinematics Matrix Te 00137 Matrix4f ARAP180::forwardKinematics(Vector6f q){ 00138 00139 Matrix4f A0 = Matrix4f::Zero(); 00140 float q1 = q[0]; 00141 float q2 = q[1]; 00142 float q3 = q[2]; 00143 float q4 = q[3]; 00144 float q5 = q[4]; 00145 float q6 = q[5]; 00146 float t2 = cos(q1); 00147 float t3 = cos(q5); 00148 float t4 = cos(q6); 00149 float t5 = sin(q1); 00150 float t6 = sin(q2); 00151 float t7 = sin(q5); 00152 float t8 = sin(q6); 00153 float t9 = q2*-1.0; 00154 float t10 = q2*1.0; 00155 float t12 = -q2; 00156 float t13 = -q3; 00157 float t11 = a2*t6; 00158 float t14 = t3*t5; 00159 float t15 = t2*t3*1.0; 00160 float t16 = q3+q4+t9; 00161 float t17 = q2+t13; 00162 float t19 = q3+q4+t12; 00163 float t18 = cos(t17); 00164 float t20 = sin(t17); 00165 float t21 = cos(t16); 00166 float t22 = sin(t16); 00167 float t23 = cos(t19); 00168 float t24 = sin(t19); 00169 float t25 = a3*t18; 00170 float t26 = a3i*t20*-1.0; 00171 float t27 = a3i*t20*1.0; 00172 float t28 = a4*t23*1.0; 00173 float t29 = d5*t24*-1.0; 00174 float t30 = d5*t24*1.0; 00175 float t31 = t2*t7*t21; 00176 float t32 = t5*t7*t21*-1.0; 00177 float t33 = t5*t7*t21*1.0; 00178 float t34 = t14+t31; 00179 float t35 = t15+t32; 00180 float t36 = a1+t11+t25+t26+t28+t29; 00181 A0(0,0) = t4*t34+t2*t8*t22; 00182 A0(0,1) = t8*t34*-1.0+t2*t4*t22; 00183 A0(0,2) = t5*t7*-1.0+t2*t3*t23; 00184 A0(0,3) = t2*t36; 00185 A0(1,0) = t4*t35*-1.0+t5*t8*t22; 00186 A0(1,1) = t8*t35+t4*t5*t22; 00187 A0(1,2) = t2*t7+t14*t23; 00188 A0(1,3) = t5*t36; 00189 A0(2,0) = t8*t23*-1.0+t4*t7*t24; 00190 A0(2,1) = t4*t23*-1.0-t7*t8*t24*1.0; 00191 A0(2,2) = t3*t24; 00192 A0(2,3) = a3*t20*-1.0+a4*t24*1.0-a3i*t18*1.0+d5*t23+a2*cos(q2); 00193 A0(3,3) = 1.0; 00194 00195 return A0; 00196 } 00197 00198 //Jacobian matrix J 00199 Matrix6f ARAP180::jacobianMatrix(Vector6f q){ 00200 00201 Matrix6f A0 = Matrix6f::Zero(); 00202 float q1 = q[0]; 00203 float q2 = q[1]; 00204 float q3 = q[2]; 00205 float q4 = q[3]; 00206 float q5 = q[4]; 00207 float q6 = q[5]; 00208 float t2 = cos(q1); 00209 float t3 = cos(q2); 00210 float t4 = cos(q5); 00211 float t5 = sin(q1); 00212 float t6 = sin(q2); 00213 float t7 = sin(q5); 00214 float t12 = -q2; 00215 float t13 = -q3; 00216 float t8 = t2*-1.0; 00217 float t9 = t2*1.0; 00218 float t10 = a2*t3; 00219 float t11 = a2*t6; 00220 float t15 = q2+t13; 00221 float t17 = q3+q4+t12; 00222 float t14 = a1+t11; 00223 float t16 = cos(t15); 00224 float t18 = sin(t15); 00225 float t19 = cos(t17); 00226 float t20 = sin(t17); 00227 float t21 = a3*t16; 00228 float t22 = a3i*t16*-1.0; 00229 float t23 = a3i*t16*1.0; 00230 float t24 = a3*t18*-1.0; 00231 float t25 = a3*t18*1.0; 00232 float t26 = a3i*t18*-1.0; 00233 float t27 = a3i*t18*1.0; 00234 float t28 = d5*t19; 00235 float t29 = a4*t19*1.0; 00236 float t30 = a4*t20*1.0; 00237 float t31 = d5*t20*-1.0; 00238 float t32 = d5*t20*1.0; 00239 float t33 = t28+t30; 00240 float t34 = t14+t21+t26; 00241 float t35 = t29+t34; 00242 float t40 = t22+t24+t33; 00243 float t36 = t2*t35; 00244 float t37 = t5*t35; 00245 float t41 = t10+t40; 00246 float t42 = t31+t35; 00247 float t38 = -t36; 00248 float t39 = -t37; 00249 float t43 = t2*t42; 00250 float t44 = t5*t42; 00251 float t45 = t38+t43; 00252 float t46 = t39+t44; 00253 A0(0,0) = -t44; 00254 A0(0,1) = t2*t41; 00255 A0(0,2) = -t8*(t23+t25-t33); 00256 A0(0,3) = t8*t33; 00257 A0(0,4) = t19*(t37-t44)-t5*t20*t28*1.0; 00258 A0(1,0) = t43; 00259 A0(1,1) = t5*t41*1.0; 00260 A0(1,2) = t5*(t23+t25-t33); 00261 A0(1,3) = -t5*t33; 00262 A0(1,4) = -t19*(t36-t43)+t9*t20*t28; 00263 A0(2,1) = -t2*(t43-a1*t2)-t5*(t44-a1*t5)*1.0; 00264 A0(2,2) = t5*(t44-t5*t14)+t9*(t43-t2*t14); 00265 A0(2,3) = t5*(t44-t5*t34)+t9*(t43-t2*t34); 00266 A0(2,4) = t5*t20*(t36-t43)*-1.0+t9*t20*(t37-t44); 00267 A0(3,1) = t5*-1.0; 00268 A0(3,2) = t5; 00269 A0(3,3) = t5; 00270 A0(3,4) = t8*t20; 00271 A0(3,5) = t5*t7*-1.0+t2*t4*t19; 00272 A0(4,1) = t2; 00273 A0(4,2) = t8; 00274 A0(4,3) = t8; 00275 A0(4,4) = t5*t20*-1.0; 00276 A0(4,5) = t2*t7+t4*t5*t19; 00277 A0(5,0) = 1.0; 00278 A0(5,4) = t19; 00279 A0(5,5) = t4*t20; 00280 return A0; 00281 } 00282 00283 Vector6f ARAP180::backwardKinematics(Matrix4f Td, Vector6f q_init, float Tsam, int n_max_iter, float max_pError, float max_oError ,float _Kp, float _Ko){ 00284 00285 00286 Matrix3f Kp = _Kp*Matrix3f::Identity(); 00287 Matrix3f Ko = _Ko*Matrix3f::Identity(); 00288 00289 Vector6f q = Vector6f::Zero(); 00290 00291 Vector6f dq = Vector6f::Zero(); 00292 Vector6f dp = Vector6f::Zero(); 00293 Matrix6f W_inv = Matrix6f::Identity(); 00294 00295 //Set destination frame 00296 00297 Matrix4f Te = Matrix4f::Zero(); 00298 Matrix6f J = Matrix6f::Zero(); 00299 Matrix6f pinvJ = Matrix6f::Zero(); 00300 Matrix6f J_T = Matrix6f::Zero(); 00301 Matrix3f L = Matrix3f::Zero(); 00302 Matrix3f pinvL = Matrix3f::Zero(); 00303 Vector3f vp = Vector3f::Zero(); 00304 Vector3f vo = Vector3f::Zero(); 00305 Vector6f v = Vector6f::Zero(); 00306 00307 00308 int n_iter = 0; 00309 00310 float pError = 0.0; 00311 float oError = 0.0; 00312 q = q_init; 00313 Te = forwardKinematics(q); 00314 J = jacobianMatrix(q); 00315 00316 00317 Vector3f ep = Td.block(0,3,3,1) - Te.block(0,3,3,1); 00318 Vector3f eo = utilities::rotationMatrixError(Td, Te); 00319 00320 pError = ep.norm(); //L_2 norm of vector 00321 oError = eo.norm(); 00322 00323 while(n_iter<n_max_iter && (pError>max_pError || oError>max_oError)){ 00324 00325 L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3)); 00326 //FullPivLU<Matrix3d> Core_L(L); 00327 pinvL = L.inverse(); 00328 00329 00330 //vp = dp.block(0,0,3,1) + Kp*ep; 00331 vp = Kp*ep; 00332 00333 vo = pinvL*(L.transpose()*dp.block(3,0,3,1) + Ko*eo); 00334 //vo = (Ko*eo); 00335 v << vp[0], vp[1], vp[2], vo[0], vo[1], vo[2]; 00336 00337 //J_T = J.transpose(); 00338 00339 00340 // If we need a weighted pinv 00341 //FullPivLU<Matrix6d> Core_J(J*W_inv*J_T); 00342 //pinvJ = W_inv*J_T*Core_J.inverse(); 00343 00344 //FullPivLU<Matrix6d> Core_J(J); 00345 00346 pinvJ = J.inverse(); 00347 00348 dq = pinvJ*v; 00349 00350 00351 q += dq*Tsam; 00352 00353 00354 Te = forwardKinematics(q); 00355 00356 00357 J = jacobianMatrix(q); 00358 00359 00360 ep = Td.block(0,3,3,1) - Te.block(0,3,3,1); 00361 eo = utilities::rotationMatrixError(Td, Te); 00362 00363 pError = ep.norm(); //L_2 norm of vector 00364 oError = eo.norm(); 00365 n_iter++; 00366 } 00367 //cout << "posError: " << pError << " orError: " << oError << endl; 00368 00369 00370 return q; 00371 00372 00373 } 00374 00375 Vector6f ARAP180::jointSaturation(Vector6f q){ 00376 00377 00378 Vector6f q_sat = q; 00379 for(int i = 0; i<6; i++){ 00380 00381 if(q_sat(i) > q_sat_max(i)){ q_sat(i) = q_sat_max(i);} 00382 if(q_sat(i) < q_sat_min(i)){ q_sat(i) = q_sat_min(i);} 00383 00384 } 00385 00386 00387 00388 return q_sat; 00389 00390 00391 } 00392 00393 00394 // Inertia matrix B 00395 00396 Matrix6f ARAP180::inertiaMatrix(Vector6f q){ 00397 00398 Matrix6f A0 = Matrix6f::Zero(); 00399 float q1 = q[0]; 00400 float q2 = q[1]; 00401 float q3 = q[2]; 00402 float q4 = q[3]; 00403 float q5 = q[4]; 00404 float q6 = q[5]; 00405 00406 float t2 = cos(q2); 00407 float t3 = cos(q3); 00408 float t4 = cos(q4); 00409 float t5 = cos(q5); 00410 float t6 = cos(q6); 00411 float t7 = sin(q2); 00412 float t8 = sin(q3); 00413 float t9 = sin(q4); 00414 float t10 = sin(q5); 00415 float t11 = sin(q6); 00416 float t12 = q3+q4; 00417 float t13 = q4+q6; 00418 float t14 = a1*a1; 00419 float t15 = a2*a2; 00420 float t16 = a3*a3; 00421 float t17 = a4*a4; 00422 float t18 = a3i*a3i; 00423 float t19 = d5*d5; 00424 float t20 = q2*2.0; 00425 float t21 = q3*2.0; 00426 float t22 = q4*2.0; 00427 float t32 = -d5; 00428 float t33 = -q2; 00429 float t35 = -q3; 00430 float t37 = -q4; 00431 float t39 = -q5; 00432 float t40 = -q6; 00433 float t23 = cos(t20); 00434 float t24 = a2*t2; 00435 float t25 = a3i*t4; 00436 float t26 = a2*t7; 00437 float t27 = a3*t9; 00438 float t28 = a4*t11; 00439 float t29 = cos(t12); 00440 float t30 = q5+t13; 00441 float t31 = sin(t12); 00442 float t34 = -t20; 00443 float t36 = -t21; 00444 float t38 = -t22; 00445 float t41 = a3*mpx4*t4; 00446 float t42 = a2*mpy3*t3; 00447 float t43 = a2*mpx3*t8; 00448 float t45 = a3i*mpx4*t9; 00449 float t46 = mass4*t16; 00450 float t47 = mass5*t16; 00451 float t48 = mass6*t16; 00452 float t49 = mass5*t17; 00453 float t50 = mass6*t17; 00454 float t51 = mass4*t18; 00455 float t52 = mass5*t18; 00456 float t53 = mass6*t18; 00457 float t54 = mass6*t19; 00458 float t56 = d5*mpy6*t6*2.0; 00459 float t58 = a3*a4*mass5*t4; 00460 float t59 = a3*a4*mass6*t4; 00461 float t60 = a2*a3i*mass4*t3; 00462 float t61 = a2*a3i*mass5*t3; 00463 float t62 = a2*a3i*mass6*t3; 00464 float t65 = a2*a3*mass4*t8; 00465 float t66 = a2*a3*mass5*t8; 00466 float t67 = a2*a3*mass6*t8; 00467 float t68 = a4*a3i*mass5*t9; 00468 float t69 = a4*a3i*mass6*t9; 00469 float t71 = a3*t4*t11; 00470 float t72 = d5*t6*t10; 00471 float t73 = a3i*t9*t11; 00472 float t75 = q2+t35; 00473 float t76 = q4+t40; 00474 float t83 = t12+t33; 00475 float t86 = t13+t39; 00476 float t87 = q5+q6+t37; 00477 float t108 = a2*mpx4*t3*t9; 00478 float t109 = a2*mpx4*t4*t8; 00479 float t111 = d5*mpy6*t5*t11; 00480 float t113 = t20+t35; 00481 float t115 = a2*d5*mass6*t3*t4; 00482 float t116 = a2*a4*mass5*t3*t9; 00483 float t117 = a2*a4*mass5*t4*t8; 00484 float t118 = a2*a4*mass6*t3*t9; 00485 float t119 = a2*a4*mass6*t4*t8; 00486 float t121 = a2*d5*mass6*t8*t9; 00487 float t150 = a2*mpy6*t3*t4*t6; 00488 float t153 = a2*mpy6*t6*t8*t9; 00489 float t167 = a2*mass6*t3*t4*t32; 00490 float t182 = a2*mpy6*t3*t9*t10*t11; 00491 float t183 = a2*mpy6*t4*t8*t10*t11; 00492 float t197 = q6-t12*2.0+t20; 00493 float t44 = mpy6*t28; 00494 float t55 = t41*2.0; 00495 float t57 = t45*2.0; 00496 float t63 = a2*t29; 00497 float t64 = d5*mass6*t25; 00498 float t70 = d5*mass6*t27; 00499 float t74 = -t25; 00500 float t77 = -t27; 00501 float t78 = -t41; 00502 float t80 = -t42; 00503 float t81 = cos(t75); 00504 float t82 = -t56; 00505 float t84 = t12+t34; 00506 float t85 = q5+t76; 00507 float t88 = -t43; 00508 float t89 = -t45; 00509 float t91 = sin(t75); 00510 float t92 = t58*2.0; 00511 float t93 = t59*2.0; 00512 float t95 = t68*2.0; 00513 float t96 = t69*2.0; 00514 float t98 = -t46; 00515 float t99 = -t47; 00516 float t100 = -t48; 00517 float t101 = -t49; 00518 float t102 = -t50; 00519 float t103 = -t51; 00520 float t104 = -t52; 00521 float t105 = -t53; 00522 float t106 = -t54; 00523 float t107 = mpy6*t6*t25; 00524 float t110 = mpy6*t6*t27; 00525 float t112 = mpy6*t72; 00526 float t114 = t20+t36; 00527 float t120 = t6*t10*t25; 00528 float t122 = t6*t10*t27; 00529 float t123 = -t58; 00530 float t125 = -t59; 00531 float t127 = mass6*t25*t32; 00532 float t129 = -t65; 00533 float t130 = -t66; 00534 float t131 = -t67; 00535 float t132 = -t68; 00536 float t134 = -t69; 00537 float t136 = cos(t83); 00538 float t137 = mass6*t27*t32; 00539 float t139 = sin(t83); 00540 float t141 = -t73; 00541 float t145 = cos(t113); 00542 float t148 = sin(t113); 00543 float t154 = mpy6*t10*t71; 00544 float t159 = mpy6*t10*t73; 00545 float t160 = -t108; 00546 float t161 = -t109; 00547 float t169 = -t116; 00548 float t170 = -t117; 00549 float t171 = -t118; 00550 float t172 = -t119; 00551 float t185 = -t153; 00552 float t196 = q6-t12+t113; 00553 float t198 = t25+t27+t32; 00554 float t203 = q5+t197; 00555 float t79 = -t55; 00556 float t90 = -t57; 00557 float t94 = t64*2.0; 00558 float t97 = t70*2.0; 00559 float t124 = -t92; 00560 float t126 = -t93; 00561 float t133 = -t95; 00562 float t135 = -t96; 00563 float t140 = sin(t84); 00564 float t142 = t107*2.0; 00565 float t143 = t110*2.0; 00566 float t144 = t10*t44*2.0; 00567 float t146 = cos(t114); 00568 float t147 = q3+t84; 00569 float t149 = sin(t114); 00570 float t151 = a3*t81; 00571 float t152 = a3i*t81; 00572 float t155 = a3*t91; 00573 float t156 = a3i*t91; 00574 float t157 = mpy6*t6*t74; 00575 float t162 = mpy6*t6*t77; 00576 float t165 = a4*t136; 00577 float t166 = d5*t136; 00578 float t168 = a4*t139; 00579 float t173 = d5*t139; 00580 float t174 = t6*t10*t74; 00581 float t175 = t6*t10*t77; 00582 float t176 = t154*2.0; 00583 float t178 = t159*2.0; 00584 float t186 = -t154; 00585 float t188 = t12+t84; 00586 float t191 = mpy6*t10*t141; 00587 float t195 = t32*t139; 00588 float t202 = q5+t196; 00589 float t206 = mpy6*t5*t11*t198; 00590 float t207 = d5+t63+t74+t77; 00591 float t128 = -t94; 00592 float t138 = -t97; 00593 float t158 = -t142; 00594 float t163 = -t143; 00595 float t164 = -t144; 00596 float t177 = cos(t147); 00597 float t179 = q6+t147; 00598 float t180 = sin(t147); 00599 float t184 = -t152; 00600 float t187 = -t176; 00601 float t189 = -t155; 00602 float t190 = -t156; 00603 float t192 = -t178; 00604 float t193 = cos(t188); 00605 float t194 = q6+t188; 00606 float t200 = q5+t40+t147; 00607 float t204 = q5+t40+t188; 00608 float t208 = t166+t168; 00609 float t209 = -t206; 00610 float t210 = mpy6*t5*t11*t207; 00611 float t214 = t28+t71+t72+t141+t174+t175; 00612 float t218 = -mpy6*t5*t11*(t152+t155-t168+t32*t136); 00613 float t225 = t45+t56+t64+t68+t69+t70+t78+t101+t102+t106+t121+t123+t125+t144+t150+t154+t157+t160+t161+t162+t167+t169+t170+t171+t172+t182+t183+t185+t191; 00614 float t181 = q5+t179; 00615 float t199 = q5+t194; 00616 float t201 = t39+t179; 00617 float t205 = t39+t194; 00618 float t211 = mpy6*t5*t11*t208; 00619 float t212 = -t210; 00620 float t213 = t184+t189+t208; 00621 float t216 = mpy6*t214; 00622 float t217 = a1+t26+t151+t165+t190+t195; 00623 float t224 = t41+t49+t50+t54+t58+t59+t82+t89+t107+t110+t127+t132+t134+t137+t159+t164+t186; 00624 float t226 = t56+t57+t60+t61+t62+t79+t80+t88+t94+t95+t96+t97+t98+t99+t100+t101+t102+t103+t104+t105+t106+t121+t124+t126+t129+t130+t131+t144+t150+t158+t160+t161+t163+t167+t169+t170+t171+t172+t176+t182+t183+t185+t192; 00625 float t215 = t24+t213; 00626 float t220 = mpy6*t10*t11*t217; 00627 float t221 = mpy6*t5*t6*t217; 00628 float t219 = mpy6*t5*t11*t215; 00629 float t223 = -t220; 00630 float t222 = -t219; 00631 A0(0,0) = t41+t42+t43+t46/2.0+t47/2.0+t48/2.0+t49/2.0+t50/2.0+t51/2.0+t52/2.0+t53/2.0+t54/2.0+t58+t59-t60-t61-t62+t65+t66+t67+t89+t127+t132+t134+t137+mass2*t14+mass3*t14+(mass3*t15)/2.0+mass4*t14+(mass4*t15)/2.0+mass5*t14+(mass5*t15)/2.0+mass6*t14+(mass6*t15)/2.0+(t46*t146)/2.0+(t47*t146)/2.0+(t48*t146)/2.0-(t51*t146)/2.0-(t52*t146)/2.0-(t53*t146)/2.0+(t49*t193)/2.0+(t50*t193)/2.0-(t54*t193)/2.0+(a3*mpy6*cos(t30))/4.0-(a3*mpy6*cos(t85))/4.0-(a3*mpy6*cos(t86))/4.0+(a3*mpy6*cos(t87))/4.0+(a3*mpy6*cos(t181))/4.0-(a3*mpy6*cos(t200))/4.0+(a4*mpy6*cos(t199))/4.0-(a3*mpy6*cos(t201))/4.0+(a3*mpy6*cos(t202))/4.0+(a4*mpy6*cos(t203))/4.0-(a4*mpy6*cos(t204))/4.0-(a4*mpy6*cos(t205))/4.0+(a3i*mpy6*cos(t13))/2.0+(a3i*mpy6*cos(t76))/2.0-(a3i*mpy6*cos(t179))/2.0-(a3i*mpy6*cos(t196))/2.0+(d5*mpy6*cos(t194))/2.0+(d5*mpy6*cos(t197))/2.0+(a3*mpy6*sin(t13))/2.0+(a3*mpy6*sin(t76))/2.0+(a3*mpy6*sin(t179))/2.0+(a4*mpy6*sin(t194))/2.0-(a3*mpy6*sin(t196))/2.0-(a4*mpy6*sin(t197))/2.0-(a3i*mpy6*sin(t30))/4.0+(a3i*mpy6*sin(t85))/4.0+(a3i*mpy6*sin(t86))/4.0+(a3i*mpy6*sin(t87))/4.0+(a3i*mpy6*sin(t181))/4.0-(a3i*mpy6*sin(t200))/4.0-(a3i*mpy6*sin(t201))/4.0-(a3i*mpy6*sin(t202))/4.0+(a1*mpy6*cos(q2+q5+q6-t12))/2.0-(d5*mpy6*sin(t199))/4.0+(d5*mpy6*sin(t203))/4.0+(d5*mpy6*sin(t204))/4.0+(d5*mpy6*sin(t205))/4.0+(a2*mpy6*sin(q5+q6-t12+t20))/4.0+(a1*mpy6*cos(q5+q6+t83))/2.0-(a1*mpy6*cos(q5+t40+t83))/2.0-(a1*mpy6*cos(q6+t39+t83))/2.0+(a2*mpy6*sin(q5+q6+t12))/4.0-(a2*mpy6*sin(q5+q6+t84))/4.0-(a2*mpy6*sin(q5+t12+t40))/4.0-(a2*mpy6*sin(q6+t12+t39))/4.0+(a2*mpy6*sin(q5+t40+t84))/4.0+(a2*mpy6*sin(q6+t39+t84))/4.0+(a2*mpy6*sin(t12+t39+t40))/4.0+a1*mass3*t26*2.0+a1*mass4*t26*2.0+a1*mass5*t26*2.0+a1*mass6*t26*2.0+a1*mass4*t151*2.0+a1*mass5*t151*2.0+a1*mass6*t151*2.0-a1*mass4*t156*2.0-a1*mass5*t156*2.0-a1*mass6*t156*2.0+a1*mass5*t165*2.0+a1*mass6*t165*2.0-a1*mass6*t173*2.0+a1*mpx2*t7*2.0+a2*mpx4*t31+a1*mpx3*t81*2.0+a1*mpx4*t136*2.0-a2*mpx4*t140+a2*mpx3*t148+a3*mpx4*t177-a1*mpy2*t2*2.0+a1*mpy3*t91*2.0-a2*mpy3*t145+a3i*mpx4*t180+d5*mass6*t63+(a2*mpy6*cos(q6-t12+t20))/2.0-(mass3*t15*t23)/2.0-(mass4*t15*t23)/2.0-(mass5*t15*t23)/2.0-(mass6*t15*t23)/2.0+mpy6*t6*t32-a1*mpy6*sin(q2+q6-t12)+(a4*mpy6*cos(q5+q6))/2.0-(a2*mpy6*cos(q6+t12))/2.0-(a4*mpy6*cos(q5+t40))/2.0+(a2*mpy6*cos(q6+t84))/2.0-(a2*mpy6*cos(t12+t40))/2.0+a1*mpy6*sin(q6+t83)+a2*a4*mass5*t31+a2*a4*mass6*t31-a2*a4*mass5*t140-a2*a4*mass6*t140+a2*a3*mass4*t148+a2*a3*mass5*t148+a2*a3*mass6*t148+a3*a4*mass5*t177+a3*a4*mass6*t177+a2*a3i*mass4*t145+a2*a3i*mass5*t145+a2*a3i*mass6*t145-a3*a3i*mass4*t149-a3*a3i*mass5*t149-a3*a3i*mass6*t149+a4*a3i*mass5*t180+a4*a3i*mass6*t180+a3i*d5*mass6*t177+a3*mass6*t32*t180+a2*mass6*t32*cos(t84)+a4*mass6*t32*sin(t188); 00632 A0(0,1) = t222; 00633 A0(0,2) = t218; 00634 A0(0,3) = t211; 00635 A0(0,4) = t223; 00636 A0(0,5) = t221; 00637 A0(1,0) = t222; 00638 A0(1,1) = t42*2.0+t43*2.0+t46+t47+t48+t49+t50+t51+t52+t53+t54+t55-t60*2.0-t61*2.0-t62*2.0+t65*2.0+t66*2.0+t67*2.0+t82+t90+t92+t93+t108*2.0+t109*2.0+t115*2.0+t116*2.0+t117*2.0+t118*2.0+t119*2.0-t121*2.0+t128+t133+t135+t138+t142+t143-t150*2.0+t153*2.0+t164+t178-t182*2.0-t183*2.0+t187+mass3*t15+mass4*t15+mass5*t15+mass6*t15; 00639 A0(1,2) = t226; 00640 A0(1,3) = t225; 00641 A0(1,4) = t212; 00642 A0(1,5) = -t44-mpy6*t71+mpy6*t73+t10*t107+t10*t110-t10*t150+t10*t153+mpy6*t6*t10*t32-a2*mpy6*t3*t9*t11-a2*mpy6*t4*t8*t11; 00643 A0(2,0) = t218; 00644 A0(2,1) = t226; 00645 A0(2,2) = t46+t47+t48+t49+t50+t51+t52+t53+t54+t55+t82+t90+t92+t93+t128+t133+t135+t138+t142+t143+t164+t178+t187; 00646 A0(2,3) = t224; 00647 A0(2,4) = t209; 00648 A0(2,5) = t216; 00649 A0(3,0) = t211; 00650 A0(3,1) = t225; 00651 A0(3,2) = t224; 00652 A0(3,3) = t49+t50+t54+t82+t164; 00653 A0(3,4) = t111; 00654 A0(3,5) = t44+t112; 00655 A0(4,0) = t223; 00656 A0(4,1) = t212; 00657 A0(4,2) = t209; 00658 A0(4,3) = t111; 00659 A0(5,0) = t221; 00660 A0(5,1) = -mpy6*(t214+a2*t3*t9*t11+a2*t4*t8*t11+a2*t3*t4*t6*t10-a2*t6*t8*t9*t10); 00661 A0(5,2) = t216; 00662 A0(5,3) = mpy6*(t28+t72); 00663 return A0; 00664 } 00665 00666 00667 00668 // Inertia matrix B 00669 Vector6f ARAP180::gravityVector(Vector6f q){ 00670 00671 Vector6f A0 = Vector6f::Zero(); 00672 float q1 = q[0]; 00673 float q2 = q[1]; 00674 float q3 = q[2]; 00675 float q4 = q[3]; 00676 float q5 = q[4]; 00677 float q6 = q[5]; 00678 00679 float t2 = sin(q2); 00680 float t3 = sin(q6); 00681 float t4 = -q2; 00682 float t5 = -q3; 00683 float t6 = -q4; 00684 float t7 = -q5; 00685 float t8 = -q6; 00686 float t9 = q2+t5; 00687 float t11 = q3+q4+t4; 00688 float t10 = cos(t9); 00689 float t12 = sin(t9); 00690 float t13 = cos(t11); 00691 float t14 = q6+t11; 00692 float t15 = sin(t11); 00693 float t19 = q6+t6+t9; 00694 float t21 = q5+t8+t11; 00695 float t16 = q5+t14; 00696 float t17 = sin(t14); 00697 float t20 = q5+t19; 00698 float t22 = t7+t14; 00699 float t23 = sin(t19); 00700 float t25 = cos(t21); 00701 float t27 = mpx3*t10*9.814; 00702 float t28 = mpy3*t12*9.814; 00703 float t29 = a3*mass4*t10*9.814; 00704 float t30 = a3*mass5*t10*9.814; 00705 float t31 = a3*mass6*t10*9.814; 00706 float t32 = a3i*mass4*t12*9.814; 00707 float t33 = a3i*mass5*t12*9.814; 00708 float t34 = a3i*mass6*t12*9.814; 00709 float t35 = mpx4*t13*9.814; 00710 float t36 = a4*mass5*t13*9.814; 00711 float t37 = a4*mass6*t13*9.814; 00712 float t38 = d5*mass6*t15*9.814; 00713 float t18 = cos(t16); 00714 float t24 = cos(t20); 00715 float t26 = cos(t22); 00716 float t39 = -t38; 00717 float t40 = mpy6*t17*4.907; 00718 float t42 = mpy6*t23*4.907; 00719 float t45 = mpy6*t25*2.4535; 00720 float t41 = mpy6*t18*2.4535; 00721 float t43 = -t42; 00722 float t44 = mpy6*t24*2.4535; 00723 float t46 = mpy6*t26*2.4535; 00724 float t47 = -t45; 00725 float t48 = -t46; 00726 A0(1) = -t27-t28-t29-t30-t31+t32+t33+t34-t35-t36-t37+t38-t40-t41+t42-t44+t45+t46-mpx2*t2*9.814+mpy2*cos(q2)*9.814-a2*mass3*t2*9.814-a2*mass4*t2*9.814-a2*mass5*t2*9.814-a2*mass6*t2*9.814; 00727 A0(2) = t27+t28+t29+t30+t31-t32-t33-t34+t35+t36+t37+t39+t40+t41+t43+t44+t47+t48; 00728 A0(3) = t35+t36+t37+t39+t40+t41+t43+t44+t47+t48; 00729 A0(4) = mpy6*t3*(sin(q5+t6+t9)-sin(q5+t11))*4.907; 00730 A0(5) = mpy6*(t3*t13*9.814-t15*cos(q6)*sin(q5)*9.814); 00731 return A0; 00732 } 00733 00734 00735 Vector6f ARAP180::frictionVector(Vector6f dq){ 00736 00737 Vector6f A0 = Vector6f::Zero(); 00738 float dq1 = dq[0]; 00739 float dq2 = dq[1]; 00740 float dq3 = dq[2]; 00741 float dq4 = dq[3]; 00742 float dq5 = dq[4]; 00743 float dq6 = dq[5]; 00744 00745 A0(0) = Fv1*dq1+Fs1*((dq1/fabs(dq1))); 00746 A0(1) = Fv2*dq2+Fs2*((dq2/fabs(dq2))); 00747 A0(2) = Fv3*dq3+Fs3*((dq3/fabs(dq3))); 00748 A0(3) = Fv4*dq4+Fs4*((dq4/fabs(dq4))); 00749 A0(4) = Fv5*dq5+Fs5*((dq5/fabs(dq5))); 00750 A0(5) = Fv6*dq6+Fs6*((dq6/fabs(dq6))); 00751 return A0; 00752 00753 } 00754 00755 Vector6f ARAP180::stiffnessVector(Vector6f q){ 00756 00757 Vector6f A0 = Vector6f::Zero(); 00758 float q1 = q[0]; 00759 float q2 = q[1]; 00760 float q3 = q[2]; 00761 float q4 = q[3]; 00762 float q5 = q[4]; 00763 float q6 = q[5]; 00764 00765 A0(1) = O2+K2*q2; 00766 A0(2) = O3+K3*q3; 00767 A0(3) = O4+K4*q4; 00768 return A0; 00769 00770 } 00771 00772 /* 00773 Matrix6f ARAP180::coriolisCentrifugalMatrix(Vector6f q, Vector6f dq){ 00774 00775 Matrix6f A0 = Matrix6f::Zero(); 00776 float q1 = q[0]; 00777 float q2 = q[1]; 00778 float q3 = q[2]; 00779 float q4 = q[3]; 00780 float q5 = q[4]; 00781 float q6 = q[5]; 00782 float dq1 = dq[0]; 00783 float dq2 = dq[1]; 00784 float dq3 = dq[2]; 00785 float dq4 = dq[3]; 00786 float dq5 = dq[4]; 00787 float dq6 = dq[5]; 00788 00789 float t2 = cos(q2); 00790 float t3 = cos(q3); 00791 float t4 = cos(q4); 00792 float t5 = cos(q5); 00793 float t6 = cos(q6); 00794 float t7 = sin(q2); 00795 float t8 = sin(q3); 00796 float t9 = sin(q4); 00797 float t10 = sin(q5); 00798 float t11 = sin(q6); 00799 float t12 = q3+q4; 00800 float t13 = q4+q6; 00801 float t14 = q5+q6; 00802 float t15 = a2*a2; 00803 float t16 = a3*a3; 00804 float t17 = a4*a4; 00805 float t18 = a3i*a3i; 00806 float t19 = d5*d5; 00807 float t20 = q2*2.0; 00808 float t21 = q3*2.0; 00809 float t22 = q4*2.0; 00810 float t36 = -q2; 00811 float t38 = -q3; 00812 float t40 = -q4; 00813 float t42 = -q5; 00814 float t43 = -q6; 00815 float t23 = cos(t20); 00816 float t24 = cos(t21); 00817 float t25 = cos(t22); 00818 float t26 = sin(t20); 00819 float t27 = sin(t21); 00820 float t28 = sin(t22); 00821 float t29 = cos(t12); 00822 float t30 = cos(t13); 00823 float t31 = q6+t12; 00824 float t32 = q5+t13; 00825 float t33 = sin(t12); 00826 float t34 = sin(t13); 00827 float t35 = sin(t14); 00828 float t37 = -t20; 00829 float t39 = -t21; 00830 float t41 = -t22; 00831 float t45 = t12+t14; 00832 float t48 = a4*dq4*t5; 00833 float t49 = d5*dq2*t11*2.0; 00834 float t50 = d5*dq3*t11*2.0; 00835 float t51 = d5*dq4*t11*2.0; 00836 float t52 = dq6*t5*t6; 00837 float t54 = dq5*t10*t11; 00838 float t55 = q2+t38; 00839 float t56 = q4+t43; 00840 float t57 = q5+t43; 00841 float t61 = t12+t36; 00842 float t63 = q3+q5+t36; 00843 float t65 = t12+t43; 00844 float t67 = t13+t42; 00845 float t68 = t14+t40; 00846 float t72 = a3i*dq2*mpx4*t4*2.0; 00847 float t73 = a3i*dq3*mpx4*t4*2.0; 00848 float t74 = a3i*dq4*mpx4*t4*2.0; 00849 float t75 = a3*dq2*mpx4*t9*2.0; 00850 float t76 = a3*dq3*mpx4*t9*2.0; 00851 float t77 = a3*dq4*mpx4*t9*2.0; 00852 float t81 = d5*dq6*mpy6*t11*2.0; 00853 float t82 = t20+t38; 00854 float t98 = d5*dq2*mpy6*t11*-2.0; 00855 float t103 = q3+t14+t36; 00856 float t116 = a4*a3i*dq2*mass5*t4*2.0; 00857 float t117 = a4*a3i*dq2*mass6*t4*2.0; 00858 float t118 = a4*a3i*dq3*mass5*t4*2.0; 00859 float t119 = a4*a3i*dq3*mass6*t4*2.0; 00860 float t120 = a4*a3i*dq4*mass5*t4*2.0; 00861 float t121 = a4*a3i*dq4*mass6*t4*2.0; 00862 float t122 = a3*d5*dq2*mass6*t4*2.0; 00863 float t123 = a3*d5*dq3*mass6*t4*2.0; 00864 float t124 = a3*d5*dq4*mass6*t4*2.0; 00865 float t125 = a3*a4*dq2*mass5*t9*2.0; 00866 float t126 = a3*a4*dq2*mass6*t9*2.0; 00867 float t127 = a3*a4*dq3*mass5*t9*2.0; 00868 float t128 = a3*a4*dq3*mass6*t9*2.0; 00869 float t129 = a3*a4*dq4*mass5*t9*2.0; 00870 float t130 = a3*a4*dq4*mass6*t9*2.0; 00871 float t131 = a3i*d5*dq2*mass6*t9*2.0; 00872 float t132 = a3i*d5*dq3*mass6*t9*2.0; 00873 float t133 = a3i*d5*dq4*mass6*t9*2.0; 00874 float t134 = a4*dq2*t6*t10*2.0; 00875 float t135 = a4*dq3*t6*t10*2.0; 00876 float t136 = a4*dq4*t6*t10*2.0; 00877 float t137 = a2*dq2*mpx4*t3*t4*2.0; 00878 float t138 = a2*dq3*mpx4*t3*t4*2.0; 00879 float t139 = a2*dq4*mpx4*t3*t4*2.0; 00880 float t140 = a3*dq2*mpy6*t4*t6*2.0; 00881 float t141 = a3*dq3*mpy6*t4*t6*2.0; 00882 float t142 = a3*dq4*mpy6*t4*t6*2.0; 00883 float t143 = d5*dq5*mpy6*t5*t6*2.0; 00884 float t145 = a4*dq2*mpy6*t5*t11*2.0; 00885 float t147 = a4*dq3*mpy6*t5*t11*2.0; 00886 float t151 = a4*dq5*mpy6*t5*t11*2.0; 00887 float t152 = a4*dq6*mpy6*t6*t10*2.0; 00888 float t153 = a3i*dq2*mpy6*t4*t11*2.0; 00889 float t154 = a3i*dq2*mpy6*t6*t9*2.0; 00890 float t155 = a3i*dq3*mpy6*t4*t11*2.0; 00891 float t156 = a3i*dq3*mpy6*t6*t9*2.0; 00892 float t157 = a3i*dq4*mpy6*t4*t11*2.0; 00893 float t158 = a3i*dq4*mpy6*t6*t9*2.0; 00894 float t159 = a3i*dq6*mpy6*t4*t11*2.0; 00895 float t160 = a2*dq2*mpx4*t8*t9*2.0; 00896 float t161 = a2*dq3*mpx4*t8*t9*2.0; 00897 float t162 = a2*dq4*mpx4*t8*t9*2.0; 00898 float t163 = a3*dq2*mpy6*t9*t11*2.0; 00899 float t164 = a3*dq3*mpy6*t9*t11*2.0; 00900 float t165 = a3*dq4*mpy6*t9*t11*2.0; 00901 float t166 = a3*dq6*mpy6*t9*t11*2.0; 00902 float t175 = a3i*dq1*t2*t3*t10; 00903 float t176 = a3*dq1*t2*t8*t10; 00904 float t177 = a3*dq1*t3*t7*t10; 00905 float t188 = a3i*dq1*t7*t8*t10; 00906 float t215 = a4*dq3*mpy6*t6*t10*-2.0; 00907 float t217 = a4*dq4*mpy6*t6*t10*-2.0; 00908 float t229 = a2*a4*dq2*mass5*t3*t4*2.0; 00909 float t230 = a2*a4*dq2*mass6*t3*t4*2.0; 00910 float t231 = a2*a4*dq3*mass5*t3*t4*2.0; 00911 float t232 = a2*a4*dq3*mass6*t3*t4*2.0; 00912 float t233 = a2*a4*dq4*mass5*t3*t4*2.0; 00913 float t234 = a2*a4*dq4*mass6*t3*t4*2.0; 00914 float t235 = a2*d5*dq2*mass6*t3*t9*2.0; 00915 float t236 = a2*d5*dq2*mass6*t4*t8*2.0; 00916 float t237 = a2*d5*dq3*mass6*t3*t9*2.0; 00917 float t238 = a2*d5*dq3*mass6*t4*t8*2.0; 00918 float t239 = a2*d5*dq4*mass6*t3*t9*2.0; 00919 float t240 = a2*d5*dq4*mass6*t4*t8*2.0; 00920 float t241 = a2*a4*dq2*mass5*t8*t9*2.0; 00921 float t242 = a2*a4*dq2*mass6*t8*t9*2.0; 00922 float t243 = a2*a4*dq3*mass5*t8*t9*2.0; 00923 float t244 = a2*a4*dq3*mass6*t8*t9*2.0; 00924 float t245 = a2*a4*dq4*mass5*t8*t9*2.0; 00925 float t246 = a2*a4*dq4*mass6*t8*t9*2.0; 00926 float t250 = a3i*dq5*mpy6*t4*t5*t6*2.0; 00927 float t252 = a2*dq2*mpy6*t3*t6*t9*2.0; 00928 float t253 = a2*dq2*mpy6*t4*t6*t8*2.0; 00929 float t254 = a2*dq3*mpy6*t3*t6*t9*2.0; 00930 float t255 = a2*dq3*mpy6*t4*t6*t8*2.0; 00931 float t256 = a2*dq4*mpy6*t3*t6*t9*2.0; 00932 float t257 = a2*dq4*mpy6*t4*t6*t8*2.0; 00933 float t258 = a3*dq2*mpy6*t4*t5*t11*2.0; 00934 float t260 = a2*dq6*mpy6*t3*t4*t11*2.0; 00935 float t261 = a3*dq3*mpy6*t4*t5*t11*2.0; 00936 float t263 = a3*dq4*mpy6*t4*t5*t11*2.0; 00937 float t265 = a3*dq5*mpy6*t4*t5*t11*2.0; 00938 float t266 = a3*dq5*mpy6*t5*t6*t9*2.0; 00939 float t267 = a3*dq6*mpy6*t4*t6*t10*2.0; 00940 float t270 = a3i*dq2*mpy6*t5*t9*t11*2.0; 00941 float t273 = a3i*dq3*mpy6*t5*t9*t11*2.0; 00942 float t276 = a3i*dq4*mpy6*t5*t9*t11*2.0; 00943 float t278 = a3i*dq5*mpy6*t5*t9*t11*2.0; 00944 float t279 = a3i*dq6*mpy6*t6*t9*t10*2.0; 00945 float t281 = a2*dq6*mpy6*t8*t9*t11*2.0; 00946 float t287 = d5*dq1*t2*t3*t4*t10; 00947 float t288 = a4*dq1*t2*t3*t9*t10; 00948 float t289 = a4*dq1*t2*t4*t8*t10; 00949 float t290 = a4*dq1*t3*t4*t7*t10; 00950 float t291 = d5*dq1*t2*t8*t9*t10; 00951 float t292 = d5*dq1*t3*t7*t9*t10; 00952 float t293 = d5*dq1*t4*t7*t8*t10; 00953 float t298 = a4*dq1*t7*t8*t9*t10; 00954 float t306 = q2+q5-t12; 00955 float t307 = q2+q6-t12; 00956 float t310 = t12-t14; 00957 float t315 = a3i*dq4*mpy6*t4*t10*t11*-2.0; 00958 float t319 = a3*dq4*mpy6*t9*t10*t11*-2.0; 00959 float t335 = a3i*dq1*t2*t3*t5*t6*2.0; 00960 float t336 = a3*dq1*t2*t5*t6*t8*2.0; 00961 float t337 = a3*dq1*t3*t5*t6*t7*2.0; 00962 float t338 = a3i*dq1*t5*t6*t7*t8*2.0; 00963 float t344 = a2*dq2*mpy6*t3*t4*t10*t11*2.0; 00964 float t345 = a2*dq3*mpy6*t3*t4*t10*t11*2.0; 00965 float t346 = a2*dq4*mpy6*t3*t4*t10*t11*2.0; 00966 float t347 = a2*dq5*mpy6*t3*t5*t9*t11*2.0; 00967 float t348 = a2*dq5*mpy6*t4*t5*t8*t11*2.0; 00968 float t349 = a2*dq6*mpy6*t3*t6*t9*t10*2.0; 00969 float t350 = a2*dq6*mpy6*t4*t6*t8*t10*2.0; 00970 float t360 = a2*dq2*mpy6*t8*t9*t10*t11*2.0; 00971 float t361 = a2*dq3*mpy6*t8*t9*t10*t11*2.0; 00972 float t362 = a2*dq4*mpy6*t8*t9*t10*t11*2.0; 00973 float t364 = q6-t12+t20; 00974 float t398 = d5*dq1*t2*t3*t4*t5*t6*2.0; 00975 float t399 = a4*dq1*t2*t3*t5*t6*t9*2.0; 00976 float t400 = a4*dq1*t2*t4*t5*t6*t8*2.0; 00977 float t401 = a4*dq1*t3*t4*t5*t6*t7*2.0; 00978 float t403 = d5*dq1*t2*t5*t6*t8*t9*2.0; 00979 float t404 = d5*dq1*t3*t5*t6*t7*t9*2.0; 00980 float t405 = d5*dq1*t4*t5*t6*t7*t8*2.0; 00981 float t408 = a4*dq1*t5*t6*t7*t8*t9*2.0; 00982 float t474 = a4*dq1*mpy6*t3*t4*t5*t6*t7*-2.0; 00983 float t481 = d5*dq1*mpy6*t2*t5*t6*t8*t9*-2.0; 00984 float t44 = cos(t32); 00985 float t46 = sin(t31); 00986 float t47 = sin(t32); 00987 float t53 = cos(t45); 00988 float t58 = cos(t55); 00989 float t59 = cos(t56); 00990 float t60 = -t49; 00991 float t62 = q5+t55; 00992 float t64 = t12+t37; 00993 float t66 = q5+t56; 00994 float t69 = sin(t55); 00995 float t70 = sin(t56); 00996 float t71 = sin(t57); 00997 float t78 = mpy6*t49; 00998 float t79 = mpy6*t50; 00999 float t80 = mpy6*t51; 01000 float t83 = t20+t39; 01001 float t84 = a2*dq1*mpx4*t29; 01002 float t85 = -t72; 01003 float t86 = -t73; 01004 float t87 = -t74; 01005 float t88 = cos(t61); 01006 float t90 = cos(t63); 01007 float t93 = cos(t67); 01008 float t94 = cos(t68); 01009 float t95 = -t75; 01010 float t96 = -t76; 01011 float t97 = -t77; 01012 float t99 = -t81; 01013 float t100 = q5+t61; 01014 float t101 = t31+t36; 01015 float t102 = t14+t55; 01016 float t104 = t31+t37; 01017 float t105 = t12+t57; 01018 float t106 = t31+t42; 01019 float t107 = sin(t61); 01020 float t109 = sin(t63); 01021 float t111 = sin(t65); 01022 float t113 = sin(t67); 01023 float t114 = sin(t68); 01024 float t115 = -t54; 01025 float t144 = d5*mpy6*t52*2.0; 01026 float t146 = mpy6*t134; 01027 float t148 = mpy6*t135; 01028 float t149 = mpy6*t11*t48*2.0; 01029 float t150 = mpy6*t136; 01030 float t167 = cos(t82); 01031 float t170 = sin(t82); 01032 float t172 = a2*a4*dq1*mass5*t29; 01033 float t173 = a2*a4*dq1*mass6*t29; 01034 float t174 = a2*d5*dq1*mass6*t33; 01035 float t179 = -t116; 01036 float t180 = -t117; 01037 float t181 = -t118; 01038 float t182 = -t119; 01039 float t183 = -t120; 01040 float t184 = -t121; 01041 float t185 = -t122; 01042 float t186 = -t123; 01043 float t187 = -t124; 01044 float t190 = -t125; 01045 float t191 = -t126; 01046 float t192 = -t127; 01047 float t193 = -t128; 01048 float t194 = -t129; 01049 float t195 = -t130; 01050 float t196 = -t133; 01051 float t197 = -t135; 01052 float t198 = -t136; 01053 float t202 = cos(t103); 01054 float t205 = t36+t45; 01055 float t206 = t37+t45; 01056 float t209 = sin(t103); 01057 float t212 = -t142; 01058 float t214 = -t147; 01059 float t218 = -t151; 01060 float t219 = -t152; 01061 float t220 = -t154; 01062 float t221 = -t156; 01063 float t222 = -t158; 01064 float t223 = -t159; 01065 float t226 = -t160; 01066 float t227 = -t166; 01067 float t251 = a3i*mpy6*t4*t52*2.0; 01068 float t259 = t10*t140; 01069 float t262 = t10*t141; 01070 float t264 = t10*t142; 01071 float t268 = a3*mpy6*t9*t52*2.0; 01072 float t269 = t10*t153; 01073 float t271 = t10*t154; 01074 float t272 = t10*t155; 01075 float t274 = t10*t156; 01076 float t275 = t10*t157; 01077 float t277 = t10*t158; 01078 float t280 = t10*t163; 01079 float t282 = t10*t164; 01080 float t283 = t10*t165; 01081 float t295 = -t235; 01082 float t296 = -t236; 01083 float t297 = -t175; 01084 float t299 = -t241; 01085 float t300 = -t242; 01086 float t301 = -t177; 01087 float t304 = -t188; 01088 float t308 = t55+t57; 01089 float t309 = q6+t42+t55; 01090 float t312 = -t260; 01091 float t313 = -t265; 01092 float t314 = -t267; 01093 float t316 = -t278; 01094 float t317 = -t279; 01095 float t322 = cos(t306); 01096 float t323 = cos(t307); 01097 float t326 = cos(t310); 01098 float t327 = q2-t310; 01099 float t328 = t57+t61; 01100 float t332 = sin(t306); 01101 float t351 = mpy6*t11*t175*2.0; 01102 float t352 = mpy6*t338; 01103 float t356 = mpy6*t11*t176*2.0; 01104 float t357 = mpy6*t11*t177*2.0; 01105 float t363 = mpy6*t11*t188*2.0; 01106 float t365 = q6-t12+t82; 01107 float t366 = -t12+t364; 01108 float t369 = -t335; 01109 float t372 = -t337; 01110 float t373 = -t290; 01111 float t374 = -t338; 01112 float t376 = -t291; 01113 float t387 = mpy6*t335; 01114 float t388 = mpy6*t336; 01115 float t389 = mpy6*t337; 01116 float t393 = -t344; 01117 float t411 = t20-t310; 01118 float t414 = t82-t310; 01119 float t416 = sin(t364); 01120 float t419 = mpy6*t11*t298*2.0; 01121 float t434 = -t401; 01122 float t445 = -t403; 01123 float t449 = mpy6*t398; 01124 float t450 = mpy6*t399; 01125 float t451 = mpy6*t400; 01126 float t452 = mpy6*t401; 01127 float t454 = mpy6*t11*t287*2.0; 01128 float t455 = mpy6*t403; 01129 float t456 = mpy6*t404; 01130 float t457 = mpy6*t405; 01131 float t459 = mpy6*t11*t288*2.0; 01132 float t460 = mpy6*t11*t289*2.0; 01133 float t461 = mpy6*t11*t290*2.0; 01134 float t462 = mpy6*t408; 01135 float t463 = mpy6*t11*t291*2.0; 01136 float t464 = mpy6*t11*t292*2.0; 01137 float t465 = mpy6*t11*t293*2.0; 01138 float t89 = cos(t62); 01139 float t91 = cos(t64); 01140 float t92 = cos(t66); 01141 float t108 = sin(t62); 01142 float t110 = sin(t64); 01143 float t112 = sin(t66); 01144 float t168 = cos(t83); 01145 float t169 = q3+t64; 01146 float t171 = sin(t83); 01147 float t178 = a3i*t58; 01148 float t189 = a3*t69; 01149 float t199 = cos(t100); 01150 float t200 = cos(t101); 01151 float t201 = cos(t102); 01152 float t203 = cos(t105); 01153 float t204 = cos(t106); 01154 float t207 = sin(t100); 01155 float t208 = sin(t102); 01156 float t210 = sin(t104); 01157 float t211 = d5*t88; 01158 float t213 = a4*t107; 01159 float t216 = -t149; 01160 float t224 = cos(t205); 01161 float t225 = cos(t206); 01162 float t228 = sin(t205); 01163 float t248 = q3+t104; 01164 float t285 = q3+t206; 01165 float t294 = -t174; 01166 float t303 = t12+t64; 01167 float t320 = t31+t64; 01168 float t324 = cos(t308); 01169 float t325 = cos(t309); 01170 float t329 = t42+t101; 01171 float t330 = t57+t64; 01172 float t331 = t42+t104; 01173 float t333 = sin(t308); 01174 float t334 = sin(t309); 01175 float t339 = a1*dq1*mpy3*t58*2.0; 01176 float t340 = a1*dq1*mpx3*t69*2.0; 01177 float t342 = (a2*dq1*mpy6*t46)/2.0; 01178 float t358 = a1*dq1*mpx4*t107*2.0; 01179 float t359 = (a2*dq1*mpy6*t53)/4.0; 01180 float t377 = t45+t64; 01181 float t379 = cos(t327); 01182 float t380 = cos(t328); 01183 float t384 = sin(t327); 01184 float t385 = sin(t328); 01185 float t409 = cos(t365); 01186 float t410 = cos(t366); 01187 float t415 = -t12+t411; 01188 float t417 = sin(t365); 01189 float t418 = sin(t366); 01190 float t420 = cos(t411); 01191 float t423 = cos(t414); 01192 float t425 = (a2*dq1*mpy6*t111)/2.0; 01193 float t428 = sin(t414); 01194 float t433 = t52+t115; 01195 float t466 = -t419; 01196 float t467 = t64+t105; 01197 float t468 = t64+t106; 01198 float t478 = a1*dq1*mpy6*t323; 01199 float t480 = -t454; 01200 float t482 = -t459; 01201 float t483 = -t460; 01202 float t484 = -t464; 01203 float t485 = -t465; 01204 float t519 = (a2*dq1*mpy6*t326)/4.0; 01205 float t545 = (a2*dq1*mpy6*t416)/2.0; 01206 float t247 = cos(t169); 01207 float t249 = sin(t169); 01208 float t284 = cos(t248); 01209 float t286 = sin(t248); 01210 float t302 = -t178; 01211 float t305 = -t189; 01212 float t311 = a2*dq1*mpx4*t91; 01213 float t318 = cos(t303); 01214 float t321 = sin(t303); 01215 float t341 = cos(t285); 01216 float t343 = sin(t285); 01217 float t353 = a1*dq1*mass4*t189*2.0; 01218 float t354 = a1*dq1*mass5*t189*2.0; 01219 float t355 = a1*dq1*mass6*t189*2.0; 01220 float t367 = a2*a4*dq1*mass5*t91; 01221 float t368 = a2*a4*dq1*mass6*t91; 01222 float t370 = a1*dq1*mpy6*t200; 01223 float t371 = a2*d5*dq1*mass6*t110; 01224 float t375 = cos(t320); 01225 float t378 = sin(t320); 01226 float t381 = cos(t329); 01227 float t382 = cos(t330); 01228 float t383 = cos(t331); 01229 float t386 = sin(t329); 01230 float t390 = a1*dq1*mass4*t178*2.0; 01231 float t391 = a1*dq1*mass5*t178*2.0; 01232 float t392 = a1*dq1*mass6*t178*2.0; 01233 float t395 = cos(t377); 01234 float t396 = -t358; 01235 float t397 = sin(t377); 01236 float t402 = a1*dq1*mass6*t211*2.0; 01237 float t406 = a1*dq1*mass5*t213*2.0; 01238 float t407 = a1*dq1*mass6*t213*2.0; 01239 float t412 = t57+t169; 01240 float t413 = t42+t248; 01241 float t424 = cos(t415); 01242 float t429 = sin(t415); 01243 float t430 = a3*a3i*dq1*mass4*t168*2.0; 01244 float t431 = a3*a3i*dq1*mass5*t168*2.0; 01245 float t432 = a3*a3i*dq1*mass6*t168*2.0; 01246 float t435 = dq1*mass4*t16*t171; 01247 float t436 = dq1*mass5*t16*t171; 01248 float t437 = dq1*mass6*t16*t171; 01249 float t438 = dq1*mass4*t18*t171; 01250 float t439 = dq1*mass5*t18*t171; 01251 float t440 = dq1*mass6*t18*t171; 01252 float t469 = (a2*dq1*mpy6*t203)/4.0; 01253 float t470 = (a2*dq1*mpy6*t204)/4.0; 01254 float t471 = (a2*dq1*mpy6*t210)/2.0; 01255 float t488 = cos(t467); 01256 float t489 = cos(t468); 01257 float t492 = sin(t467); 01258 float t493 = sin(t468); 01259 float t495 = (a2*dq1*mpy6*t225)/4.0; 01260 float t496 = (d5*dq2*mpy6*t224)/2.0; 01261 float t497 = (d5*dq3*mpy6*t224)/2.0; 01262 float t498 = (d5*dq4*mpy6*t224)/2.0; 01263 float t499 = (a1*dq1*mpy6*t228)/2.0; 01264 float t500 = (a4*dq2*mpy6*t228)/2.0; 01265 float t501 = (a4*dq3*mpy6*t228)/2.0; 01266 float t502 = (a4*dq4*mpy6*t228)/2.0; 01267 float t506 = a3*dq1*mpy6*t409; 01268 float t507 = a4*dq1*mpy6*t410; 01269 float t509 = a3i*dq1*mpy6*t417; 01270 float t510 = d5*dq1*mpy6*t418; 01271 float t524 = (d5*dq2*mpy6*t379)/2.0; 01272 float t525 = (d5*dq2*mpy6*t380)/2.0; 01273 float t527 = (d5*dq3*mpy6*t379)/2.0; 01274 float t528 = (d5*dq3*mpy6*t380)/2.0; 01275 float t530 = (d5*dq4*mpy6*t379)/2.0; 01276 float t531 = (d5*dq4*mpy6*t380)/2.0; 01277 float t533 = (a1*dq1*mpy6*t384)/2.0; 01278 float t534 = (a1*dq1*mpy6*t385)/2.0; 01279 float t536 = (a4*dq2*mpy6*t384)/2.0; 01280 float t537 = (a4*dq2*mpy6*t385)/2.0; 01281 float t539 = (a4*dq3*mpy6*t384)/2.0; 01282 float t540 = (a4*dq3*mpy6*t385)/2.0; 01283 float t542 = (a4*dq4*mpy6*t384)/2.0; 01284 float t543 = (a4*dq4*mpy6*t385)/2.0; 01285 float t551 = (a2*dq1*mpy6*t420)/4.0; 01286 float t554 = (a3i*dq1*mpy6*t423)/2.0; 01287 float t559 = (a3*dq1*mpy6*t428)/2.0; 01288 float t394 = -t311; 01289 float t421 = cos(t412); 01290 float t422 = cos(t413); 01291 float t426 = sin(t412); 01292 float t427 = sin(t413); 01293 float t441 = a3i*dq1*mpx4*t247*2.0; 01294 float t442 = -t367; 01295 float t443 = -t368; 01296 float t444 = -t402; 01297 float t446 = a3*dq1*mpx4*t249*2.0; 01298 float t447 = -t406; 01299 float t448 = -t407; 01300 float t453 = a3*dq1*mpy6*t284; 01301 float t458 = a3i*dq1*mpy6*t286; 01302 float t472 = a4*a3i*dq1*mass5*t247*2.0; 01303 float t473 = a4*a3i*dq1*mass6*t247*2.0; 01304 float t475 = a3*d5*dq1*mass6*t247*2.0; 01305 float t476 = a3*a4*dq1*mass5*t249*2.0; 01306 float t477 = a3*a4*dq1*mass6*t249*2.0; 01307 float t479 = a3i*d5*dq1*mass6*t249*2.0; 01308 float t486 = a4*dq1*mpy6*t375; 01309 float t487 = d5*dq1*mpy6*t378; 01310 float t490 = -t469; 01311 float t491 = -t470; 01312 float t494 = -t471; 01313 float t503 = a4*d5*dq1*mass6*t318*2.0; 01314 float t504 = dq1*mass5*t17*t321; 01315 float t505 = dq1*mass6*t17*t321; 01316 float t508 = dq1*mass6*t19*t321; 01317 float t511 = -t495; 01318 float t512 = -t496; 01319 float t513 = -t499; 01320 float t514 = -t500; 01321 float t516 = (a3i*dq1*mpy6*t341)/2.0; 01322 float t518 = (a3*dq1*mpy6*t343)/2.0; 01323 float t522 = (a2*dq1*mpy6*t382)/4.0; 01324 float t523 = (a2*dq1*mpy6*t383)/4.0; 01325 float t526 = (d5*dq2*mpy6*t381)/2.0; 01326 float t529 = (d5*dq3*mpy6*t381)/2.0; 01327 float t532 = (d5*dq4*mpy6*t381)/2.0; 01328 float t535 = (a1*dq1*mpy6*t386)/2.0; 01329 float t538 = (a4*dq2*mpy6*t386)/2.0; 01330 float t541 = (a4*dq3*mpy6*t386)/2.0; 01331 float t544 = (a4*dq4*mpy6*t386)/2.0; 01332 float t547 = -t536; 01333 float t549 = (d5*dq1*mpy6*t395)/2.0; 01334 float t550 = (a4*dq1*mpy6*t397)/2.0; 01335 float t556 = (d5*dq1*mpy6*t424)/2.0; 01336 float t561 = (a4*dq1*mpy6*t429)/2.0; 01337 float t562 = -t551; 01338 float t564 = (d5*dq1*mpy6*t488)/2.0; 01339 float t565 = (d5*dq1*mpy6*t489)/2.0; 01340 float t566 = (a4*dq1*mpy6*t492)/2.0; 01341 float t567 = (a4*dq1*mpy6*t493)/2.0; 01342 float t515 = -t503; 01343 float t517 = -t487; 01344 float t520 = -t504; 01345 float t521 = -t505; 01346 float t546 = -t526; 01347 float t548 = -t538; 01348 float t552 = (a3i*dq1*mpy6*t421)/2.0; 01349 float t553 = (a3i*dq1*mpy6*t422)/2.0; 01350 float t555 = -t549; 01351 float t557 = (a3*dq1*mpy6*t426)/2.0; 01352 float t558 = (a3*dq1*mpy6*t427)/2.0; 01353 float t560 = -t550; 01354 float t563 = -t556; 01355 A0(0,0) = a1*dq2*mass4*t178*-2.0-a1*dq2*mass5*t178*2.0+a1*dq3*mass4*t178*2.0-a1*dq2*mass6*t178*2.0+a1*dq3*mass5*t178*2.0+a1*dq3*mass6*t178*2.0-a1*dq2*mass4*t189*2.0-a1*dq2*mass5*t189*2.0+a1*dq3*mass4*t189*2.0-a1*dq2*mass6*t189*2.0+a1*dq3*mass5*t189*2.0+a1*dq3*mass6*t189*2.0+a1*dq2*mass6*t211*2.0+a1*dq2*mass5*t213*2.0-a1*dq3*mass6*t211*2.0+a1*dq2*mass6*t213*2.0-a1*dq3*mass5*t213*2.0-a1*dq4*mass6*t211*2.0-a1*dq3*mass6*t213*2.0-a1*dq4*mass5*t213*2.0-a1*dq4*mass6*t213*2.0+a1*dq2*mpx2*t2*2.0+a2*dq3*mpx3*t3-a3*dq4*mpx4*t9+a2*dq3*mpx4*t29+a2*dq4*mpx4*t29-a1*dq2*mpx3*t69*2.0+a1*dq3*mpx3*t69*2.0+a2*dq2*mpx4*t91*2.0-a2*dq3*mpx4*t91-a2*dq4*mpx4*t91+a1*dq2*mpx4*t107*2.0-a1*dq3*mpx4*t107*2.0-a1*dq4*mpx4*t107*2.0+a2*dq2*mpx3*t167*2.0-a2*dq3*mpx3*t167+a3*dq2*mpx4*t249*2.0-a3*dq3*mpx4*t249*2.0-a3*dq4*mpx4*t249+a1*dq2*mpy2*t7*2.0-a2*dq3*mpy3*t8+(a3*dq4*mpy6*t30)/2.0+(a3*dq6*mpy6*t30)/2.0-(a4*dq5*mpy6*t35)/2.0-(a4*dq6*mpy6*t35)/2.0+(a2*dq3*mpy6*t46)/2.0+(a2*dq4*mpy6*t46)/2.0+(a2*dq6*mpy6*t46)/2.0-(a3*dq4*mpy6*t47)/4.0-(a3*dq5*mpy6*t47)/4.0-(a3*dq6*mpy6*t47)/4.0+a1*dq2*mpy3*t58*2.0+(a2*dq3*mpy6*t53)/4.0-a1*dq3*mpy3*t58*2.0+(a2*dq4*mpy6*t53)/4.0+(a2*dq5*mpy6*t53)/4.0+(a2*dq6*mpy6*t53)/4.0+(a3*dq4*mpy6*t59)/2.0-(a3*dq6*mpy6*t59)/2.0+(a4*dq5*mpy6*t71)/2.0-(a4*dq6*mpy6*t71)/2.0+(a2*dq3*mpy6*t111)/2.0+(a2*dq4*mpy6*t111)/2.0-(a2*dq6*mpy6*t111)/2.0+(a3*dq4*mpy6*t112)/4.0+(a3*dq4*mpy6*t113)/4.0+(a3*dq5*mpy6*t112)/4.0+(a3*dq4*mpy6*t114)/4.0-(a3*dq5*mpy6*t113)/4.0-(a3*dq6*mpy6*t112)/4.0-(a3*dq5*mpy6*t114)/4.0+(a3*dq6*mpy6*t113)/4.0-(a3*dq6*mpy6*t114)/4.0+a2*dq2*mpy3*t170*2.0-a2*dq3*mpy3*t170-a1*dq2*mpy6*t200+a1*dq3*mpy6*t200+a1*dq4*mpy6*t200+a1*dq6*mpy6*t200-(a2*dq3*mpy6*t203)/4.0-(a2*dq3*mpy6*t204)/4.0-(a2*dq4*mpy6*t203)/4.0-(a2*dq4*mpy6*t204)/4.0-(a2*dq5*mpy6*t203)/4.0+(a2*dq5*mpy6*t204)/4.0+(a2*dq6*mpy6*t203)/4.0-(a2*dq6*mpy6*t204)/4.0+a2*dq2*mpy6*t210-(a2*dq3*mpy6*t210)/2.0-(a2*dq4*mpy6*t210)/2.0-(a2*dq6*mpy6*t210)/2.0+(a2*dq2*mpy6*t225)/2.0-(a2*dq3*mpy6*t225)/4.0+(a1*dq2*mpy6*t228)/2.0-(a2*dq4*mpy6*t225)/4.0-(a1*dq3*mpy6*t228)/2.0-(a2*dq5*mpy6*t225)/4.0-(a1*dq4*mpy6*t228)/2.0-(a2*dq6*mpy6*t225)/4.0-(a1*dq5*mpy6*t228)/2.0-(a1*dq6*mpy6*t228)/2.0-a3*dq2*mpy6*t284+a3*dq3*mpy6*t284+(a3*dq4*mpy6*t284)/2.0+(a3*dq6*mpy6*t284)/2.0-a1*dq2*mpy6*t323+a1*dq3*mpy6*t323+a1*dq4*mpy6*t323-a1*dq6*mpy6*t323+(a2*dq3*mpy6*t326)/4.0+(a2*dq4*mpy6*t326)/4.0-(a2*dq5*mpy6*t326)/4.0-(a2*dq6*mpy6*t326)/4.0+(a3*dq2*mpy6*t343)/2.0-(a3*dq3*mpy6*t343)/2.0-(a3*dq4*mpy6*t343)/4.0-(a3*dq5*mpy6*t343)/4.0-(a3*dq6*mpy6*t343)/4.0-a4*dq2*mpy6*t375+a4*dq3*mpy6*t375+a4*dq4*mpy6*t375+(a4*dq6*mpy6*t375)/2.0-(a2*dq2*mpy6*t382)/2.0-(a1*dq2*mpy6*t384)/2.0-(a2*dq2*mpy6*t383)/2.0+(a2*dq3*mpy6*t382)/4.0-(a1*dq2*mpy6*t385)/2.0+(a1*dq3*mpy6*t384)/2.0+(a2*dq3*mpy6*t383)/4.0+(a2*dq4*mpy6*t382)/4.0-(a1*dq2*mpy6*t386)/2.0+(a1*dq3*mpy6*t385)/2.0+(a1*dq4*mpy6*t384)/2.0+(a2*dq4*mpy6*t383)/4.0+(a2*dq5*mpy6*t382)/4.0+(a1*dq3*mpy6*t386)/2.0+(a1*dq4*mpy6*t385)/2.0-(a1*dq5*mpy6*t384)/2.0-(a2*dq5*mpy6*t383)/4.0-(a2*dq6*mpy6*t382)/4.0+(a1*dq4*mpy6*t386)/2.0+(a1*dq5*mpy6*t385)/2.0-(a1*dq6*mpy6*t384)/2.0+(a2*dq6*mpy6*t383)/4.0-(a1*dq5*mpy6*t386)/2.0-(a1*dq6*mpy6*t385)/2.0+(a1*dq6*mpy6*t386)/2.0+(a4*dq2*mpy6*t397)/2.0-(a4*dq3*mpy6*t397)/2.0-(a4*dq4*mpy6*t397)/2.0-(a4*dq5*mpy6*t397)/4.0-(a4*dq6*mpy6*t397)/4.0-a3*dq2*mpy6*t409+a3*dq3*mpy6*t409+(a3*dq4*mpy6*t409)/2.0-a4*dq2*mpy6*t410+a4*dq3*mpy6*t410-(a3*dq6*mpy6*t409)/2.0+a4*dq4*mpy6*t410-a2*dq2*mpy6*t416-(a4*dq6*mpy6*t410)/2.0+(a2*dq3*mpy6*t416)/2.0+(a2*dq4*mpy6*t416)/2.0+(a2*dq2*mpy6*t420)/2.0-(a2*dq6*mpy6*t416)/2.0-(a2*dq3*mpy6*t420)/4.0-(a2*dq4*mpy6*t420)/4.0+(a2*dq5*mpy6*t420)/4.0+(a2*dq6*mpy6*t420)/4.0-(a3*dq2*mpy6*t426)/2.0-(a3*dq2*mpy6*t427)/2.0+(a3*dq3*mpy6*t426)/2.0-(a3*dq2*mpy6*t428)/2.0+(a3*dq3*mpy6*t427)/2.0+(a3*dq4*mpy6*t426)/4.0+(a3*dq3*mpy6*t428)/2.0+(a3*dq4*mpy6*t427)/4.0+(a3*dq5*mpy6*t426)/4.0+(a3*dq4*mpy6*t428)/4.0-(a3*dq5*mpy6*t427)/4.0-(a3*dq6*mpy6*t426)/4.0-(a4*dq2*mpy6*t429)/2.0-(a3*dq5*mpy6*t428)/4.0+(a3*dq6*mpy6*t427)/4.0+(a4*dq3*mpy6*t429)/2.0-(a3*dq6*mpy6*t428)/4.0+(a4*dq4*mpy6*t429)/2.0-(a4*dq5*mpy6*t429)/4.0-(a4*dq6*mpy6*t429)/4.0-(a4*dq2*mpy6*t492)/2.0-(a4*dq2*mpy6*t493)/2.0+(a4*dq3*mpy6*t492)/2.0+(a4*dq3*mpy6*t493)/2.0+(a4*dq4*mpy6*t492)/2.0+(a4*dq4*mpy6*t493)/2.0+(a4*dq5*mpy6*t492)/4.0-(a4*dq5*mpy6*t493)/4.0-(a4*dq6*mpy6*t492)/4.0+(a4*dq6*mpy6*t493)/4.0-a3i*dq4*mpx4*t4-a3i*dq2*mpx4*t247*2.0+a3i*dq3*mpx4*t247*2.0+a3i*dq4*mpx4*t247-(a3i*dq4*mpy6*t34)/2.0-(a3i*dq6*mpy6*t34)/2.0-(a3i*dq4*mpy6*t44)/4.0-(a3i*dq5*mpy6*t44)/4.0-(a3i*dq6*mpy6*t44)/4.0-(a3i*dq4*mpy6*t70)/2.0+(a3i*dq6*mpy6*t70)/2.0+(a3i*dq4*mpy6*t92)/4.0+(a3i*dq4*mpy6*t93)/4.0+(a3i*dq5*mpy6*t92)/4.0-(a3i*dq4*mpy6*t94)/4.0-(a3i*dq5*mpy6*t93)/4.0-(a3i*dq6*mpy6*t92)/4.0+(a3i*dq5*mpy6*t94)/4.0+(a3i*dq6*mpy6*t93)/4.0+(a3i*dq6*mpy6*t94)/4.0-a3i*dq2*mpy6*t286+a3i*dq3*mpy6*t286+(a3i*dq4*mpy6*t286)/2.0+(a3i*dq6*mpy6*t286)/2.0-(a3i*dq2*mpy6*t341)/2.0+(a3i*dq3*mpy6*t341)/2.0+(a3i*dq4*mpy6*t341)/4.0+(a3i*dq5*mpy6*t341)/4.0+(a3i*dq6*mpy6*t341)/4.0+a3i*dq2*mpy6*t417-a3i*dq3*mpy6*t417-(a3i*dq4*mpy6*t417)/2.0+(a3i*dq2*mpy6*t421)/2.0+(a3i*dq6*mpy6*t417)/2.0+(a3i*dq2*mpy6*t422)/2.0-(a3i*dq3*mpy6*t421)/2.0-(a3i*dq2*mpy6*t423)/2.0-(a3i*dq3*mpy6*t422)/2.0-(a3i*dq4*mpy6*t421)/4.0+(a3i*dq3*mpy6*t423)/2.0-(a3i*dq4*mpy6*t422)/4.0-(a3i*dq5*mpy6*t421)/4.0+(a3i*dq4*mpy6*t423)/4.0+(a3i*dq5*mpy6*t422)/4.0+(a3i*dq6*mpy6*t421)/4.0-(a3i*dq5*mpy6*t423)/4.0-(a3i*dq6*mpy6*t422)/4.0-(a3i*dq6*mpy6*t423)/4.0+d5*dq6*mpy6*t11+d5*dq2*mpy6*t378-d5*dq3*mpy6*t378-d5*dq4*mpy6*t378-(d5*dq6*mpy6*t378)/2.0+(d5*dq2*mpy6*t395)/2.0-(d5*dq3*mpy6*t395)/2.0-(d5*dq4*mpy6*t395)/2.0-(d5*dq5*mpy6*t395)/4.0-(d5*dq6*mpy6*t395)/4.0-d5*dq2*mpy6*t418+d5*dq3*mpy6*t418+d5*dq4*mpy6*t418-(d5*dq6*mpy6*t418)/2.0+(d5*dq2*mpy6*t424)/2.0-(d5*dq3*mpy6*t424)/2.0-(d5*dq4*mpy6*t424)/2.0+(d5*dq5*mpy6*t424)/4.0+(d5*dq6*mpy6*t424)/4.0-(d5*dq2*mpy6*t488)/2.0-(d5*dq2*mpy6*t489)/2.0+(d5*dq3*mpy6*t488)/2.0+(d5*dq3*mpy6*t489)/2.0+(d5*dq4*mpy6*t488)/2.0+(d5*dq4*mpy6*t489)/2.0+(d5*dq5*mpy6*t488)/4.0-(d5*dq5*mpy6*t489)/4.0-(d5*dq6*mpy6*t488)/4.0+(d5*dq6*mpy6*t489)/4.0+dq2*mass3*t15*t26+dq2*mass4*t15*t26+dq2*mass5*t15*t26+dq2*mass6*t15*t26-dq2*mass4*t16*t171-dq2*mass5*t16*t171+dq3*mass4*t16*t171+dq2*mass4*t18*t171-dq2*mass6*t16*t171+dq3*mass5*t16*t171+dq2*mass5*t18*t171-dq3*mass4*t18*t171+dq3*mass6*t16*t171+dq2*mass6*t18*t171-dq3*mass5*t18*t171-dq3*mass6*t18*t171+dq2*mass5*t17*t321+dq2*mass6*t17*t321-dq3*mass5*t17*t321-dq3*mass6*t17*t321-dq4*mass5*t17*t321-dq2*mass6*t19*t321-dq4*mass6*t17*t321+dq3*mass6*t19*t321+dq4*mass6*t19*t321+a1*a2*dq2*mass3*t2*2.0+a1*a2*dq2*mass4*t2*2.0+a1*a2*dq2*mass5*t2*2.0+a1*a2*dq2*mass6*t2*2.0+a2*a3*dq3*mass4*t3+a2*a3*dq3*mass5*t3+a2*a3*dq3*mass6*t3-a3*a4*dq4*mass5*t9-a3*a4*dq4*mass6*t9+a2*a4*dq3*mass5*t29+a2*a4*dq3*mass6*t29+a2*a4*dq4*mass5*t29+a2*a4*dq4*mass6*t29+a2*a4*dq2*mass5*t91*2.0+a2*a4*dq2*mass6*t91*2.0-a2*a4*dq3*mass5*t91-a2*a4*dq3*mass6*t91-a2*a4*dq4*mass5*t91-a2*a4*dq4*mass6*t91+a2*a3*dq2*mass4*t167*2.0+a2*a3*dq2*mass5*t167*2.0-a2*a3*dq3*mass4*t167+a2*a3*dq2*mass6*t167*2.0-a2*a3*dq3*mass5*t167-a2*a3*dq3*mass6*t167+a3*a4*dq2*mass5*t249*2.0+a3*a4*dq2*mass6*t249*2.0-a3*a4*dq3*mass5*t249*2.0-a3*a4*dq3*mass6*t249*2.0-a3*a4*dq4*mass5*t249-a3*a4*dq4*mass6*t249+a2*a3i*dq3*mass4*t8-a4*a3i*dq4*mass5*t4+a2*a3i*dq3*mass5*t8-a4*a3i*dq4*mass6*t4+a2*a3i*dq3*mass6*t8-a3*a3i*dq2*mass4*t168*2.0-a2*a3i*dq2*mass4*t170*2.0-a3*a3i*dq2*mass5*t168*2.0+a3*a3i*dq3*mass4*t168*2.0-a2*a3i*dq2*mass5*t170*2.0+a2*a3i*dq3*mass4*t170-a3*a3i*dq2*mass6*t168*2.0+a3*a3i*dq3*mass5*t168*2.0-a2*a3i*dq2*mass6*t170*2.0+a2*a3i*dq3*mass5*t170+a3*a3i*dq3*mass6*t168*2.0+a2*a3i*dq3*mass6*t170-a4*a3i*dq2*mass5*t247*2.0-a4*a3i*dq2*mass6*t247*2.0+a4*a3i*dq3*mass5*t247*2.0+a4*a3i*dq3*mass6*t247*2.0+a4*a3i*dq4*mass5*t247+a4*a3i*dq4*mass6*t247-a3*d5*dq4*mass6*t4-a2*d5*dq3*mass6*t33-a2*d5*dq4*mass6*t33-a2*d5*dq2*mass6*t110*2.0+a2*d5*dq3*mass6*t110+a2*d5*dq4*mass6*t110+a3*d5*dq2*mass6*t247*2.0-a3*d5*dq3*mass6*t247*2.0-a3*d5*dq4*mass6*t247+a4*d5*dq2*mass6*t318*2.0-a4*d5*dq3*mass6*t318*2.0-a4*d5*dq4*mass6*t318*2.0+a3i*d5*dq4*mass6*t9+a3i*d5*dq2*mass6*t249*2.0-a3i*d5*dq3*mass6*t249*2.0-a3i*d5*dq4*mass6*t249; 01356 A0(0,1) = t311*2.0+t339-t340-t353-t354-t355+t358+t367*2.0+t368*2.0-t370-t371*2.0-t390-t391-t392+t402+t406+t407-t430-t431-t432-t435-t436-t437+t438+t439+t440-t441+t446-t453-t458-t472-t473+t475+t476+t477-t478+t479-t486+t487-t497-t498+t499-t501-t502+t503+t504+t505-t506-t507-t508+t509-t510-t516+t518+t527+t528-t529+t530+t531-t532-t533-t534-t535-t539+t540-t541-t542+t543-t544+t549+t550+t552+t553-t554+t556-t557-t558-t559-t561-t564-t565-t566-t567+a1*dq1*mpx2*t2*2.0+a2*dq1*mpx3*t167*2.0+a1*dq1*mpy2*t7*2.0+a2*dq1*mpy3*t170*2.0+a2*dq1*mpy6*t210-(a3*dq3*mpy6*t208)/2.0-(a3*dq3*mpy6*t209)/2.0+(a2*dq1*mpy6*t225)/2.0+(a3*dq3*mpy6*t333)/2.0-(a3*dq3*mpy6*t334)/2.0-(a2*dq1*mpy6*t382)/2.0-(a2*dq1*mpy6*t383)/2.0-a2*dq1*mpy6*t416+(a2*dq1*mpy6*t420)/2.0-(a3i*dq3*mpy6*t201)/2.0+(a3i*dq3*mpy6*t202)/2.0+(a3i*dq3*mpy6*t324)/2.0-(a3i*dq3*mpy6*t325)/2.0+dq1*mass3*t15*t26+dq1*mass4*t15*t26+dq1*mass5*t15*t26+dq1*mass6*t15*t26+a1*a2*dq1*mass3*t2*2.0+a1*a2*dq1*mass4*t2*2.0+a1*a2*dq1*mass5*t2*2.0+a1*a2*dq1*mass6*t2*2.0+a2*a3*dq1*mass4*t167*2.0+a2*a3*dq1*mass5*t167*2.0+a2*a3*dq1*mass6*t167*2.0-a2*a3i*dq1*mass4*t170*2.0-a2*a3i*dq1*mass5*t170*2.0-a2*a3i*dq1*mass6*t170*2.0; 01357 A0(0,2) = t84+t172+t173+t294-t339+t340+t342+t353+t354+t355+t359+t370+t371+t390+t391+t392+t394+t396+t425+t430+t431+t432+t435+t436+t437-t438-t439-t440+t441+t442+t443+t444-t446+t447+t448+t453+t458+t472+t473-t475-t476-t477+t478-t479+t486+t490+t491+t494+t498+t502+t506+t507+t508-t509+t510+t511+t512+t513+t514+t515+t516+t517-t518+t519+t520+t521+t522+t523+t524+t525-t530-t531+t532+t533+t534+t535+t537+t542-t543+t544+t545+t546+t547+t548-t552-t553+t554+t555+t557+t558+t559+t560+t561+t562+t563+t564+t565+t566+t567+a2*dq1*mpx3*t3-a2*dq1*mpx3*t167-a2*dq1*mpy3*t8-a2*dq1*mpy3*t170-(a3*dq2*mpy6*t208)/2.0-(a3*dq2*mpy6*t209)/2.0+(a3*dq2*mpy6*t333)/2.0-(a3*dq2*mpy6*t334)/2.0-(a3i*dq2*mpy6*t201)/2.0+(a3i*dq2*mpy6*t202)/2.0+(a3i*dq2*mpy6*t324)/2.0-(a3i*dq2*mpy6*t325)/2.0+a2*a3*dq1*mass4*t3+a2*a3*dq1*mass5*t3+a2*a3*dq1*mass6*t3-a2*a3*dq1*mass4*t167-a2*a3*dq1*mass5*t167-a2*a3*dq1*mass6*t167+a2*a3i*dq1*mass4*t8+a2*a3i*dq1*mass5*t8+a2*a3i*dq1*mass6*t8+a2*a3i*dq1*mass4*t170+a2*a3i*dq1*mass5*t170+a2*a3i*dq1*mass6*t170; 01358 A0(0,3) = t84+t172+t173+t294+t342+t359+t370+t371+t394+t396+t425+t442+t443+t444+t447+t448+t453/2.0+t458/2.0+t478+t486+t490+t491+t494+t497+t501+t506/2.0+t507+t508-t509/2.0+t510+t511+t512+t513+t514+t515+t517+t519+t520+t521+t522+t523+t524+t525-t527-t528+t529+t533+t534+t535+t537+t539-t540+t541+t545+t546+t547+t548+t555+t560+t561+t562+t563+t564+t565+t566+t567-a3*dq1*mpx4*t9-a3*dq1*mpx4*t249+(a3*dq1*mpy6*t30)/2.0-(a3*dq1*mpy6*t47)/4.0+(a3*dq1*mpy6*t59)/2.0+(a3*dq1*mpy6*t112)/4.0+(a3*dq1*mpy6*t113)/4.0+(a3*dq1*mpy6*t114)/4.0-(a3*dq1*mpy6*t343)/4.0+(a3*dq1*mpy6*t426)/4.0+(a3*dq1*mpy6*t427)/4.0+(a3*dq1*mpy6*t428)/4.0-a3i*dq1*mpx4*t4+a3i*dq1*mpx4*t247-(a3i*dq1*mpy6*t34)/2.0-(a3i*dq1*mpy6*t44)/4.0-(a3i*dq1*mpy6*t70)/2.0+(a3i*dq1*mpy6*t92)/4.0+(a3i*dq1*mpy6*t93)/4.0-(a3i*dq1*mpy6*t94)/4.0+(a3i*dq1*mpy6*t341)/4.0-(a3i*dq1*mpy6*t421)/4.0-(a3i*dq1*mpy6*t422)/4.0+(a3i*dq1*mpy6*t423)/4.0-a3*a4*dq1*mass5*t9-a3*a4*dq1*mass6*t9-a3*a4*dq1*mass5*t249-a3*a4*dq1*mass6*t249-a4*a3i*dq1*mass5*t4-a4*a3i*dq1*mass6*t4+a4*a3i*dq1*mass5*t247+a4*a3i*dq1*mass6*t247-a3*d5*dq1*mass6*t4-a3*d5*dq1*mass6*t247+a3i*d5*dq1*mass6*t9-a3i*d5*dq1*mass6*t249; 01359 A0(0,4) = t2*t8*t9*t152-a4*dq1*mpy6*t5*t11-a1*dq6*mpy6*t6*t10*2.0-a3*dq1*mpy6*t4*t5*t11-a2*dq6*mpy6*t6*t7*t10*2.0+a3i*dq1*mpy6*t5*t9*t11-a3*dq6*mpy6*t2*t3*t6*t10*2.0-a2*dq1*mpy6*t3*t5*t9*t11-a2*dq1*mpy6*t4*t5*t8*t11-a3*dq6*mpy6*t6*t7*t8*t10*2.0-a3i*dq6*mpy6*t2*t6*t8*t10*2.0+a3i*dq6*mpy6*t3*t6*t7*t10*2.0-a1*dq1*mpy6*t2*t3*t4*t5*t11*2.0-a4*dq6*mpy6*t2*t3*t4*t6*t10*2.0+a1*dq1*mpy6*t2*t5*t8*t9*t11*2.0-a1*dq1*mpy6*t3*t5*t7*t9*t11*2.0-a1*dq1*mpy6*t4*t5*t7*t8*t11*2.0-a4*dq6*mpy6*t3*t6*t7*t9*t10*2.0-a4*dq6*mpy6*t4*t6*t7*t8*t10*2.0-a2*dq1*mpy6*t3*t4*t5*t11*t26+a2*dq1*mpy6*t3*t5*t9*t11*t23+a2*dq1*mpy6*t4*t5*t8*t11*t23+a2*dq1*mpy6*t5*t8*t9*t11*t26-a3*dq1*mpy6*t4*t5*t11*t23*t24-a3*dq1*mpy6*t4*t5*t11*t26*t27+a3*dq1*mpy6*t5*t9*t11*t23*t27-a3*dq1*mpy6*t5*t9*t11*t24*t26-a4*dq1*mpy6*t5*t11*t23*t24*t25+a4*dq1*mpy6*t5*t11*t23*t27*t28-a4*dq1*mpy6*t5*t11*t24*t26*t28-a4*dq1*mpy6*t5*t11*t25*t26*t27-a3i*dq1*mpy6*t4*t5*t11*t23*t27+a3i*dq1*mpy6*t4*t5*t11*t24*t26-a3i*dq1*mpy6*t5*t9*t11*t23*t24-a3i*dq1*mpy6*t5*t9*t11*t26*t27+d5*dq6*mpy6*t2*t3*t6*t9*t10*2.0+d5*dq6*mpy6*t2*t4*t6*t8*t10*2.0-d5*dq6*mpy6*t3*t4*t6*t7*t10*2.0+d5*dq6*mpy6*t6*t7*t8*t9*t10*2.0+d5*dq1*mpy6*t5*t11*t23*t24*t28+d5*dq1*mpy6*t5*t11*t23*t25*t27-d5*dq1*mpy6*t5*t11*t24*t25*t26+d5*dq1*mpy6*t5*t11*t26*t27*t28; 01360 A0(0,5) = d5*dq1*mpy6*t11-a4*dq1*mpy6*t6*t10-a1*dq5*mpy6*t6*t10*2.0-a3*dq1*mpy6*t9*t11-a3i*dq1*mpy6*t4*t11+a2*dq1*mpy6*t3*t4*t11-a3*dq1*mpy6*t4*t6*t10-a2*dq5*mpy6*t6*t7*t10*2.0-a2*dq1*mpy6*t8*t9*t11+a3i*dq1*mpy6*t6*t9*t10-a1*dq1*mpy6*t2*t3*t9*t11*2.0-a1*dq1*mpy6*t2*t4*t8*t11*2.0+a1*dq1*mpy6*t3*t4*t7*t11*2.0-a3*dq5*mpy6*t2*t3*t6*t10*2.0-a2*dq1*mpy6*t3*t6*t9*t10-a2*dq1*mpy6*t4*t6*t8*t10-a1*dq1*mpy6*t7*t8*t9*t11*2.0-a3*dq5*mpy6*t6*t7*t8*t10*2.0-a2*dq1*mpy6*t3*t4*t11*t23-a2*dq1*mpy6*t3*t9*t11*t26-a2*dq1*mpy6*t4*t8*t11*t26+a2*dq1*mpy6*t8*t9*t11*t23-a3*dq1*mpy6*t4*t11*t23*t27+a3*dq1*mpy6*t4*t11*t24*t26-a3*dq1*mpy6*t9*t11*t23*t24-a3*dq1*mpy6*t9*t11*t26*t27-a4*dq1*mpy6*t11*t23*t24*t28-a4*dq1*mpy6*t11*t23*t25*t27+a4*dq1*mpy6*t11*t24*t25*t26-a4*dq1*mpy6*t11*t26*t27*t28-a3i*dq5*mpy6*t2*t6*t8*t10*2.0+a3i*dq5*mpy6*t3*t6*t7*t10*2.0+a3i*dq1*mpy6*t4*t11*t23*t24+a3i*dq1*mpy6*t4*t11*t26*t27-a3i*dq1*mpy6*t9*t11*t23*t27+a3i*dq1*mpy6*t9*t11*t24*t26-d5*dq1*mpy6*t11*t23*t24*t25+d5*dq1*mpy6*t11*t23*t27*t28-d5*dq1*mpy6*t11*t24*t26*t28-d5*dq1*mpy6*t11*t25*t26*t27-a1*dq1*mpy6*t2*t3*t4*t6*t10*2.0-a4*dq5*mpy6*t2*t3*t4*t6*t10*2.0+a1*dq1*mpy6*t2*t6*t8*t9*t10*2.0-a1*dq1*mpy6*t3*t6*t7*t9*t10*2.0-a1*dq1*mpy6*t4*t6*t7*t8*t10*2.0+a4*dq5*mpy6*t2*t6*t8*t9*t10*2.0-a4*dq5*mpy6*t3*t6*t7*t9*t10*2.0-a4*dq5*mpy6*t4*t6*t7*t8*t10*2.0-a2*dq1*mpy6*t3*t4*t6*t10*t26+a2*dq1*mpy6*t3*t6*t9*t10*t23+a2*dq1*mpy6*t4*t6*t8*t10*t23+a2*dq1*mpy6*t6*t8*t9*t10*t26-a3*dq1*mpy6*t4*t6*t10*t23*t24-a3*dq1*mpy6*t4*t6*t10*t26*t27+a3*dq1*mpy6*t6*t9*t10*t23*t27-a3*dq1*mpy6*t6*t9*t10*t24*t26-a4*dq1*mpy6*t6*t10*t23*t24*t25+a4*dq1*mpy6*t6*t10*t23*t27*t28-a4*dq1*mpy6*t6*t10*t24*t26*t28-a4*dq1*mpy6*t6*t10*t25*t26*t27-a3i*dq1*mpy6*t4*t6*t10*t23*t27+a3i*dq1*mpy6*t4*t6*t10*t24*t26-a3i*dq1*mpy6*t6*t9*t10*t23*t24-a3i*dq1*mpy6*t6*t9*t10*t26*t27+d5*dq5*mpy6*t2*t3*t6*t9*t10*2.0+d5*dq5*mpy6*t2*t4*t6*t8*t10*2.0-d5*dq5*mpy6*t3*t4*t6*t7*t10*2.0+d5*dq5*mpy6*t6*t7*t8*t9*t10*2.0+d5*dq1*mpy6*t6*t10*t23*t24*t28+d5*dq1*mpy6*t6*t10*t23*t25*t27-d5*dq1*mpy6*t6*t10*t24*t25*t26+d5*dq1*mpy6*t6*t10*t26*t27*t28; 01361 A0(1,0) = mpy6*t433*(t211+t213+t302+t305+a2*t2)*-2.0; 01362 A0(1,1) = t81+t87+t97+t133+t138+t139+t142-t161-t162+t183+t184+t187+t194+t195+t218+t219+t222+t223+t227+t231+t232+t233+t234-t237-t238-t239-t240-t243-t244-t245-t246+t254+t255+t256+t257+t260+t275+t278+t279-t281+t283+t313+t314-t345-t346-t347-t348-t349-t350+t361+t362+a2*dq3*mpx3*t3*2.0-a2*dq3*mpy3*t8*2.0+a2*a3*dq3*mass4*t3*2.0+a2*a3*dq3*mass5*t3*2.0+a2*a3*dq3*mass6*t3*2.0+a2*a3i*dq3*mass4*t8*2.0+a2*a3i*dq3*mass5*t8*2.0+a2*a3i*dq3*mass6*t8*2.0; 01363 A0(1,2) = t74+t77+t99+t120+t121+t124+t129+t130+t137-t139+t151+t152+t158+t159+t162+t166+t196+t212+t226+t229+t230-t233-t234+t239+t240+t245+t246+t252+t253-t256-t257+t265+t267+t281+t295+t296+t299+t300+t312+t315+t316+t317+t319+t346+t347+t348+t349+t350+t360-t362+t393+a2*dq2*mpx3*t3*2.0-a2*dq2*mpy3*t8*2.0+a2*a3*dq2*mass4*t3*2.0+a2*a3*dq2*mass5*t3*2.0+a2*a3*dq2*mass6*t3*2.0+a2*a3i*dq2*mass4*t8*2.0+a2*a3i*dq2*mass5*t8*2.0+a2*a3i*dq2*mass6*t8*2.0; 01364 A0(1,3) = t73+t76+t85+t95+t99+t118+t119+t123+t127+t128+t131-t132+t137-t138+t140-t141+t151+t152+t156+t159+t161+t166+t179+t180+t185+t190+t191+t220+t226+t229+t230-t231-t232+t237+t238+t243+t244+t252+t253-t254-t255+t265+t267+t269+t280+t281+t295+t296+t299+t300+t312+t316+t317+t345+t347+t348+t349+t350+t360-t361+t393-a3*dq3*mpy6*t9*t10*t11*2.0-a3i*dq3*mpy6*t4*t10*t11*2.0; 01365 A0(1,4) = -t144-t145+t147+t149+t251-t258+t261+t263+t268+t270-t273-t276-t351+t356-t357-t363+t419+t454+t459+t460-t461-t463+t464+t465-a2*mpy6*t3*t4*t52*2.0+a2*mpy6*t8*t9*t52*2.0+a2*dq1*mpy6*t2*t10*t11*2.0-a2*dq2*mpy6*t3*t5*t9*t11*2.0-a2*dq2*mpy6*t4*t5*t8*t11*2.0+a2*dq3*mpy6*t3*t5*t9*t11*2.0+a2*dq3*mpy6*t4*t5*t8*t11*2.0+a2*dq4*mpy6*t3*t5*t9*t11*2.0+a2*dq4*mpy6*t4*t5*t8*t11*2.0; 01366 A0(1,5) = t78-t143+t148+t150-t153+t155+t157-t163+t164+t165+t250+t262+t264+t266+t271+t352+t387+t389+t452+t455+t10*t254+t10*t255+t10*t256+t10*t257-d5*dq3*mpy6*t11*2.0-d5*dq4*mpy6*t11*2.0-a4*dq2*mpy6*t6*t10*2.0-a2*dq1*mpy6*t2*t5*t6*2.0+a2*dq2*mpy6*t3*t4*t11*2.0-a2*dq3*mpy6*t3*t4*t11*2.0-a2*dq4*mpy6*t3*t4*t11*2.0-a3*dq2*mpy6*t4*t6*t10*2.0-a2*dq2*mpy6*t8*t9*t11*2.0+a2*dq3*mpy6*t8*t9*t11*2.0+a2*dq4*mpy6*t8*t9*t11*2.0-a3i*dq3*mpy6*t6*t9*t10*2.0-a3i*dq4*mpy6*t6*t9*t10*2.0-a2*dq5*mpy6*t3*t4*t5*t6*2.0-a3*dq1*mpy6*t2*t5*t6*t8*2.0-a2*dq2*mpy6*t3*t6*t9*t10*2.0-a2*dq2*mpy6*t4*t6*t8*t10*2.0+a2*dq5*mpy6*t5*t6*t8*t9*2.0-a4*dq1*mpy6*t2*t3*t5*t6*t9*2.0-a4*dq1*mpy6*t2*t4*t5*t6*t8*2.0-a4*dq1*mpy6*t5*t6*t7*t8*t9*2.0-d5*dq1*mpy6*t2*t3*t4*t5*t6*2.0-d5*dq1*mpy6*t3*t5*t6*t7*t9*2.0-d5*dq1*mpy6*t4*t5*t6*t7*t8*2.0; 01367 A0(2,0) = mpy6*t433*(t178+t189-t211-t213)*-2.0; 01368 A0(2,1) = t74+t77+t99+t120+t121+t124+t129+t130+t151+t152+t158+t159+t166+t196+t212+t265+t267+t315+t316+t317+t319; 01369 A0(2,2) = t81+t87+t97+t133+t142+t183+t184+t187+t194+t195+t218+t219+t222+t223+t227+t275+t278+t279+t283+t313+t314; 01370 A0(2,3) = t72+t75+t81+t86+t96+t116+t117+t122+t125+t126-t131+t132-t140+t141+t154+t181+t182+t186+t192+t193+t218+t219+t221+t223+t227+t272+t278+t279+t282+t313+t314-a3*dq2*mpy6*t9*t10*t11*2.0-a3i*dq2*mpy6*t4*t10*t11*2.0; 01371 A0(2,4) = t144+t145+t214+t216-t251+t258-t261-t263-t268-t270+t273+t276+t351-t356+t357+t363+t461+t463+t466+t480+t482+t483+t484+t485; 01372 A0(2,5) = t79+t80+t98+t143+t146+t153-t155-t157+t163-t164-t165+t215+t217-t250+t259-t266+t274+t277+t388+t449+t450+t451+t456+t457+t462+t474+t481-a3*dq3*mpy6*t4*t6*t10*2.0-a3*dq4*mpy6*t4*t6*t10*2.0-a3i*dq2*mpy6*t6*t9*t10*2.0-a3*dq1*mpy6*t3*t5*t6*t7*2.0-a3i*dq1*mpy6*t2*t3*t5*t6*2.0-a3i*dq1*mpy6*t5*t6*t7*t8*2.0; 01373 A0(3,0) = mpy6*t433*(t211+t213)*2.0; 01374 A0(3,1) = t86+t96+t99+t132+t141+t151+t152+t181+t182+t186+t192+t193+t221+t272+t282; 01375 A0(3,2) = t81+t85+t95+t131+t140+t179+t180+t185+t190+t191+t218+t219+t220+t269+t280; 01376 A0(3,3) = mpy6*(-d5*dq6*t11+a4*dq5*t5*t11+a4*dq6*t6*t10)*-2.0; 01377 A0(3,4) = t144+t145+t214+t216+t461+t463+t466+t480+t482+t483+t484+t485; 01378 A0(3,5) = t79+t80+t98+t143+t146+t215+t217+t449+t450+t451+t456+t457+t462+t474+t481; 01379 A0(4,0) = mpy6*t11*(-a3*dq2*t89+a3*dq2*t90+a3*dq3*t89-a3*dq3*t90+a4*dq2*t199-a4*dq3*t199-a4*dq4*t199-a4*dq2*t322+a4*dq3*t322+a4*dq4*t322+a3i*dq2*t108+a3i*dq2*t109-a3i*dq3*t108-a3i*dq3*t109-d5*dq2*t207+d5*dq3*t207+d5*dq4*t207-d5*dq2*t332+d5*dq3*t332+d5*dq4*t332-a2*dq2*sin(q2+q5)+a2*dq2*sin(q2+t42)); 01380 A0(4,1) = mpy6*t11*(t48+t176+t287+t288+t289+t292+t293+t297+t298+t301+t304+t373+t376+a4*dq3*t5+a2*dq1*t2*t10+a3*dq3*t4*t5-a3i*dq3*t5*t9)*-2.0; 01381 A0(4,2) = mpy6*t11*(t48+t176+t287+t288+t289+t292+t293+t297+t298+t301+t304+t373+t376-a4*dq2*t5-a3*dq2*t4*t5+a3i*dq2*t5*t9)*2.0; 01382 A0(4,3) = mpy6*t11*(a4*dq2*t5*-2.0+a4*dq3*t5*2.0-a4*dq1*t199+a4*dq1*t322+d5*dq1*t207+d5*dq1*t332); 01383 A0(5,0) = mpy6*t5*t6*(dq2*t178+dq2*t189-dq2*t211+dq3*t211-dq2*t213+dq4*t211+dq3*t213+dq4*t213+dq3*t302+dq3*t305-a2*dq2*t2)*-2.0; 01384 A0(5,1) = mpy6*(t50+t51+t197+t198+t336+t369+t372+t374+t398+t399+t400+t404+t405+t408+t434+t445-a3*dq3*t9*t11*2.0-a3i*dq3*t4*t11*2.0+a2*dq1*t2*t5*t6*2.0-a3*dq3*t4*t6*t10*2.0+a3i*dq3*t6*t9*t10*2.0); 01385 A0(5,2) = -mpy6*(t51+t60+t134+t198+t336+t369+t372+t374+t398+t399+t400+t404+t405+t408+t434+t445+a3*dq2*t9*t11*2.0+a3i*dq2*t4*t11*2.0+a3*dq2*t4*t6*t10*2.0-a3i*dq2*t6*t9*t10*2.0); 01386 A0(5,3) = -mpy6*(t50+t60+t134+t197+t398+t399+t400+t404+t405+t408+t434+t445); 01387 return A0; 01388 } 01389 */ 01390 01391 01392 01393
Generated on Mon Jul 18 2022 17:05:26 by
1.7.2