Marco De Silva / ARM

a

Dependencies:   MX64 ARMControl

ARAP180_with_rover.cpp@2:8d9a79e18fb9, 2021-10-04 (annotated)

Committer:
marcodesilva
Date:
Mon Oct 04 13:36:53 2021 +0000
Revision:
2:8d9a79e18fb9
Parent:
0:46ef60040f6a 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcodesilva 0:46ef60040f6a 1 #include "ARAP180_with_rover.h"
marcodesilva 0:46ef60040f6a 2 #include "ARM_parameters.h"
marcodesilva 0:46ef60040f6a 3 #include <iostream>
marcodesilva 0:46ef60040f6a 4
marcodesilva 0:46ef60040f6a 5 //*****************************************************************************
marcodesilva 0:46ef60040f6a 6
marcodesilva 0:46ef60040f6a 7 ARAP180_WITH_ROVER::ARAP180_WITH_ROVER(){
marcodesilva 0:46ef60040f6a 8 dxl_port = new UARTSerial_half(PE_8,PE_7,PE_10, 1000000);
marcodesilva 0:46ef60040f6a 9 //tool_com = new UARTSerial_mio(tool_tx, tool_rx);
marcodesilva 0:46ef60040f6a 10 //tool_com->set_baud(460800);
marcodesilva 0:46ef60040f6a 11
marcodesilva 0:46ef60040f6a 12 mx_MotorChain = new MX(ID, sizeof(ID)/sizeof(int), 1000000, dxl_port);
marcodesilva 0:46ef60040f6a 13
marcodesilva 0:46ef60040f6a 14 ID[0] = 1;
marcodesilva 0:46ef60040f6a 15 ID[1] = 2;
marcodesilva 0:46ef60040f6a 16 ID[2] = 3;
marcodesilva 0:46ef60040f6a 17 ID[3] = 4;
marcodesilva 0:46ef60040f6a 18 ID[4] = 5;
marcodesilva 0:46ef60040f6a 19 ID[5] = 6;
marcodesilva 0:46ef60040f6a 20
marcodesilva 0:46ef60040f6a 21 motorSign(0) = 1;
marcodesilva 0:46ef60040f6a 22 motorSign(1) = 1;
marcodesilva 0:46ef60040f6a 23 motorSign(2) = 1;
marcodesilva 0:46ef60040f6a 24 motorSign(3) = -1;
marcodesilva 0:46ef60040f6a 25 motorSign(4) = 1;
marcodesilva 0:46ef60040f6a 26 motorSign(5) = 1;
marcodesilva 0:46ef60040f6a 27 motorSign(6) = -1;
marcodesilva 0:46ef60040f6a 28 motorSign(7) = 1;
marcodesilva 0:46ef60040f6a 29
marcodesilva 0:46ef60040f6a 30 toolGearRatio = 12.4553;
marcodesilva 0:46ef60040f6a 31
marcodesilva 0:46ef60040f6a 32
marcodesilva 0:46ef60040f6a 33 offsets(0) = 0.0;
marcodesilva 0:46ef60040f6a 34 offsets(1) = 0.0;
marcodesilva 0:46ef60040f6a 35 offsets(2) = -22.5;
marcodesilva 0:46ef60040f6a 36 offsets(3) = 0.0;
marcodesilva 0:46ef60040f6a 37 offsets(4) = 0.0;
marcodesilva 0:46ef60040f6a 38 offsets(5) = 0.0;
marcodesilva 0:46ef60040f6a 39 offsets(6) = 0.0;
marcodesilva 0:46ef60040f6a 40 offsets(7) = -360.0/toolGearRatio;
marcodesilva 0:46ef60040f6a 41
marcodesilva 0:46ef60040f6a 42 extendedSelect[0] = 0;
marcodesilva 0:46ef60040f6a 43 extendedSelect[1] = 0;
marcodesilva 0:46ef60040f6a 44 extendedSelect[2] = 0;
marcodesilva 0:46ef60040f6a 45 extendedSelect[3] = 0;
marcodesilva 0:46ef60040f6a 46 extendedSelect[4] = 0;
marcodesilva 0:46ef60040f6a 47 extendedSelect[5] = 1;
marcodesilva 0:46ef60040f6a 48
marcodesilva 0:46ef60040f6a 49
marcodesilva 0:46ef60040f6a 50 a3 = 0.029;
marcodesilva 0:46ef60040f6a 51 a4 = 0.340;
marcodesilva 0:46ef60040f6a 52 a5 = 0.029;
marcodesilva 0:46ef60040f6a 53 a5i = 0.335;
marcodesilva 0:46ef60040f6a 54 a6 = 0.084;
marcodesilva 0:46ef60040f6a 55 d7 = -0.218;
marcodesilva 0:46ef60040f6a 56 // drone distances
marcodesilva 0:46ef60040f6a 57 x_0_r2 = 0.0;
marcodesilva 0:46ef60040f6a 58 y_0_r2 = -0.3;
marcodesilva 0:46ef60040f6a 59 z_0_r2 = 0.36;
marcodesilva 0:46ef60040f6a 60
marcodesilva 0:46ef60040f6a 61
marcodesilva 0:46ef60040f6a 62 }
marcodesilva 0:46ef60040f6a 63
marcodesilva 0:46ef60040f6a 64
marcodesilva 0:46ef60040f6a 65 void ARAP180_WITH_ROVER::setParameters(VectorXd armParameters){
marcodesilva 0:46ef60040f6a 66
marcodesilva 0:46ef60040f6a 67 a3 = armParameters(0);// 0.036
marcodesilva 0:46ef60040f6a 68 a4 = armParameters(1);// 0.36836
marcodesilva 0:46ef60040f6a 69 a5 = armParameters(2);// -0.029
marcodesilva 0:46ef60040f6a 70 a5i = armParameters(3);
marcodesilva 0:46ef60040f6a 71 a6 = armParameters(4);
marcodesilva 0:46ef60040f6a 72 d7 = armParameters(5);// 0.370
marcodesilva 0:46ef60040f6a 73 x_0_r2 = armParameters(6);
marcodesilva 0:46ef60040f6a 74 y_0_r2 = armParameters(7);
marcodesilva 0:46ef60040f6a 75 z_0_r2 = armParameters(8);// 0.53378
marcodesilva 0:46ef60040f6a 76
marcodesilva 0:46ef60040f6a 77 massr = armParameters(9);
marcodesilva 0:46ef60040f6a 78 mpxr = armParameters(10);
marcodesilva 0:46ef60040f6a 79 mpyr = armParameters(11);
marcodesilva 0:46ef60040f6a 80 mpzr = armParameters(12);
marcodesilva 0:46ef60040f6a 81 mass1 = armParameters(13);
marcodesilva 0:46ef60040f6a 82 mpx1 = armParameters(14);
marcodesilva 0:46ef60040f6a 83 mpy1 = armParameters(15);
marcodesilva 0:46ef60040f6a 84 mpz1 = armParameters(16);
marcodesilva 0:46ef60040f6a 85 mass2 = armParameters(17);
marcodesilva 0:46ef60040f6a 86 mpx2 = armParameters(18);
marcodesilva 0:46ef60040f6a 87 mpy2 = armParameters(19);
marcodesilva 0:46ef60040f6a 88 mpz2 = armParameters(20);
marcodesilva 0:46ef60040f6a 89 mass3 = armParameters(21);
marcodesilva 0:46ef60040f6a 90 mpx3 = armParameters(22);
marcodesilva 0:46ef60040f6a 91 mpy3 = armParameters(23);
marcodesilva 0:46ef60040f6a 92 mpz3 = armParameters(24);
marcodesilva 0:46ef60040f6a 93 mass4 = armParameters(25);
marcodesilva 0:46ef60040f6a 94 mpx4 = armParameters(26);
marcodesilva 0:46ef60040f6a 95 mpy4 = armParameters(27);
marcodesilva 0:46ef60040f6a 96 mpz4 = armParameters(28);
marcodesilva 0:46ef60040f6a 97 mass5 = armParameters(29);
marcodesilva 0:46ef60040f6a 98 mpx5 = armParameters(30);
marcodesilva 0:46ef60040f6a 99 mpy5 = armParameters(31);
marcodesilva 0:46ef60040f6a 100 mpz5 = armParameters(32);
marcodesilva 0:46ef60040f6a 101 mass6 = armParameters(33);
marcodesilva 0:46ef60040f6a 102 mpx6 = armParameters(34);
marcodesilva 0:46ef60040f6a 103 mpy6 = armParameters(35);
marcodesilva 0:46ef60040f6a 104 mpz6 = armParameters(36);
marcodesilva 0:46ef60040f6a 105
marcodesilva 0:46ef60040f6a 106 mass_battery = armParameters(37);
marcodesilva 0:46ef60040f6a 107 mpx_battery = armParameters(38);
marcodesilva 0:46ef60040f6a 108 mpz_battery = armParameters(39);
marcodesilva 0:46ef60040f6a 109
marcodesilva 0:46ef60040f6a 110 std::cout << "armParameters: " << armParameters << std::endl;
marcodesilva 0:46ef60040f6a 111 }
marcodesilva 0:46ef60040f6a 112
marcodesilva 0:46ef60040f6a 113
marcodesilva 0:46ef60040f6a 114
marcodesilva 0:46ef60040f6a 115 void ARAP180_WITH_ROVER::setOperatingMode(int modeArm, int modeTool){ //mode = 3 for position control
marcodesilva 0:46ef60040f6a 116
marcodesilva 0:46ef60040f6a 117 int operatingMode[6];
marcodesilva 0:46ef60040f6a 118
marcodesilva 0:46ef60040f6a 119 operatingMode[0] = modeArm;
marcodesilva 0:46ef60040f6a 120 operatingMode[1] = modeArm;
marcodesilva 0:46ef60040f6a 121 operatingMode[2] = modeArm;
marcodesilva 0:46ef60040f6a 122 operatingMode[3] = modeArm;
marcodesilva 0:46ef60040f6a 123 operatingMode[4] = modeArm;
marcodesilva 0:46ef60040f6a 124 operatingMode[5] = modeTool;
marcodesilva 0:46ef60040f6a 125
marcodesilva 0:46ef60040f6a 126 mx_MotorChain->SyncOperatingMode(operatingMode, -1);
marcodesilva 0:46ef60040f6a 127 }
marcodesilva 0:46ef60040f6a 128
marcodesilva 0:46ef60040f6a 129 void ARAP180_WITH_ROVER::enableMotors(int enable){
marcodesilva 0:46ef60040f6a 130 bool enableVal[6];
marcodesilva 0:46ef60040f6a 131 enableVal[0] = enable;
marcodesilva 0:46ef60040f6a 132 enableVal[1] = enable;
marcodesilva 0:46ef60040f6a 133 enableVal[2] = enable;
marcodesilva 0:46ef60040f6a 134 enableVal[3] = enable;
marcodesilva 0:46ef60040f6a 135 enableVal[4] = enable;
marcodesilva 0:46ef60040f6a 136 enableVal[5] = enable;
marcodesilva 0:46ef60040f6a 137
marcodesilva 0:46ef60040f6a 138 mx_MotorChain->SyncTorqueEnable(enableVal, -1);
marcodesilva 0:46ef60040f6a 139 }
marcodesilva 0:46ef60040f6a 140
marcodesilva 0:46ef60040f6a 141 void ARAP180_WITH_ROVER::setMaxVel(float maxVel){
marcodesilva 0:46ef60040f6a 142
marcodesilva 0:46ef60040f6a 143 maxVel = 695.5*maxVel;//conversione velocità
marcodesilva 0:46ef60040f6a 144 float Vel_profile[6] = {maxVel,maxVel,maxVel,maxVel,maxVel,maxVel};
marcodesilva 0:46ef60040f6a 145 mx_MotorChain->SyncProfileVel(Vel_profile);
marcodesilva 0:46ef60040f6a 146
marcodesilva 0:46ef60040f6a 147 }
marcodesilva 0:46ef60040f6a 148
marcodesilva 0:46ef60040f6a 149
marcodesilva 0:46ef60040f6a 150 void ARAP180_WITH_ROVER::setMaxAcc(float maxAcc){
marcodesilva 0:46ef60040f6a 151
marcodesilva 0:46ef60040f6a 152 maxAcc = 8.9*maxAcc; //8.9=60*200/(6.28*214) //742
marcodesilva 0:46ef60040f6a 153 float Acc_profile[6] = {maxAcc,maxAcc,maxAcc,maxAcc,maxAcc,maxAcc};
marcodesilva 0:46ef60040f6a 154 mx_MotorChain->SyncProfileAccel(Acc_profile);
marcodesilva 0:46ef60040f6a 155
marcodesilva 0:46ef60040f6a 156 }
marcodesilva 0:46ef60040f6a 157
marcodesilva 0:46ef60040f6a 158 void ARAP180_WITH_ROVER::setJointPos(Vector8f q){
marcodesilva 0:46ef60040f6a 159
marcodesilva 0:46ef60040f6a 160
marcodesilva 0:46ef60040f6a 161 float Goal_position[6];
marcodesilva 0:46ef60040f6a 162
marcodesilva 0:46ef60040f6a 163 //For pololu control
marcodesilva 0:46ef60040f6a 164
marcodesilva 0:46ef60040f6a 165 /*union Packet{
marcodesilva 0:46ef60040f6a 166 float f;
marcodesilva 0:46ef60040f6a 167 char c[8];
marcodesilva 0:46ef60040f6a 168 };
marcodesilva 0:46ef60040f6a 169 Packet p;*/
marcodesilva 0:46ef60040f6a 170
marcodesilva 0:46ef60040f6a 171 Goal_position[0] = utilities::rad2deg(q(2)) * motorSign(2) + offsets(2);
marcodesilva 0:46ef60040f6a 172 Goal_position[1] = utilities::rad2deg(q(3)) * motorSign(3) + offsets(3);
marcodesilva 0:46ef60040f6a 173 Goal_position[2] = utilities::rad2deg(q(4)) * motorSign(4) + offsets(4);
marcodesilva 0:46ef60040f6a 174 Goal_position[3] = utilities::rad2deg(q(5)) * motorSign(5) + offsets(5);
marcodesilva 0:46ef60040f6a 175 Goal_position[4] = utilities::rad2deg(q(6)) * motorSign(6) + offsets(6);
marcodesilva 0:46ef60040f6a 176
marcodesilva 0:46ef60040f6a 177 Goal_position[5] = toolGearRatio*(utilities::rad2deg(q(7)) * motorSign(7) + offsets(7));
marcodesilva 0:46ef60040f6a 178
marcodesilva 0:46ef60040f6a 179 mx_MotorChain->SyncSetGoalExtended(Goal_position, extendedSelect);
marcodesilva 0:46ef60040f6a 180
marcodesilva 0:46ef60040f6a 181 //For pololu control
marcodesilva 0:46ef60040f6a 182 /*mx_MotorChain->SyncSetGoal(Goal_position);
marcodesilva 0:46ef60040f6a 183
marcodesilva 0:46ef60040f6a 184 p.f = (q(7) * motorSign(7) + offsets(7));
marcodesilva 0:46ef60040f6a 185
marcodesilva 0:46ef60040f6a 186 tool_com->write(p.c,8);*/
marcodesilva 0:46ef60040f6a 187
marcodesilva 0:46ef60040f6a 188 }
marcodesilva 0:46ef60040f6a 189
marcodesilva 0:46ef60040f6a 190
marcodesilva 0:46ef60040f6a 191
marcodesilva 0:46ef60040f6a 192 Vector6f ARAP180_WITH_ROVER::getJointPos(bool checkData){
marcodesilva 0:46ef60040f6a 193
marcodesilva 0:46ef60040f6a 194 Vector6f getPosConverted = Vector6f::Zero();
marcodesilva 0:46ef60040f6a 195 float getPos[6];
marcodesilva 0:46ef60040f6a 196 bool dataCheck;
marcodesilva 0:46ef60040f6a 197
marcodesilva 0:46ef60040f6a 198 if(checkData){
marcodesilva 0:46ef60040f6a 199 while(!mx_MotorChain->SyncGetExtendedPosition(getPos, extendedSelect)){
marcodesilva 0:46ef60040f6a 200 ThisThread::sleep_for(0.1);
marcodesilva 0:46ef60040f6a 201 //printf("wait for arm position!\n");
marcodesilva 0:46ef60040f6a 202 }
marcodesilva 0:46ef60040f6a 203 }else{
marcodesilva 0:46ef60040f6a 204 mx_MotorChain->SyncGetExtendedPosition(getPos, extendedSelect);
marcodesilva 0:46ef60040f6a 205 }
marcodesilva 0:46ef60040f6a 206
marcodesilva 0:46ef60040f6a 207 getPosConverted(0) = utilities::deg2rad(getPos[0] - offsets(2)) * motorSign(2);
marcodesilva 0:46ef60040f6a 208 getPosConverted(1) = utilities::deg2rad(getPos[1] - offsets(3)) * motorSign(3);
marcodesilva 0:46ef60040f6a 209 getPosConverted(2) = utilities::deg2rad(getPos[2] - offsets(4)) * motorSign(4);
marcodesilva 0:46ef60040f6a 210 getPosConverted(3) = utilities::deg2rad(getPos[3] - offsets(5)) * motorSign(5);
marcodesilva 0:46ef60040f6a 211 getPosConverted(4) = utilities::deg2rad(getPos[4] - offsets(6)) * motorSign(6);
marcodesilva 0:46ef60040f6a 212 getPosConverted(5) = utilities::deg2rad(getPos[5]/toolGearRatio - offsets(7)) * motorSign(7);
marcodesilva 0:46ef60040f6a 213
marcodesilva 0:46ef60040f6a 214 return getPosConverted;
marcodesilva 0:46ef60040f6a 215
marcodesilva 0:46ef60040f6a 216 }
marcodesilva 0:46ef60040f6a 217
marcodesilva 0:46ef60040f6a 218
marcodesilva 0:46ef60040f6a 219 //Dyrect Kinematics Matrix Te
marcodesilva 0:46ef60040f6a 220 Matrix4f ARAP180_WITH_ROVER::forwardKinematics(Vector8f q){
marcodesilva 0:46ef60040f6a 221
marcodesilva 0:46ef60040f6a 222 Matrix4f A0 = Matrix4f::Zero();
marcodesilva 0:46ef60040f6a 223 volatile float q1 = q[0];
marcodesilva 0:46ef60040f6a 224 volatile float q2 = q[1];
marcodesilva 0:46ef60040f6a 225 volatile float q3 = q[2];
marcodesilva 0:46ef60040f6a 226 volatile float q4 = q[3];
marcodesilva 0:46ef60040f6a 227 volatile float q5 = q[4];
marcodesilva 0:46ef60040f6a 228 volatile float q6 = q[5];
marcodesilva 0:46ef60040f6a 229 volatile float q7 = q[6];
marcodesilva 0:46ef60040f6a 230 volatile float q8 = q[7];
marcodesilva 0:46ef60040f6a 231
marcodesilva 0:46ef60040f6a 232 float t2 = cos(q2);
marcodesilva 0:46ef60040f6a 233 float t3 = cos(q3);
marcodesilva 0:46ef60040f6a 234 float t4 = cos(q4);
marcodesilva 0:46ef60040f6a 235 float t5 = cos(q5);
marcodesilva 0:46ef60040f6a 236 float t6 = cos(q6);
marcodesilva 0:46ef60040f6a 237 float t7 = cos(q7);
marcodesilva 0:46ef60040f6a 238 float t8 = cos(q8);
marcodesilva 0:46ef60040f6a 239 float t9 = sin(q2);
marcodesilva 0:46ef60040f6a 240 float t10 = sin(q3);
marcodesilva 0:46ef60040f6a 241 float t11 = sin(q4);
marcodesilva 0:46ef60040f6a 242 float t12 = sin(q5);
marcodesilva 0:46ef60040f6a 243 float t13 = sin(q6);
marcodesilva 0:46ef60040f6a 244 float t14 = sin(q7);
marcodesilva 0:46ef60040f6a 245 float t15 = sin(q8);
marcodesilva 0:46ef60040f6a 246 float t16 = q4*-1.0;
marcodesilva 0:46ef60040f6a 247 float t17 = q4*1.0;
marcodesilva 0:46ef60040f6a 248 float t18 = q5*-1.0;
marcodesilva 0:46ef60040f6a 249 float t19 = q5*1.0;
marcodesilva 0:46ef60040f6a 250 float t20 = -q4;
marcodesilva 0:46ef60040f6a 251 float t21 = t2*t4;
marcodesilva 0:46ef60040f6a 252 float t22 = t2*t11;
marcodesilva 0:46ef60040f6a 253 float t23 = t4*t9;
marcodesilva 0:46ef60040f6a 254 float t24 = t7*t10;
marcodesilva 0:46ef60040f6a 255 float t25 = t9*t11;
marcodesilva 0:46ef60040f6a 256 float t26 = q4+t18;
marcodesilva 0:46ef60040f6a 257 float t27 = q5+q6+t16;
marcodesilva 0:46ef60040f6a 258 float t30 = q5+q6+t20;
marcodesilva 0:46ef60040f6a 259 float t34 = t2*t3*t7*-1.0;
marcodesilva 0:46ef60040f6a 260 float t35 = t2*t3*t7*1.0;
marcodesilva 0:46ef60040f6a 261 float t36 = t3*t7*t9*-1.0;
marcodesilva 0:46ef60040f6a 262 float t37 = t3*t7*t9*1.0;
marcodesilva 0:46ef60040f6a 263 float t28 = sin(t26);
marcodesilva 0:46ef60040f6a 264 float t29 = t10*t25;
marcodesilva 0:46ef60040f6a 265 float t31 = cos(t27);
marcodesilva 0:46ef60040f6a 266 float t32 = sin(t27);
marcodesilva 0:46ef60040f6a 267 float t33 = t10*t21;
marcodesilva 0:46ef60040f6a 268 float t38 = t10*t22*-1.0;
marcodesilva 0:46ef60040f6a 269 float t39 = t10*t22*1.0;
marcodesilva 0:46ef60040f6a 270 float t40 = t10*t23*-1.0;
marcodesilva 0:46ef60040f6a 271 float t41 = t10*t23*1.0;
marcodesilva 0:46ef60040f6a 272 float t42 = t3*t14*t31;
marcodesilva 0:46ef60040f6a 273 float t43 = t21+t29;
marcodesilva 0:46ef60040f6a 274 float t44 = t25+t33;
marcodesilva 0:46ef60040f6a 275 float t45 = t22+t40;
marcodesilva 0:46ef60040f6a 276 float t46 = t23+t38;
marcodesilva 0:46ef60040f6a 277 float t47 = t5*t43;
marcodesilva 0:46ef60040f6a 278 float t48 = t12*t43;
marcodesilva 0:46ef60040f6a 279 float t49 = t5*t44*1.0;
marcodesilva 0:46ef60040f6a 280 float t50 = t12*t45;
marcodesilva 0:46ef60040f6a 281 float t51 = t12*t44*1.0;
marcodesilva 0:46ef60040f6a 282 float t52 = t5*t45*-1.0;
marcodesilva 0:46ef60040f6a 283 float t53 = t5*t45*1.0;
marcodesilva 0:46ef60040f6a 284 float t54 = t5*t46*1.0;
marcodesilva 0:46ef60040f6a 285 float t55 = t12*t46*-1.0;
marcodesilva 0:46ef60040f6a 286 float t56 = t12*t46*1.0;
marcodesilva 0:46ef60040f6a 287 float t57 = t24+t42;
marcodesilva 0:46ef60040f6a 288 float t58 = t47+t50;
marcodesilva 0:46ef60040f6a 289 float t59 = t48+t52;
marcodesilva 0:46ef60040f6a 290 float t60 = t49+t55;
marcodesilva 0:46ef60040f6a 291 float t61 = t51+t54;
marcodesilva 0:46ef60040f6a 292 float t62 = t6*t58;
marcodesilva 0:46ef60040f6a 293 float t63 = t13*t58;
marcodesilva 0:46ef60040f6a 294 float t64 = t6*t59;
marcodesilva 0:46ef60040f6a 295 float t65 = t13*t59*-1.0;
marcodesilva 0:46ef60040f6a 296 float t66 = t13*t59*1.0;
marcodesilva 0:46ef60040f6a 297 float t67 = t13*t60*1.0;
marcodesilva 0:46ef60040f6a 298 float t68 = t13*t61*-1.0;
marcodesilva 0:46ef60040f6a 299 float t69 = t13*t61*1.0;
marcodesilva 0:46ef60040f6a 300 float t70 = t6*t60*1.0;
marcodesilva 0:46ef60040f6a 301 float t71 = t6*t61*1.0;
marcodesilva 0:46ef60040f6a 302 float t72 = t63+t64;
marcodesilva 0:46ef60040f6a 303 float t73 = t62+t65;
marcodesilva 0:46ef60040f6a 304 float t75 = t68+t70;
marcodesilva 0:46ef60040f6a 305 float t76 = t67+t71;
marcodesilva 0:46ef60040f6a 306 float t74 = t14*t72;
marcodesilva 0:46ef60040f6a 307 float t77 = t14*t75*1.0;
marcodesilva 0:46ef60040f6a 308 float t78 = t36+t74;
marcodesilva 0:46ef60040f6a 309 float t79 = t34+t77;
marcodesilva 0:46ef60040f6a 310 A0(0,0) = t8*t57+t3*t15*t32;
marcodesilva 0:46ef60040f6a 311 A0(0,1) = t15*t57*-1.0+t3*t8*t32;
marcodesilva 0:46ef60040f6a 312 A0(0,2) = t10*t14*-1.0+t3*t7*cos(t30);
marcodesilva 0:46ef60040f6a 313 A0(0,3) = q1+z_0_r2+a3*t3-d7*t3*sin(t30)*1.0+a4*t3*t11-a5i*t3*t28*1.0+a5*t3*t4*t5+a5*t3*t11*t12+a6*t3*t13*t28+a6*t3*t6*cos(t26);
marcodesilva 0:46ef60040f6a 314 A0(1,0) = t8*t79*-1.0-t15*t76*1.0;
marcodesilva 0:46ef60040f6a 315 A0(1,1) = t8*t76*-1.0+t15*t79;
marcodesilva 0:46ef60040f6a 316 A0(1,2) = t7*t75*-1.0-t2*t3*t14*1.0;
marcodesilva 0:46ef60040f6a 317 A0(1,3) = a4*t23+a4*t38+t2*x_0_r2-t9*y_0_r2*1.0-a5i*(t5*t46+t12*t44)*1.0+d7*(t6*t61+t13*t60)-a3*t2*t10*1.0-a5*t5*t44*1.0+a5*t12*t46-a6*t6*t60*1.0+a6*t13*t61;
marcodesilva 0:46ef60040f6a 318 A0(2,0) = t8*t78*-1.0+t15*t73;
marcodesilva 0:46ef60040f6a 319 A0(2,1) = t8*t73+t15*t78;
marcodesilva 0:46ef60040f6a 320 A0(2,2) = t7*t72*-1.0-t3*t9*t14*1.0;
marcodesilva 0:46ef60040f6a 321 A0(2,3) = a4*t21*-1.0-a4*t29*1.0-a5*t48*1.0-a6*t63*1.0-a6*t64*1.0+a5i*t58-d7*t73*1.0+t9*x_0_r2+t2*y_0_r2-a3*t9*t10*1.0+a5*t5*t45;
marcodesilva 0:46ef60040f6a 322 A0(3,3) = 1.0;
marcodesilva 0:46ef60040f6a 323
marcodesilva 0:46ef60040f6a 324
marcodesilva 0:46ef60040f6a 325 return A0;
marcodesilva 0:46ef60040f6a 326 }
marcodesilva 0:46ef60040f6a 327
marcodesilva 0:46ef60040f6a 328 //Jacobian matrix J
marcodesilva 0:46ef60040f6a 329 Matrix<float,6,8> ARAP180_WITH_ROVER::jacobianMatrix(Vector8f q){
marcodesilva 0:46ef60040f6a 330
marcodesilva 0:46ef60040f6a 331 Matrix<float,6,8> A0 = Matrix<float,6,8>::Zero();
marcodesilva 0:46ef60040f6a 332 volatile float q1 = q[0];
marcodesilva 0:46ef60040f6a 333 volatile float q2 = q[1];
marcodesilva 0:46ef60040f6a 334 volatile float q3 = q[2];
marcodesilva 0:46ef60040f6a 335 volatile float q4 = q[3];
marcodesilva 0:46ef60040f6a 336 volatile float q5 = q[4];
marcodesilva 0:46ef60040f6a 337 volatile float q6 = q[5];
marcodesilva 0:46ef60040f6a 338 volatile float q7 = q[6];
marcodesilva 0:46ef60040f6a 339 volatile float q8 = q[7];
marcodesilva 0:46ef60040f6a 340
marcodesilva 0:46ef60040f6a 341
marcodesilva 0:46ef60040f6a 342 float t2 = cos(q2);
marcodesilva 0:46ef60040f6a 343 float t3 = cos(q3);
marcodesilva 0:46ef60040f6a 344 float t4 = cos(q4);
marcodesilva 0:46ef60040f6a 345 float t5 = cos(q5);
marcodesilva 0:46ef60040f6a 346 float t6 = cos(q6);
marcodesilva 0:46ef60040f6a 347 float t7 = cos(q7);
marcodesilva 0:46ef60040f6a 348 float t8 = sin(q2);
marcodesilva 0:46ef60040f6a 349 float t9 = sin(q3);
marcodesilva 0:46ef60040f6a 350 float t10 = sin(q4);
marcodesilva 0:46ef60040f6a 351 float t11 = sin(q5);
marcodesilva 0:46ef60040f6a 352 float t12 = sin(q6);
marcodesilva 0:46ef60040f6a 353 float t13 = sin(q7);
marcodesilva 0:46ef60040f6a 354 float t14 = q5*-1.0;
marcodesilva 0:46ef60040f6a 355 float t15 = q5*1.0;
marcodesilva 0:46ef60040f6a 356 float t17 = -q4;
marcodesilva 0:46ef60040f6a 357 float t16 = a3*t3;
marcodesilva 0:46ef60040f6a 358 float t18 = t2*t3;
marcodesilva 0:46ef60040f6a 359 float t19 = t2*t4;
marcodesilva 0:46ef60040f6a 360 float t20 = t3*t8;
marcodesilva 0:46ef60040f6a 361 float t21 = t2*t10;
marcodesilva 0:46ef60040f6a 362 float t22 = t4*t8;
marcodesilva 0:46ef60040f6a 363 float t23 = t8*t10;
marcodesilva 0:46ef60040f6a 364 float t24 = q4+t14;
marcodesilva 0:46ef60040f6a 365 float t28 = a4*t3*t10;
marcodesilva 0:46ef60040f6a 366 float t30 = q5+q6+t17;
marcodesilva 0:46ef60040f6a 367 float t32 = a3*t2*t9*1.0;
marcodesilva 0:46ef60040f6a 368 float t33 = a3*t8*t9*1.0;
marcodesilva 0:46ef60040f6a 369 float t40 = a5*t3*t4*t5;
marcodesilva 0:46ef60040f6a 370 float t41 = a5*t3*t10*t11;
marcodesilva 0:46ef60040f6a 371 float t25 = cos(t24);
marcodesilva 0:46ef60040f6a 372 float t26 = sin(t24);
marcodesilva 0:46ef60040f6a 373 float t27 = a4*t22;
marcodesilva 0:46ef60040f6a 374 float t29 = t9*t23;
marcodesilva 0:46ef60040f6a 375 float t31 = a4*t19*1.0;
marcodesilva 0:46ef60040f6a 376 float t34 = t9*t19;
marcodesilva 0:46ef60040f6a 377 float t35 = t9*t21;
marcodesilva 0:46ef60040f6a 378 float t38 = t9*t22*-1.0;
marcodesilva 0:46ef60040f6a 379 float t39 = t9*t22*1.0;
marcodesilva 0:46ef60040f6a 380 float t42 = sin(t30);
marcodesilva 0:46ef60040f6a 381 float t36 = t35*-1.0;
marcodesilva 0:46ef60040f6a 382 float t37 = t35*1.0;
marcodesilva 0:46ef60040f6a 383 float t45 = a4*t29*1.0;
marcodesilva 0:46ef60040f6a 384 float t46 = -t35;
marcodesilva 0:46ef60040f6a 385 float t47 = a5i*t3*t26*-1.0;
marcodesilva 0:46ef60040f6a 386 float t48 = a5i*t3*t26*1.0;
marcodesilva 0:46ef60040f6a 387 float t49 = a6*t3*t6*t25;
marcodesilva 0:46ef60040f6a 388 float t50 = a6*t3*t12*t26;
marcodesilva 0:46ef60040f6a 389 float t51 = d7*t3*t42*-1.0;
marcodesilva 0:46ef60040f6a 390 float t52 = d7*t3*t42*1.0;
marcodesilva 0:46ef60040f6a 391 float t53 = t19+t29;
marcodesilva 0:46ef60040f6a 392 float t54 = t23+t34;
marcodesilva 0:46ef60040f6a 393 float t55 = t21+t38;
marcodesilva 0:46ef60040f6a 394 float t43 = a4*t36;
marcodesilva 0:46ef60040f6a 395 float t44 = a4*t37;
marcodesilva 0:46ef60040f6a 396 float t56 = t22+t36;
marcodesilva 0:46ef60040f6a 397 float t57 = t22+t46;
marcodesilva 0:46ef60040f6a 398 float t58 = t5*t53;
marcodesilva 0:46ef60040f6a 399 float t59 = t11*t53;
marcodesilva 0:46ef60040f6a 400 float t60 = t11*t54;
marcodesilva 0:46ef60040f6a 401 float t62 = t5*t54*1.0;
marcodesilva 0:46ef60040f6a 402 float t63 = t11*t55;
marcodesilva 0:46ef60040f6a 403 float t65 = t5*t55*-1.0;
marcodesilva 0:46ef60040f6a 404 float t66 = t5*t55*1.0;
marcodesilva 0:46ef60040f6a 405 float t70 = a5*t5*t55;
marcodesilva 0:46ef60040f6a 406 float t71 = a5*t5*t54*-1.0;
marcodesilva 0:46ef60040f6a 407 float t89 = t49+t50+t51;
marcodesilva 0:46ef60040f6a 408 float t61 = t5*t56;
marcodesilva 0:46ef60040f6a 409 float t64 = t60*1.0;
marcodesilva 0:46ef60040f6a 410 float t68 = t11*t56*-1.0;
marcodesilva 0:46ef60040f6a 411 float t69 = t11*t56*1.0;
marcodesilva 0:46ef60040f6a 412 float t72 = a5*t62;
marcodesilva 0:46ef60040f6a 413 float t73 = a5*t11*t56;
marcodesilva 0:46ef60040f6a 414 float t74 = a5*t59*1.0;
marcodesilva 0:46ef60040f6a 415 float t75 = t5*t57;
marcodesilva 0:46ef60040f6a 416 float t76 = a5*t11*t57;
marcodesilva 0:46ef60040f6a 417 float t77 = -t70;
marcodesilva 0:46ef60040f6a 418 float t79 = t58+t63;
marcodesilva 0:46ef60040f6a 419 float t81 = t59+t65;
marcodesilva 0:46ef60040f6a 420 float t104 = t40+t41+t47+t89;
marcodesilva 0:46ef60040f6a 421 float t67 = t61*1.0;
marcodesilva 0:46ef60040f6a 422 float t78 = -t76;
marcodesilva 0:46ef60040f6a 423 float t80 = t60+t61;
marcodesilva 0:46ef60040f6a 424 float t82 = t60+t75;
marcodesilva 0:46ef60040f6a 425 float t83 = t62+t68;
marcodesilva 0:46ef60040f6a 426 float t85 = a5i*t79;
marcodesilva 0:46ef60040f6a 427 float t88 = t6*t79;
marcodesilva 0:46ef60040f6a 428 float t91 = t12*t81;
marcodesilva 0:46ef60040f6a 429 float t98 = a6*t12*t79*1.0;
marcodesilva 0:46ef60040f6a 430 float t99 = a6*t6*t81*1.0;
marcodesilva 0:46ef60040f6a 431 float t105 = t28+t104;
marcodesilva 0:46ef60040f6a 432 float t84 = t64+t67;
marcodesilva 0:46ef60040f6a 433 float t86 = a5i*t80*-1.0;
marcodesilva 0:46ef60040f6a 434 float t87 = a5i*t80*1.0;
marcodesilva 0:46ef60040f6a 435 float t90 = t88*1.0;
marcodesilva 0:46ef60040f6a 436 float t92 = a5i*t82*1.0;
marcodesilva 0:46ef60040f6a 437 float t93 = -t85;
marcodesilva 0:46ef60040f6a 438 float t95 = t12*t83;
marcodesilva 0:46ef60040f6a 439 float t96 = t91*-1.0;
marcodesilva 0:46ef60040f6a 440 float t97 = t91*1.0;
marcodesilva 0:46ef60040f6a 441 float t101 = -t91;
marcodesilva 0:46ef60040f6a 442 float t102 = a6*t6*t83*-1.0;
marcodesilva 0:46ef60040f6a 443 float t103 = a6*t6*t83*1.0;
marcodesilva 0:46ef60040f6a 444 float t106 = t16+t105;
marcodesilva 0:46ef60040f6a 445 float t94 = t6*t84;
marcodesilva 0:46ef60040f6a 446 float t100 = a6*t12*t84;
marcodesilva 0:46ef60040f6a 447 float t107 = t88+t96;
marcodesilva 0:46ef60040f6a 448 float t108 = t90+t101;
marcodesilva 0:46ef60040f6a 449 float t109 = t94+t95;
marcodesilva 0:46ef60040f6a 450 float t110 = d7*t107*1.0;
marcodesilva 0:46ef60040f6a 451 float t111 = d7*t109;
marcodesilva 0:46ef60040f6a 452 float t112 = t98+t99+t110;
marcodesilva 0:46ef60040f6a 453 float t113 = t74+t77+t93+t112;
marcodesilva 0:46ef60040f6a 454 float t114 = t71+t73+t86+t100+t102+t111;
marcodesilva 0:46ef60040f6a 455 float t117 = t73+t78+t86+t92+t100+t102+t111;
marcodesilva 0:46ef60040f6a 456 float t115 = t27+t43+t114;
marcodesilva 0:46ef60040f6a 457 float t116 = t31+t45+t113;
marcodesilva 0:46ef60040f6a 458 A0(0,0) = 1.0;
marcodesilva 0:46ef60040f6a 459 A0(0,2) = -t8*(t33+t116)-t2*(-t27+t32+t44+t72-t73+t87-t100+t103-t111)*1.0;
marcodesilva 0:46ef60040f6a 460 A0(0,3) = t18*t116*1.0+t20*t115*1.0;
marcodesilva 0:46ef60040f6a 461 A0(0,4) = t20*(t72-t73+t87-t100+t103-t111)-t18*t113;
marcodesilva 0:46ef60040f6a 462 A0(0,5) = -t18*t112-t20*t117;
marcodesilva 0:46ef60040f6a 463 A0(0,6) = t107*t111*-1.0+t108*t111;
marcodesilva 0:46ef60040f6a 464 A0(1,1) = t31+t33+t45+t74-t85*1.0+t112+a5*t65-t8*x_0_r2*1.0-t2*y_0_r2*1.0;
marcodesilva 0:46ef60040f6a 465 A0(1,2) = t2*t106*-1.0;
marcodesilva 0:46ef60040f6a 466 A0(1,3) = t9*t116*-1.0-t20*t105*1.0;
marcodesilva 0:46ef60040f6a 467 A0(1,4) = t9*t113+t20*t104;
marcodesilva 0:46ef60040f6a 468 A0(1,5) = t20*t89+t9*t112;
marcodesilva 0:46ef60040f6a 469 A0(1,6) = t51*t107+t52*t108;
marcodesilva 0:46ef60040f6a 470 A0(2,1) = t27*1.0-t32+t43+t71+t86+t100*1.0+t102+t111*1.0+a5*t69+t2*x_0_r2*1.0-t8*y_0_r2*1.0;
marcodesilva 0:46ef60040f6a 471 A0(2,2) = -t8*t106;
marcodesilva 0:46ef60040f6a 472 A0(2,3) = t18*t105*1.0-t9*t115*1.0;
marcodesilva 0:46ef60040f6a 473 A0(2,4) = -t9*(t72-t73+t87-t100+t103-t111)-t18*t104;
marcodesilva 0:46ef60040f6a 474 A0(2,5) = -t18*t89+t9*t117;
marcodesilva 0:46ef60040f6a 475 A0(3,1) = 1.0;
marcodesilva 0:46ef60040f6a 476 A0(3,3) = t9*-1.0;
marcodesilva 0:46ef60040f6a 477 A0(3,4) = t9;
marcodesilva 0:46ef60040f6a 478 A0(3,5) = t9;
marcodesilva 0:46ef60040f6a 479 A0(3,6) = t3*t42*-1.0;
marcodesilva 0:46ef60040f6a 480 A0(3,7) = t9*t13*-1.0+t3*t7*cos(t30);
marcodesilva 0:46ef60040f6a 481 A0(4,2) = t8;
marcodesilva 0:46ef60040f6a 482 A0(4,3) = t18*-1.0;
marcodesilva 0:46ef60040f6a 483 A0(4,4) = t18;
marcodesilva 0:46ef60040f6a 484 A0(4,5) = t18;
marcodesilva 0:46ef60040f6a 485 A0(4,6) = t109;
marcodesilva 0:46ef60040f6a 486 A0(4,7) = t7*(t6*t83*1.0-t12*t84*1.0)*-1.0-t13*t18*1.0;
marcodesilva 0:46ef60040f6a 487 A0(5,2) = t2*-1.0;
marcodesilva 0:46ef60040f6a 488 A0(5,3) = t20*-1.0;
marcodesilva 0:46ef60040f6a 489 A0(5,4) = t20;
marcodesilva 0:46ef60040f6a 490 A0(5,5) = t20;
marcodesilva 0:46ef60040f6a 491 A0(5,6) = -t90+t91;
marcodesilva 0:46ef60040f6a 492 A0(5,7) = t13*t20*-1.0-t7*(t6*t81+t12*t79)*1.0;
marcodesilva 0:46ef60040f6a 493 return A0;
marcodesilva 0:46ef60040f6a 494 }
marcodesilva 0:46ef60040f6a 495
marcodesilva 0:46ef60040f6a 496
marcodesilva 0:46ef60040f6a 497 //Jacobian matrix J
marcodesilva 0:46ef60040f6a 498 Matrix<float,6,8> ARAP180_WITH_ROVER::jacobianTimeDerivativeMatrix(Vector8f q, Vector8f dq){
marcodesilva 0:46ef60040f6a 499 volatile float q1 = q[0];
marcodesilva 0:46ef60040f6a 500 volatile float q2 = q[1];
marcodesilva 0:46ef60040f6a 501 volatile float q3 = q[2];
marcodesilva 0:46ef60040f6a 502 volatile float q4 = q[3];
marcodesilva 0:46ef60040f6a 503 volatile float q5 = q[4];
marcodesilva 0:46ef60040f6a 504 volatile float q6 = q[5];
marcodesilva 0:46ef60040f6a 505 volatile float q7 = q[6];
marcodesilva 0:46ef60040f6a 506 volatile float q8 = q[7];
marcodesilva 0:46ef60040f6a 507
marcodesilva 0:46ef60040f6a 508 volatile float dq1 = dq[0];
marcodesilva 0:46ef60040f6a 509 volatile float dq2 = dq[1];
marcodesilva 0:46ef60040f6a 510 volatile float dq3 = dq[2];
marcodesilva 0:46ef60040f6a 511 volatile float dq4 = dq[3];
marcodesilva 0:46ef60040f6a 512 volatile float dq5 = dq[4];
marcodesilva 0:46ef60040f6a 513 volatile float dq6 = dq[5];
marcodesilva 0:46ef60040f6a 514 volatile float dq7 = dq[6];
marcodesilva 0:46ef60040f6a 515 volatile float dq8 = dq[7];
marcodesilva 0:46ef60040f6a 516
marcodesilva 0:46ef60040f6a 517 Matrix<float,6,8> A0 = Matrix<float,6,8>::Zero();
marcodesilva 0:46ef60040f6a 518
marcodesilva 0:46ef60040f6a 519
marcodesilva 0:46ef60040f6a 520 return A0;
marcodesilva 0:46ef60040f6a 521 }
marcodesilva 0:46ef60040f6a 522
marcodesilva 0:46ef60040f6a 523
marcodesilva 0:46ef60040f6a 524 Vector6f ARAP180_WITH_ROVER::Gr(Vector8f q, Vector3f droneOrientation){
marcodesilva 0:46ef60040f6a 525
marcodesilva 0:46ef60040f6a 526 Vector6f A0 = Vector6f::Zero();
marcodesilva 0:46ef60040f6a 527 volatile float q1 = 0.0; //q[0]; Per evitare che il drone si sposti nel calcolo della gravità rispetto alla terna drone fissa sul tubo
marcodesilva 0:46ef60040f6a 528 volatile float q2 = q[1];
marcodesilva 0:46ef60040f6a 529 volatile float q3 = q[2];
marcodesilva 0:46ef60040f6a 530 volatile float q4 = q[3];
marcodesilva 0:46ef60040f6a 531 volatile float q5 = q[4];
marcodesilva 0:46ef60040f6a 532 volatile float q6 = q[5];
marcodesilva 0:46ef60040f6a 533 volatile float q7 = q[6];
marcodesilva 0:46ef60040f6a 534 volatile float q8 = q[7];
marcodesilva 0:46ef60040f6a 535
marcodesilva 0:46ef60040f6a 536 float r_b_d = -droneOrientation[0];
marcodesilva 0:46ef60040f6a 537 float p_b_d = -droneOrientation[1];
marcodesilva 0:46ef60040f6a 538
marcodesilva 0:46ef60040f6a 539 float t2 = cos(p_b_d);
marcodesilva 0:46ef60040f6a 540 float t3 = cos(q2);
marcodesilva 0:46ef60040f6a 541 float t4 = cos(q3);
marcodesilva 0:46ef60040f6a 542 float t5 = cos(q4);
marcodesilva 0:46ef60040f6a 543 float t6 = cos(q5);
marcodesilva 0:46ef60040f6a 544 float t7 = cos(q6);
marcodesilva 0:46ef60040f6a 545 float t8 = cos(q7);
marcodesilva 0:46ef60040f6a 546 float t9 = cos(q8);
marcodesilva 0:46ef60040f6a 547 float t10 = cos(r_b_d);
marcodesilva 0:46ef60040f6a 548 float t11 = sin(p_b_d);
marcodesilva 0:46ef60040f6a 549 float t12 = sin(q2);
marcodesilva 0:46ef60040f6a 550 float t13 = sin(q3);
marcodesilva 0:46ef60040f6a 551 float t14 = sin(q4);
marcodesilva 0:46ef60040f6a 552 float t15 = sin(q5);
marcodesilva 0:46ef60040f6a 553 float t16 = sin(q6);
marcodesilva 0:46ef60040f6a 554 float t17 = sin(q7);
marcodesilva 0:46ef60040f6a 555 float t18 = sin(q8);
marcodesilva 0:46ef60040f6a 556 float t19 = sin(r_b_d);
marcodesilva 0:46ef60040f6a 557 float t20 = t3*t4*t19;
marcodesilva 0:46ef60040f6a 558 float t21 = t10*t11*t13;
marcodesilva 0:46ef60040f6a 559 float t22 = t2*t4*t10*t12;
marcodesilva 0:46ef60040f6a 560 float t24 = t12*t19*9.814;
marcodesilva 0:46ef60040f6a 561 float t25 = t2*t3*t10*9.814;
marcodesilva 0:46ef60040f6a 562 float t26 = t4*t10*t11*9.814;
marcodesilva 0:46ef60040f6a 563 float t27 = t3*t13*t19*9.814;
marcodesilva 0:46ef60040f6a 564 float t28 = t2*t10*t12*t13*9.814;
marcodesilva 0:46ef60040f6a 565 float t62 = mpx4*t5*t6*t7*t12*t19*(-9.814);
marcodesilva 0:46ef60040f6a 566 float t63 = mpx4*t6*t12*t14*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 567 float t64 = mpx4*t7*t12*t14*t15*t19*(-9.814);
marcodesilva 0:46ef60040f6a 568 float t117 = a6*mass5*t5*t6*t7*t12*t19*(-9.814);
marcodesilva 0:46ef60040f6a 569 float t118 = a6*mass6*t5*t6*t7*t12*t19*(-9.814);
marcodesilva 0:46ef60040f6a 570 float t119 = d7*mass6*t6*t7*t12*t14*t19*(-9.814);
marcodesilva 0:46ef60040f6a 571 float t120 = a6*mass5*t6*t12*t14*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 572 float t121 = a6*mass5*t7*t12*t14*t15*t19*(-9.814);
marcodesilva 0:46ef60040f6a 573 float t122 = a6*mass6*t6*t12*t14*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 574 float t123 = a6*mass6*t7*t12*t14*t15*t19*(-9.814);
marcodesilva 0:46ef60040f6a 575 float t140 = mpx4*t4*t10*t11*t14*t15*t16*(-9.814);
marcodesilva 0:46ef60040f6a 576 float t141 = mpx4*t3*t13*t14*t15*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 577 float t142 = mpy6*t9*t12*t14*t15*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 578 float t191 = mpx4*t2*t3*t5*t6*t7*t10*(-9.814);
marcodesilva 0:46ef60040f6a 579 float t192 = mpx4*t2*t3*t6*t10*t14*t16*(-9.814);
marcodesilva 0:46ef60040f6a 580 float t193 = mpx4*t2*t3*t7*t10*t14*t15*(-9.814);
marcodesilva 0:46ef60040f6a 581 float t194 = mpx4*t4*t5*t6*t10*t11*t16*(-9.814);
marcodesilva 0:46ef60040f6a 582 float t195 = mpx4*t4*t5*t7*t10*t11*t15*(-9.814);
marcodesilva 0:46ef60040f6a 583 float t196 = mpx4*t3*t5*t6*t13*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 584 float t197 = mpx4*t3*t5*t7*t13*t15*t19*(-9.814);
marcodesilva 0:46ef60040f6a 585 float t198 = mpy6*t5*t6*t9*t12*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 586 float t199 = mpy6*t5*t7*t9*t12*t15*t19*(-9.814);
marcodesilva 0:46ef60040f6a 587 float t200 = a6*mass5*t2*t3*t5*t6*t7*t10*(-9.814);
marcodesilva 0:46ef60040f6a 588 float t201 = a6*mass6*t2*t3*t5*t6*t7*t10*(-9.814);
marcodesilva 0:46ef60040f6a 589 float t202 = d7*mass6*t2*t3*t6*t7*t10*t14*(-9.814);
marcodesilva 0:46ef60040f6a 590 float t203 = d7*mass6*t4*t5*t6*t7*t10*t11*(-9.814);
marcodesilva 0:46ef60040f6a 591 float t204 = a6*mass5*t2*t3*t6*t10*t14*t16*(-9.814);
marcodesilva 0:46ef60040f6a 592 float t205 = a6*mass5*t2*t3*t7*t10*t14*t15*(-9.814);
marcodesilva 0:46ef60040f6a 593 float t206 = a6*mass5*t4*t5*t6*t10*t11*t16*(-9.814);
marcodesilva 0:46ef60040f6a 594 float t207 = a6*mass5*t4*t5*t7*t10*t11*t15*(-9.814);
marcodesilva 0:46ef60040f6a 595 float t208 = a6*mass6*t2*t3*t6*t10*t14*t16*(-9.814);
marcodesilva 0:46ef60040f6a 596 float t209 = a6*mass6*t2*t3*t7*t10*t14*t15*(-9.814);
marcodesilva 0:46ef60040f6a 597 float t210 = a6*mass6*t4*t5*t6*t10*t11*t16*(-9.814);
marcodesilva 0:46ef60040f6a 598 float t211 = a6*mass6*t4*t5*t7*t10*t11*t15*(-9.814);
marcodesilva 0:46ef60040f6a 599 float t212 = d7*mass6*t3*t5*t6*t7*t13*t19*(-9.814);
marcodesilva 0:46ef60040f6a 600 float t213 = a6*mass5*t3*t5*t6*t13*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 601 float t214 = a6*mass5*t3*t5*t7*t13*t15*t19*(-9.814);
marcodesilva 0:46ef60040f6a 602 float t215 = a6*mass6*t3*t5*t6*t13*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 603 float t216 = a6*mass6*t3*t5*t7*t13*t15*t19*(-9.814);
marcodesilva 0:46ef60040f6a 604 float t217 = d7*mass6*t4*t6*t10*t11*t14*t16*(-9.814);
marcodesilva 0:46ef60040f6a 605 float t218 = d7*mass6*t4*t7*t10*t11*t14*t15*(-9.814);
marcodesilva 0:46ef60040f6a 606 float t219 = a6*mass5*t4*t10*t11*t14*t15*t16*(-9.814);
marcodesilva 0:46ef60040f6a 607 float t220 = a6*mass6*t4*t10*t11*t14*t15*t16*(-9.814);
marcodesilva 0:46ef60040f6a 608 float t221 = d7*mass6*t3*t6*t13*t14*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 609 float t222 = d7*mass6*t3*t7*t13*t14*t15*t19*(-9.814);
marcodesilva 0:46ef60040f6a 610 float t223 = a6*mass5*t3*t13*t14*t15*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 611 float t224 = a6*mass6*t3*t13*t14*t15*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 612 float t257 = mpy6*t2*t3*t5*t6*t9*t10*t16*(-9.814);
marcodesilva 0:46ef60040f6a 613 float t258 = mpy6*t2*t3*t5*t7*t9*t10*t15*(-9.814);
marcodesilva 0:46ef60040f6a 614 float t259 = mpx4*t2*t6*t7*t10*t12*t13*t14*(-9.814);
marcodesilva 0:46ef60040f6a 615 float t260 = mpy6*t2*t3*t9*t10*t14*t15*t16*(-9.814);
marcodesilva 0:46ef60040f6a 616 float t261 = mpy6*t4*t5*t9*t10*t11*t15*t16*(-9.814);
marcodesilva 0:46ef60040f6a 617 float t262 = mpy6*t3*t5*t9*t13*t15*t16*t19*(-9.814);
marcodesilva 0:46ef60040f6a 618 float t263 = mpy6*t5*t12*t15*t16*t17*t18*t19*(-9.814);
marcodesilva 0:46ef60040f6a 619 float t265 = a6*mass5*t2*t6*t7*t10*t12*t13*t14*(-9.814);
marcodesilva 0:46ef60040f6a 620 float t266 = a6*mass6*t2*t6*t7*t10*t12*t13*t14*(-9.814);
marcodesilva 0:46ef60040f6a 621 float t267 = d7*mass6*t2*t5*t10*t12*t13*t15*t16*(-9.814);
marcodesilva 0:46ef60040f6a 622 float t284 = mpy6*t2*t5*t6*t7*t9*t10*t12*t13*(-9.814);
marcodesilva 0:46ef60040f6a 623 float t285 = mpy6*t2*t6*t9*t10*t12*t13*t14*t16*(-9.814);
marcodesilva 0:46ef60040f6a 624 float t286 = mpy6*t2*t7*t9*t10*t12*t13*t14*t15*(-9.814);
marcodesilva 0:46ef60040f6a 625 float t287 = mpy6*t2*t3*t5*t10*t15*t16*t17*t18*(-9.814);
marcodesilva 0:46ef60040f6a 626 float t288 = mpy6*t4*t6*t7*t10*t11*t14*t17*t18*(-9.814);
marcodesilva 0:46ef60040f6a 627 float t289 = mpy6*t3*t6*t7*t13*t14*t17*t18*t19*(-9.814);
marcodesilva 0:46ef60040f6a 628 float t295 = mpy6*t2*t5*t6*t10*t12*t13*t16*t17*t18*(-9.814);
marcodesilva 0:46ef60040f6a 629 float t296 = mpy6*t2*t5*t7*t10*t12*t13*t15*t17*t18*(-9.814);
marcodesilva 0:46ef60040f6a 630 float t297 = mpy6*t2*t10*t12*t13*t14*t15*t16*t17*t18*(-9.814);
marcodesilva 0:46ef60040f6a 631 float t23 = -t20;
marcodesilva 0:46ef60040f6a 632 float t29 = mpx3*t5*t6*t24;
marcodesilva 0:46ef60040f6a 633 float t30 = mpy3*t5*t15*t24;
marcodesilva 0:46ef60040f6a 634 float t31 = mpy3*t6*t14*t24;
marcodesilva 0:46ef60040f6a 635 float t32 = mpx3*t14*t15*t24;
marcodesilva 0:46ef60040f6a 636 float t33 = -t28;
marcodesilva 0:46ef60040f6a 637 float t34 = a5*mass4*t5*t6*t24;
marcodesilva 0:46ef60040f6a 638 float t35 = a5*mass5*t5*t6*t24;
marcodesilva 0:46ef60040f6a 639 float t36 = a5*mass6*t5*t6*t24;
marcodesilva 0:46ef60040f6a 640 float t37 = a5i*mass4*t5*t15*t24;
marcodesilva 0:46ef60040f6a 641 float t38 = a5i*mass4*t6*t14*t24;
marcodesilva 0:46ef60040f6a 642 float t39 = a5i*mass5*t5*t15*t24;
marcodesilva 0:46ef60040f6a 643 float t40 = a5i*mass5*t6*t14*t24;
marcodesilva 0:46ef60040f6a 644 float t41 = a5i*mass6*t5*t15*t24;
marcodesilva 0:46ef60040f6a 645 float t42 = a5i*mass6*t6*t14*t24;
marcodesilva 0:46ef60040f6a 646 float t43 = a5*mass4*t14*t15*t24;
marcodesilva 0:46ef60040f6a 647 float t44 = a5*mass5*t14*t15*t24;
marcodesilva 0:46ef60040f6a 648 float t45 = a5*mass6*t14*t15*t24;
marcodesilva 0:46ef60040f6a 649 float t46 = mpy3*t5*t15*t25;
marcodesilva 0:46ef60040f6a 650 float t47 = mpy3*t6*t14*t25;
marcodesilva 0:46ef60040f6a 651 float t48 = mpy3*t5*t6*t26;
marcodesilva 0:46ef60040f6a 652 float t49 = mpx3*t14*t15*t25;
marcodesilva 0:46ef60040f6a 653 float t50 = mpx3*t5*t15*t26;
marcodesilva 0:46ef60040f6a 654 float t51 = mpx3*t6*t14*t26;
marcodesilva 0:46ef60040f6a 655 float t52 = mpx4*t5*t6*t7*t24;
marcodesilva 0:46ef60040f6a 656 float t53 = mpy3*t5*t6*t27;
marcodesilva 0:46ef60040f6a 657 float t54 = mpy3*t14*t15*t26;
marcodesilva 0:46ef60040f6a 658 float t55 = mpx3*t5*t15*t27;
marcodesilva 0:46ef60040f6a 659 float t56 = mpx3*t6*t14*t27;
marcodesilva 0:46ef60040f6a 660 float t57 = mpx4*t5*t15*t16*t24;
marcodesilva 0:46ef60040f6a 661 float t58 = mpx4*t6*t14*t16*t24;
marcodesilva 0:46ef60040f6a 662 float t59 = mpx4*t7*t14*t15*t24;
marcodesilva 0:46ef60040f6a 663 float t60 = mpy3*t14*t15*t27;
marcodesilva 0:46ef60040f6a 664 float t61 = mpx3*t5*t6*t25;
marcodesilva 0:46ef60040f6a 665 float t65 = a5*mass4*t5*t6*t25;
marcodesilva 0:46ef60040f6a 666 float t66 = a5*mass5*t5*t6*t25;
marcodesilva 0:46ef60040f6a 667 float t67 = a5*mass6*t5*t6*t25;
marcodesilva 0:46ef60040f6a 668 float t68 = a5i*mass4*t5*t15*t25;
marcodesilva 0:46ef60040f6a 669 float t69 = a5i*mass4*t6*t14*t25;
marcodesilva 0:46ef60040f6a 670 float t70 = a5i*mass4*t5*t6*t26;
marcodesilva 0:46ef60040f6a 671 float t71 = a5i*mass5*t5*t15*t25;
marcodesilva 0:46ef60040f6a 672 float t72 = a5i*mass5*t6*t14*t25;
marcodesilva 0:46ef60040f6a 673 float t73 = a5i*mass5*t5*t6*t26;
marcodesilva 0:46ef60040f6a 674 float t74 = a5i*mass6*t5*t15*t25;
marcodesilva 0:46ef60040f6a 675 float t75 = a5i*mass6*t6*t14*t25;
marcodesilva 0:46ef60040f6a 676 float t76 = a5i*mass6*t5*t6*t26;
marcodesilva 0:46ef60040f6a 677 float t77 = a5*mass4*t14*t15*t25;
marcodesilva 0:46ef60040f6a 678 float t78 = a5*mass4*t5*t15*t26;
marcodesilva 0:46ef60040f6a 679 float t79 = a5*mass4*t6*t14*t26;
marcodesilva 0:46ef60040f6a 680 float t80 = a5*mass5*t14*t15*t25;
marcodesilva 0:46ef60040f6a 681 float t81 = a5*mass5*t5*t15*t26;
marcodesilva 0:46ef60040f6a 682 float t82 = a5*mass5*t6*t14*t26;
marcodesilva 0:46ef60040f6a 683 float t83 = a5*mass6*t14*t15*t25;
marcodesilva 0:46ef60040f6a 684 float t84 = a5*mass6*t5*t15*t26;
marcodesilva 0:46ef60040f6a 685 float t85 = a5*mass6*t6*t14*t26;
marcodesilva 0:46ef60040f6a 686 float t86 = a6*mass5*t5*t6*t7*t24;
marcodesilva 0:46ef60040f6a 687 float t87 = a6*mass6*t5*t6*t7*t24;
marcodesilva 0:46ef60040f6a 688 float t88 = a5i*mass4*t5*t6*t27;
marcodesilva 0:46ef60040f6a 689 float t89 = a5i*mass5*t5*t6*t27;
marcodesilva 0:46ef60040f6a 690 float t90 = a5i*mass6*t5*t6*t27;
marcodesilva 0:46ef60040f6a 691 float t91 = a5i*mass4*t14*t15*t26;
marcodesilva 0:46ef60040f6a 692 float t92 = a5i*mass5*t14*t15*t26;
marcodesilva 0:46ef60040f6a 693 float t93 = a5i*mass6*t14*t15*t26;
marcodesilva 0:46ef60040f6a 694 float t94 = a5*mass4*t5*t15*t27;
marcodesilva 0:46ef60040f6a 695 float t95 = a5*mass4*t6*t14*t27;
marcodesilva 0:46ef60040f6a 696 float t96 = a5*mass5*t5*t15*t27;
marcodesilva 0:46ef60040f6a 697 float t97 = a5*mass5*t6*t14*t27;
marcodesilva 0:46ef60040f6a 698 float t98 = a5*mass6*t5*t15*t27;
marcodesilva 0:46ef60040f6a 699 float t99 = a5*mass6*t6*t14*t27;
marcodesilva 0:46ef60040f6a 700 float t100 = d7*mass6*t5*t6*t16*t24;
marcodesilva 0:46ef60040f6a 701 float t101 = d7*mass6*t5*t7*t15*t24;
marcodesilva 0:46ef60040f6a 702 float t102 = d7*mass6*t6*t7*t14*t24;
marcodesilva 0:46ef60040f6a 703 float t103 = a6*mass5*t5*t15*t16*t24;
marcodesilva 0:46ef60040f6a 704 float t104 = a6*mass5*t6*t14*t16*t24;
marcodesilva 0:46ef60040f6a 705 float t105 = a6*mass5*t7*t14*t15*t24;
marcodesilva 0:46ef60040f6a 706 float t106 = a6*mass6*t5*t15*t16*t24;
marcodesilva 0:46ef60040f6a 707 float t107 = a6*mass6*t6*t14*t16*t24;
marcodesilva 0:46ef60040f6a 708 float t108 = a6*mass6*t7*t14*t15*t24;
marcodesilva 0:46ef60040f6a 709 float t109 = a5i*mass4*t14*t15*t27;
marcodesilva 0:46ef60040f6a 710 float t110 = a5i*mass5*t14*t15*t27;
marcodesilva 0:46ef60040f6a 711 float t111 = a5i*mass6*t14*t15*t27;
marcodesilva 0:46ef60040f6a 712 float t112 = d7*mass6*t14*t15*t16*t24;
marcodesilva 0:46ef60040f6a 713 float t113 = mpx4*t14*t15*t16*t26;
marcodesilva 0:46ef60040f6a 714 float t114 = mpy3*t14*t15*t28;
marcodesilva 0:46ef60040f6a 715 float t115 = mpx4*t14*t15*t16*t27;
marcodesilva 0:46ef60040f6a 716 float t116 = mpy6*t9*t14*t15*t16*t24;
marcodesilva 0:46ef60040f6a 717 float t124 = mpx4*t5*t6*t7*t25;
marcodesilva 0:46ef60040f6a 718 float t125 = mpx4*t5*t15*t16*t25;
marcodesilva 0:46ef60040f6a 719 float t126 = mpx4*t6*t14*t16*t25;
marcodesilva 0:46ef60040f6a 720 float t127 = mpx4*t7*t14*t15*t25;
marcodesilva 0:46ef60040f6a 721 float t128 = mpx4*t5*t6*t16*t26;
marcodesilva 0:46ef60040f6a 722 float t129 = mpx4*t5*t7*t15*t26;
marcodesilva 0:46ef60040f6a 723 float t130 = mpx4*t6*t7*t14*t26;
marcodesilva 0:46ef60040f6a 724 float t131 = mpy3*t5*t6*t28;
marcodesilva 0:46ef60040f6a 725 float t132 = mpx3*t5*t15*t28;
marcodesilva 0:46ef60040f6a 726 float t133 = mpx3*t6*t14*t28;
marcodesilva 0:46ef60040f6a 727 float t134 = mpx4*t5*t6*t16*t27;
marcodesilva 0:46ef60040f6a 728 float t135 = mpx4*t5*t7*t15*t27;
marcodesilva 0:46ef60040f6a 729 float t136 = mpx4*t6*t7*t14*t27;
marcodesilva 0:46ef60040f6a 730 float t137 = mpy6*t5*t6*t9*t16*t24;
marcodesilva 0:46ef60040f6a 731 float t138 = mpy6*t5*t7*t9*t15*t24;
marcodesilva 0:46ef60040f6a 732 float t139 = mpy6*t6*t7*t9*t14*t24;
marcodesilva 0:46ef60040f6a 733 float t143 = a6*mass5*t5*t6*t7*t25;
marcodesilva 0:46ef60040f6a 734 float t144 = a6*mass6*t5*t6*t7*t25;
marcodesilva 0:46ef60040f6a 735 float t145 = d7*mass6*t5*t6*t16*t25;
marcodesilva 0:46ef60040f6a 736 float t146 = d7*mass6*t5*t7*t15*t25;
marcodesilva 0:46ef60040f6a 737 float t147 = d7*mass6*t6*t7*t14*t25;
marcodesilva 0:46ef60040f6a 738 float t148 = d7*mass6*t5*t6*t7*t26;
marcodesilva 0:46ef60040f6a 739 float t149 = a6*mass5*t5*t15*t16*t25;
marcodesilva 0:46ef60040f6a 740 float t150 = a6*mass5*t6*t14*t16*t25;
marcodesilva 0:46ef60040f6a 741 float t151 = a6*mass5*t7*t14*t15*t25;
marcodesilva 0:46ef60040f6a 742 float t152 = a6*mass5*t5*t6*t16*t26;
marcodesilva 0:46ef60040f6a 743 float t153 = a6*mass5*t5*t7*t15*t26;
marcodesilva 0:46ef60040f6a 744 float t154 = a6*mass5*t6*t7*t14*t26;
marcodesilva 0:46ef60040f6a 745 float t155 = a6*mass6*t5*t15*t16*t25;
marcodesilva 0:46ef60040f6a 746 float t156 = a6*mass6*t6*t14*t16*t25;
marcodesilva 0:46ef60040f6a 747 float t157 = a6*mass6*t7*t14*t15*t25;
marcodesilva 0:46ef60040f6a 748 float t158 = a6*mass6*t5*t6*t16*t26;
marcodesilva 0:46ef60040f6a 749 float t159 = a6*mass6*t5*t7*t15*t26;
marcodesilva 0:46ef60040f6a 750 float t160 = a6*mass6*t6*t7*t14*t26;
marcodesilva 0:46ef60040f6a 751 float t161 = a5i*mass4*t5*t6*t28;
marcodesilva 0:46ef60040f6a 752 float t162 = a5i*mass5*t5*t6*t28;
marcodesilva 0:46ef60040f6a 753 float t163 = a5i*mass6*t5*t6*t28;
marcodesilva 0:46ef60040f6a 754 float t164 = d7*mass6*t5*t6*t7*t27;
marcodesilva 0:46ef60040f6a 755 float t165 = a5*mass4*t5*t15*t28;
marcodesilva 0:46ef60040f6a 756 float t166 = a5*mass4*t6*t14*t28;
marcodesilva 0:46ef60040f6a 757 float t167 = a5*mass5*t5*t15*t28;
marcodesilva 0:46ef60040f6a 758 float t168 = a5*mass5*t6*t14*t28;
marcodesilva 0:46ef60040f6a 759 float t169 = a5*mass6*t5*t15*t28;
marcodesilva 0:46ef60040f6a 760 float t170 = a5*mass6*t6*t14*t28;
marcodesilva 0:46ef60040f6a 761 float t171 = a6*mass5*t5*t6*t16*t27;
marcodesilva 0:46ef60040f6a 762 float t172 = a6*mass5*t5*t7*t15*t27;
marcodesilva 0:46ef60040f6a 763 float t173 = a6*mass5*t6*t7*t14*t27;
marcodesilva 0:46ef60040f6a 764 float t174 = a6*mass6*t5*t6*t16*t27;
marcodesilva 0:46ef60040f6a 765 float t175 = a6*mass6*t5*t7*t15*t27;
marcodesilva 0:46ef60040f6a 766 float t176 = a6*mass6*t6*t7*t14*t27;
marcodesilva 0:46ef60040f6a 767 float t177 = d7*mass6*t14*t15*t16*t25;
marcodesilva 0:46ef60040f6a 768 float t178 = d7*mass6*t5*t15*t16*t26;
marcodesilva 0:46ef60040f6a 769 float t179 = d7*mass6*t6*t14*t16*t26;
marcodesilva 0:46ef60040f6a 770 float t180 = d7*mass6*t7*t14*t15*t26;
marcodesilva 0:46ef60040f6a 771 float t181 = a6*mass5*t14*t15*t16*t26;
marcodesilva 0:46ef60040f6a 772 float t182 = a6*mass6*t14*t15*t16*t26;
marcodesilva 0:46ef60040f6a 773 float t183 = a5i*mass4*t14*t15*t28;
marcodesilva 0:46ef60040f6a 774 float t184 = a5i*mass5*t14*t15*t28;
marcodesilva 0:46ef60040f6a 775 float t185 = a5i*mass6*t14*t15*t28;
marcodesilva 0:46ef60040f6a 776 float t186 = d7*mass6*t5*t15*t16*t27;
marcodesilva 0:46ef60040f6a 777 float t187 = d7*mass6*t6*t14*t16*t27;
marcodesilva 0:46ef60040f6a 778 float t188 = d7*mass6*t7*t14*t15*t27;
marcodesilva 0:46ef60040f6a 779 float t189 = a6*mass5*t14*t15*t16*t27;
marcodesilva 0:46ef60040f6a 780 float t190 = a6*mass6*t14*t15*t16*t27;
marcodesilva 0:46ef60040f6a 781 float t225 = mpy6*t5*t6*t9*t16*t25;
marcodesilva 0:46ef60040f6a 782 float t226 = mpy6*t5*t7*t9*t15*t25;
marcodesilva 0:46ef60040f6a 783 float t227 = mpy6*t6*t7*t9*t14*t25;
marcodesilva 0:46ef60040f6a 784 float t228 = mpy6*t5*t6*t7*t9*t26;
marcodesilva 0:46ef60040f6a 785 float t229 = mpy6*t5*t6*t7*t9*t27;
marcodesilva 0:46ef60040f6a 786 float t230 = mpx4*t5*t6*t16*t28;
marcodesilva 0:46ef60040f6a 787 float t231 = mpx4*t5*t7*t15*t28;
marcodesilva 0:46ef60040f6a 788 float t232 = mpx4*t6*t7*t14*t28;
marcodesilva 0:46ef60040f6a 789 float t233 = mpy6*t9*t14*t15*t16*t25;
marcodesilva 0:46ef60040f6a 790 float t234 = mpy6*t5*t9*t15*t16*t26;
marcodesilva 0:46ef60040f6a 791 float t235 = mpy6*t6*t9*t14*t16*t26;
marcodesilva 0:46ef60040f6a 792 float t236 = mpy6*t7*t9*t14*t15*t26;
marcodesilva 0:46ef60040f6a 793 float t237 = mpy6*t5*t9*t15*t16*t27;
marcodesilva 0:46ef60040f6a 794 float t238 = mpy6*t6*t9*t14*t16*t27;
marcodesilva 0:46ef60040f6a 795 float t239 = mpy6*t7*t9*t14*t15*t27;
marcodesilva 0:46ef60040f6a 796 float t240 = mpy6*t5*t6*t7*t17*t18*t24;
marcodesilva 0:46ef60040f6a 797 float t241 = mpx4*t14*t15*t16*t28;
marcodesilva 0:46ef60040f6a 798 float t242 = mpy6*t5*t15*t16*t17*t18*t24;
marcodesilva 0:46ef60040f6a 799 float t243 = mpy6*t6*t14*t16*t17*t18*t24;
marcodesilva 0:46ef60040f6a 800 float t244 = mpy6*t7*t14*t15*t17*t18*t24;
marcodesilva 0:46ef60040f6a 801 float t245 = d7*mass6*t5*t6*t7*t28;
marcodesilva 0:46ef60040f6a 802 float t246 = a6*mass5*t5*t6*t16*t28;
marcodesilva 0:46ef60040f6a 803 float t247 = a6*mass5*t5*t7*t15*t28;
marcodesilva 0:46ef60040f6a 804 float t248 = a6*mass5*t6*t7*t14*t28;
marcodesilva 0:46ef60040f6a 805 float t249 = a6*mass6*t5*t6*t16*t28;
marcodesilva 0:46ef60040f6a 806 float t250 = a6*mass6*t5*t7*t15*t28;
marcodesilva 0:46ef60040f6a 807 float t251 = a6*mass6*t6*t7*t14*t28;
marcodesilva 0:46ef60040f6a 808 float t252 = d7*mass6*t5*t15*t16*t28;
marcodesilva 0:46ef60040f6a 809 float t253 = d7*mass6*t6*t14*t16*t28;
marcodesilva 0:46ef60040f6a 810 float t254 = d7*mass6*t7*t14*t15*t28;
marcodesilva 0:46ef60040f6a 811 float t255 = a6*mass5*t14*t15*t16*t28;
marcodesilva 0:46ef60040f6a 812 float t256 = a6*mass6*t14*t15*t16*t28;
marcodesilva 0:46ef60040f6a 813 float t268 = mpy6*t5*t6*t7*t9*t28;
marcodesilva 0:46ef60040f6a 814 float t269 = mpy6*t5*t6*t7*t17*t18*t25;
marcodesilva 0:46ef60040f6a 815 float t270 = mpy6*t5*t9*t15*t16*t28;
marcodesilva 0:46ef60040f6a 816 float t271 = mpy6*t6*t9*t14*t16*t28;
marcodesilva 0:46ef60040f6a 817 float t272 = mpy6*t7*t9*t14*t15*t28;
marcodesilva 0:46ef60040f6a 818 float t273 = mpy6*t5*t15*t16*t17*t18*t25;
marcodesilva 0:46ef60040f6a 819 float t274 = mpy6*t6*t14*t16*t17*t18*t25;
marcodesilva 0:46ef60040f6a 820 float t275 = mpy6*t7*t14*t15*t17*t18*t25;
marcodesilva 0:46ef60040f6a 821 float t276 = mpy6*t5*t6*t16*t17*t18*t26;
marcodesilva 0:46ef60040f6a 822 float t277 = mpy6*t5*t7*t15*t17*t18*t26;
marcodesilva 0:46ef60040f6a 823 float t278 = mpy6*t6*t7*t14*t17*t18*t26;
marcodesilva 0:46ef60040f6a 824 float t279 = mpy6*t5*t6*t16*t17*t18*t27;
marcodesilva 0:46ef60040f6a 825 float t280 = mpy6*t5*t7*t15*t17*t18*t27;
marcodesilva 0:46ef60040f6a 826 float t281 = mpy6*t6*t7*t14*t17*t18*t27;
marcodesilva 0:46ef60040f6a 827 float t282 = mpy6*t14*t15*t16*t17*t18*t26;
marcodesilva 0:46ef60040f6a 828 float t283 = mpy6*t14*t15*t16*t17*t18*t27;
marcodesilva 0:46ef60040f6a 829 float t290 = mpy6*t5*t6*t16*t17*t18*t28;
marcodesilva 0:46ef60040f6a 830 float t291 = mpy6*t5*t7*t15*t17*t18*t28;
marcodesilva 0:46ef60040f6a 831 float t292 = mpy6*t6*t7*t14*t17*t18*t28;
marcodesilva 0:46ef60040f6a 832 float t293 = mpy6*t14*t15*t16*t17*t18*t28;
marcodesilva 0:46ef60040f6a 833 float t294 = t24+t25;
marcodesilva 0:46ef60040f6a 834 float t264 = t21+t22+t23;
marcodesilva 0:46ef60040f6a 835 float t298 = t5*t294;
marcodesilva 0:46ef60040f6a 836 float t299 = t14*t294;
marcodesilva 0:46ef60040f6a 837 float t301 = t26+t27+t33;
marcodesilva 0:46ef60040f6a 838 float t300 = -t298;
marcodesilva 0:46ef60040f6a 839 float t302 = t5*t301;
marcodesilva 0:46ef60040f6a 840 float t303 = t14*t301;
marcodesilva 0:46ef60040f6a 841 float t304 = t299+t302;
marcodesilva 0:46ef60040f6a 842 float t305 = t300+t303;
marcodesilva 0:46ef60040f6a 843 float t308 = -t6*(t298-t303);
marcodesilva 0:46ef60040f6a 844 float t309 = -t15*(t298-t303);
marcodesilva 0:46ef60040f6a 845 float t306 = t6*t304;
marcodesilva 0:46ef60040f6a 846 float t307 = t15*t304;
marcodesilva 0:46ef60040f6a 847 float t310 = -t307;
marcodesilva 0:46ef60040f6a 848 float t311 = t306+t309;
marcodesilva 0:46ef60040f6a 849 float t314 = -t16*(t307+t6*(t298-t303));
marcodesilva 0:46ef60040f6a 850 float t312 = t308+t310;
marcodesilva 0:46ef60040f6a 851 float t313 = t7*t311;
marcodesilva 0:46ef60040f6a 852 float t315 = t313+t314;
marcodesilva 0:46ef60040f6a 853 A0(0) = t10*t11*(mass1+mass2+mass3+mass4+mass5+mass6+massr)*9.814;
marcodesilva 0:46ef60040f6a 854 A0(1) = mpxr*t24+mpxr*t25+t13*t30+t13*t38+t13*t40+t13*t42+t13*t46+t13*t57+t13*t62+t13*t63+t13*t64+t13*t69+t13*t72+t13*t75+t13*t100+t13*t101+t13*t103+t13*t106+t13*t112+t13*t117+t13*t118+t13*t119+t13*t120+t13*t121+t13*t122+t13*t123+t13*t125+t13*t139+t13*t142+t13*t145+t13*t146+t13*t149+t13*t155+t13*t177+t13*t191+t13*t192+t13*t193+t13*t198+t13*t199+t13*t200+t13*t201+t13*t202+t13*t204+t13*t205+t13*t208+t13*t209+t13*t227+t13*t240+t13*t243+t13*t244+t13*t257+t13*t258+t13*t260+t13*t263+t13*t269+t13*t274+t13*t275+t13*t287+mpy1*t3*t19*9.814+mpyr*t3*t19*9.814+mass1*t24*x_0_r2+mass1*t25*x_0_r2+mass2*t24*x_0_r2+mass2*t25*x_0_r2+mass3*t24*x_0_r2+mass3*t25*x_0_r2+mass4*t24*x_0_r2+mass4*t25*x_0_r2+mass5*t24*x_0_r2+mass5*t25*x_0_r2+mass6*t24*x_0_r2+mass6*t25*x_0_r2-mpx2*t3*t5*t19*9.814-mpx1*t12*t13*t19*9.814-mpy1*t2*t10*t12*9.814-mpy2*t3*t14*t19*9.814+mpy2*t5*t13*t24+mpy2*t5*t13*t25-mpyr*t2*t10*t12*9.814+mass1*t3*t19*y_0_r2*9.814+mass2*t3*t19*y_0_r2*9.814+mass3*t3*t19*y_0_r2*9.814+mass4*t3*t19*y_0_r2*9.814+mass5*t3*t19*y_0_r2*9.814+mass6*t3*t19*y_0_r2*9.814-a4*mass3*t3*t5*t19*9.814-a4*mass4*t3*t5*t19*9.814-a4*mass5*t3*t5*t19*9.814-a4*mass6*t3*t5*t19*9.814-a3*mass2*t12*t13*t19*9.814-a3*mass3*t12*t13*t19*9.814-a3*mass4*t12*t13*t19*9.814-a3*mass5*t12*t13*t19*9.814-a3*mass6*t12*t13*t19*9.814-mpx1*t2*t3*t10*t13*9.814+mpx2*t2*t5*t10*t12*9.814-mpx3*t3*t5*t15*t19*9.814+mpx3*t3*t6*t14*t19*9.814-mpx2*t12*t13*t14*t19*9.814-mpy3*t3*t5*t6*t19*9.814+mpy2*t2*t10*t12*t14*9.814-mpy3*t3*t14*t15*t19*9.814-mass1*t2*t10*t12*y_0_r2*9.814-mass2*t2*t10*t12*y_0_r2*9.814-mass3*t2*t10*t12*y_0_r2*9.814-mass4*t2*t10*t12*y_0_r2*9.814-mass5*t2*t10*t12*y_0_r2*9.814-mass6*t2*t10*t12*y_0_r2*9.814-mpx2*t2*t3*t10*t13*t14*9.814+mpx3*t2*t5*t10*t12*t15*9.814-mpx3*t2*t6*t10*t12*t14*9.814-mpx4*t3*t5*t6*t16*t19*9.814-mpx4*t3*t5*t7*t15*t19*9.814+mpx4*t3*t6*t7*t14*t19*9.814-mpx3*t5*t6*t12*t13*t19*9.814-mpx4*t3*t14*t15*t16*t19*9.814-mpx3*t12*t13*t14*t15*t19*9.814+mpy3*t2*t5*t6*t10*t12*9.814+mpy3*t2*t10*t12*t14*t15*9.814-mpy3*t6*t12*t13*t14*t19*9.814-mpy6*t4*t8*t12*t18*t19*9.814-a3*mass2*t2*t3*t10*t13*9.814-a3*mass3*t2*t3*t10*t13*9.814-a3*mass4*t2*t3*t10*t13*9.814-a3*mass5*t2*t3*t10*t13*9.814+a4*mass3*t2*t5*t10*t12*9.814-a3*mass6*t2*t3*t10*t13*9.814+a4*mass4*t2*t5*t10*t12*9.814+a4*mass5*t2*t5*t10*t12*9.814+a4*mass6*t2*t5*t10*t12*9.814-a5*mass4*t3*t5*t15*t19*9.814+a5*mass4*t3*t6*t14*t19*9.814-a5*mass5*t3*t5*t15*t19*9.814+a5*mass5*t3*t6*t14*t19*9.814-a5*mass6*t3*t5*t15*t19*9.814+a5*mass6*t3*t6*t14*t19*9.814-a4*mass3*t12*t13*t14*t19*9.814-a4*mass4*t12*t13*t14*t19*9.814-a4*mass5*t12*t13*t14*t19*9.814-a4*mass6*t12*t13*t14*t19*9.814+a5i*mass4*t3*t5*t6*t19*9.814+a5i*mass5*t3*t5*t6*t19*9.814+a5i*mass6*t3*t5*t6*t19*9.814+a5i*mass4*t3*t14*t15*t19*9.814+a5i*mass5*t3*t14*t15*t19*9.814+a5i*mass6*t3*t14*t15*t19*9.814-a4*mass3*t2*t3*t10*t13*t14*9.814-a4*mass4*t2*t3*t10*t13*t14*9.814-a4*mass5*t2*t3*t10*t13*t14*9.814-a4*mass6*t2*t3*t10*t13*t14*9.814+a5*mass4*t2*t5*t10*t12*t15*9.814-a5*mass4*t2*t6*t10*t12*t14*9.814+a5*mass5*t2*t5*t10*t12*t15*9.814-a5*mass5*t2*t6*t10*t12*t14*9.814+a5*mass6*t2*t5*t10*t12*t15*9.814-a5*mass6*t2*t6*t10*t12*t14*9.814-a6*mass5*t3*t5*t6*t16*t19*9.814-a6*mass5*t3*t5*t7*t15*t19*9.814+a6*mass5*t3*t6*t7*t14*t19*9.814-a6*mass6*t3*t5*t6*t16*t19*9.814-a6*mass6*t3*t5*t7*t15*t19*9.814+a6*mass6*t3*t6*t7*t14*t19*9.814-a5*mass4*t5*t6*t12*t13*t19*9.814-a5*mass5*t5*t6*t12*t13*t19*9.814-a5*mass6*t5*t6*t12*t13*t19*9.814-a6*mass5*t3*t14*t15*t16*t19*9.814-a6*mass6*t3*t14*t15*t16*t19*9.814-a5*mass4*t12*t13*t14*t15*t19*9.814-a5*mass5*t12*t13*t14*t15*t19*9.814-a5*mass6*t12*t13*t14*t15*t19*9.814-a5i*mass4*t2*t5*t6*t10*t12*9.814-a5i*mass5*t2*t5*t6*t10*t12*9.814-a5i*mass6*t2*t5*t6*t10*t12*9.814-a5i*mass4*t2*t10*t12*t14*t15*9.814-a5i*mass5*t2*t10*t12*t14*t15*9.814-a5i*mass6*t2*t10*t12*t14*t15*9.814-a5i*mass4*t5*t12*t13*t15*t19*9.814-a5i*mass5*t5*t12*t13*t15*t19*9.814-a5i*mass6*t5*t12*t13*t15*t19*9.814-d7*mass6*t3*t5*t6*t7*t19*9.814+d7*mass6*t3*t5*t15*t16*t19*9.814-d7*mass6*t3*t6*t14*t16*t19*9.814-d7*mass6*t3*t7*t14*t15*t19*9.814-mpx3*t2*t3*t5*t6*t10*t13*9.814+mpx4*t2*t5*t6*t10*t12*t16*9.814+mpx4*t2*t5*t7*t10*t12*t15*9.814-mpx4*t2*t6*t7*t10*t12*t14*9.814-mpx3*t2*t3*t10*t13*t14*t15*9.814+mpx4*t2*t10*t12*t14*t15*t16*9.814-mpy3*t2*t3*t6*t10*t13*t14*9.814-mpy6*t2*t3*t4*t8*t10*t18*9.814+mpy6*t3*t5*t6*t7*t9*t19*9.814-mpy6*t3*t5*t9*t15*t16*t19*9.814+mpy6*t3*t6*t9*t14*t16*t19*9.814+mpy6*t3*t7*t9*t14*t15*t19*9.814-a5*mass4*t2*t3*t5*t6*t10*t13*9.814-a5*mass5*t2*t3*t5*t6*t10*t13*9.814-a5*mass6*t2*t3*t5*t6*t10*t13*9.814+a6*mass5*t2*t5*t6*t10*t12*t16*9.814+a6*mass5*t2*t5*t7*t10*t12*t15*9.814-a6*mass5*t2*t6*t7*t10*t12*t14*9.814+a6*mass6*t2*t5*t6*t10*t12*t16*9.814+a6*mass6*t2*t5*t7*t10*t12*t15*9.814-a6*mass6*t2*t6*t7*t10*t12*t14*9.814-a5*mass4*t2*t3*t10*t13*t14*t15*9.814-a5*mass5*t2*t3*t10*t13*t14*t15*9.814-a5*mass6*t2*t3*t10*t13*t14*t15*9.814+a6*mass5*t2*t10*t12*t14*t15*t16*9.814+a6*mass6*t2*t10*t12*t14*t15*t16*9.814-a5i*mass4*t2*t3*t5*t10*t13*t15*9.814-a5i*mass5*t2*t3*t5*t10*t13*t15*9.814-a5i*mass6*t2*t3*t5*t10*t13*t15*9.814+d7*mass6*t2*t5*t6*t7*t10*t12*9.814-d7*mass6*t2*t5*t10*t12*t15*t16*9.814+d7*mass6*t2*t6*t10*t12*t14*t16*9.814+d7*mass6*t2*t7*t10*t12*t14*t15*9.814-mpy6*t2*t5*t6*t7*t9*t10*t12*9.814+mpy6*t2*t5*t9*t10*t12*t15*t16*9.814-mpy6*t2*t6*t9*t10*t12*t14*t16*9.814-mpy6*t2*t7*t9*t10*t12*t14*t15*9.814+mpy6*t3*t5*t6*t16*t17*t18*t19*9.814+mpy6*t3*t5*t7*t15*t17*t18*t19*9.814-mpy6*t3*t6*t7*t14*t17*t18*t19*9.814+mpy6*t3*t14*t15*t16*t17*t18*t19*9.814-mpy6*t2*t5*t6*t10*t12*t16*t17*t18*9.814-mpy6*t2*t5*t7*t10*t12*t15*t17*t18*9.814+mpy6*t2*t6*t7*t10*t12*t14*t17*t18*9.814-mpy6*t2*t10*t12*t14*t15*t16*t17*t18*9.814;
marcodesilva 0:46ef60040f6a 855 A0(2) = mpx1*t20*9.814-mpx1*t21*9.814-mpx1*t22*9.814+a3*mass2*t20*9.814-a3*mass2*t21*9.814+a3*mass3*t20*9.814-a3*mass2*t22*9.814-a3*mass3*t21*9.814+a3*mass4*t20*9.814-a3*mass3*t22*9.814-a3*mass4*t21*9.814+a3*mass5*t20*9.814-a3*mass4*t22*9.814-a3*mass5*t21*9.814+a3*mass6*t20*9.814-a3*mass5*t22*9.814-a3*mass6*t21*9.814-a3*mass6*t22*9.814+mpx2*t14*t20*9.814-mpx2*t14*t21*9.814-mpx2*t14*t22*9.814-mpy2*t5*t20*9.814+mpy2*t5*t21*9.814+mpy2*t5*t22*9.814+a4*mass3*t14*t20*9.814-a4*mass3*t14*t21*9.814+a4*mass4*t14*t20*9.814-a4*mass3*t14*t22*9.814-a4*mass4*t14*t21*9.814+a4*mass5*t14*t20*9.814-a4*mass4*t14*t22*9.814-a4*mass5*t14*t21*9.814+a4*mass6*t14*t20*9.814-a4*mass5*t14*t22*9.814-a4*mass6*t14*t21*9.814-a4*mass6*t14*t22*9.814+mpx3*t5*t6*t20*9.814-mpx3*t5*t6*t21*9.814-mpx3*t5*t6*t22*9.814+mpx3*t14*t15*t20*9.814-mpx3*t14*t15*t21*9.814-mpx3*t14*t15*t22*9.814-mpy3*t5*t15*t20*9.814+mpy3*t6*t14*t20*9.814+mpy3*t5*t15*t21*9.814-mpy3*t6*t14*t21*9.814+mpy3*t5*t15*t22*9.814-mpy3*t6*t14*t22*9.814+mpy6*t8*t18*t28+a5*mass4*t5*t6*t20*9.814-a5*mass4*t5*t6*t21*9.814+a5*mass5*t5*t6*t20*9.814-a5*mass4*t5*t6*t22*9.814-a5*mass5*t5*t6*t21*9.814+a5*mass6*t5*t6*t20*9.814-a5*mass5*t5*t6*t22*9.814-a5*mass6*t5*t6*t21*9.814-a5*mass6*t5*t6*t22*9.814+a5*mass4*t14*t15*t20*9.814-a5*mass4*t14*t15*t21*9.814+a5*mass5*t14*t15*t20*9.814-a5*mass4*t14*t15*t22*9.814-a5*mass5*t14*t15*t21*9.814+a5*mass6*t14*t15*t20*9.814-a5*mass5*t14*t15*t22*9.814-a5*mass6*t14*t15*t21*9.814-a5*mass6*t14*t15*t22*9.814+a5i*mass4*t5*t15*t20*9.814-a5i*mass4*t6*t14*t20*9.814-a5i*mass4*t5*t15*t21*9.814+a5i*mass4*t6*t14*t21*9.814+a5i*mass5*t5*t15*t20*9.814-a5i*mass5*t6*t14*t20*9.814-a5i*mass4*t5*t15*t22*9.814+a5i*mass4*t6*t14*t22*9.814-a5i*mass5*t5*t15*t21*9.814+a5i*mass5*t6*t14*t21*9.814+a5i*mass6*t5*t15*t20*9.814-a5i*mass6*t6*t14*t20*9.814-a5i*mass5*t5*t15*t22*9.814+a5i*mass5*t6*t14*t22*9.814-a5i*mass6*t5*t15*t21*9.814+a5i*mass6*t6*t14*t21*9.814-a5i*mass6*t5*t15*t22*9.814+a5i*mass6*t6*t14*t22*9.814+mpx4*t5*t6*t7*t20*9.814-mpx4*t5*t6*t7*t21*9.814-mpx4*t5*t6*t7*t22*9.814-mpx4*t5*t15*t16*t20*9.814+mpx4*t6*t14*t16*t20*9.814+mpx4*t7*t14*t15*t20*9.814+mpx4*t5*t15*t16*t21*9.814-mpx4*t6*t14*t16*t21*9.814-mpx4*t7*t14*t15*t21*9.814+mpx4*t5*t15*t16*t22*9.814-mpx4*t6*t14*t16*t22*9.814-mpx4*t7*t14*t15*t22*9.814-d7*mass6*t5*t6*t16*t20*9.814-d7*mass6*t5*t7*t15*t20*9.814+d7*mass6*t6*t7*t14*t20*9.814+d7*mass6*t5*t6*t16*t21*9.814+d7*mass6*t5*t7*t15*t21*9.814-d7*mass6*t6*t7*t14*t21*9.814+d7*mass6*t5*t6*t16*t22*9.814+d7*mass6*t5*t7*t15*t22*9.814-d7*mass6*t6*t7*t14*t22*9.814-d7*mass6*t14*t15*t16*t20*9.814+d7*mass6*t14*t15*t16*t21*9.814+d7*mass6*t14*t15*t16*t22*9.814-mpy6*t4*t8*t10*t11*t18*9.814+mpy6*t5*t6*t9*t16*t20*9.814+mpy6*t5*t7*t9*t15*t20*9.814-mpy6*t6*t7*t9*t14*t20*9.814-mpy6*t5*t6*t9*t16*t21*9.814-mpy6*t5*t7*t9*t15*t21*9.814+mpy6*t6*t7*t9*t14*t21*9.814-mpy6*t5*t6*t9*t16*t22*9.814-mpy6*t5*t7*t9*t15*t22*9.814+mpy6*t6*t7*t9*t14*t22*9.814-mpy6*t3*t8*t13*t18*t19*9.814+mpy6*t9*t14*t15*t16*t20*9.814-mpy6*t9*t14*t15*t16*t21*9.814-mpy6*t9*t14*t15*t16*t22*9.814+a6*mass5*t5*t6*t7*t20*9.814-a6*mass5*t5*t6*t7*t21*9.814+a6*mass6*t5*t6*t7*t20*9.814-a6*mass5*t5*t6*t7*t22*9.814-a6*mass6*t5*t6*t7*t21*9.814-a6*mass6*t5*t6*t7*t22*9.814-a6*mass5*t5*t15*t16*t20*9.814+a6*mass5*t6*t14*t16*t20*9.814+a6*mass5*t7*t14*t15*t20*9.814+a6*mass5*t5*t15*t16*t21*9.814-a6*mass5*t6*t14*t16*t21*9.814-a6*mass5*t7*t14*t15*t21*9.814-a6*mass6*t5*t15*t16*t20*9.814+a6*mass6*t6*t14*t16*t20*9.814+a6*mass6*t7*t14*t15*t20*9.814+a6*mass5*t5*t15*t16*t22*9.814-a6*mass5*t6*t14*t16*t22*9.814-a6*mass5*t7*t14*t15*t22*9.814+a6*mass6*t5*t15*t16*t21*9.814-a6*mass6*t6*t14*t16*t21*9.814-a6*mass6*t7*t14*t15*t21*9.814+a6*mass6*t5*t15*t16*t22*9.814-a6*mass6*t6*t14*t16*t22*9.814-a6*mass6*t7*t14*t15*t22*9.814-mpy6*t5*t6*t7*t17*t18*t20*9.814+mpy6*t5*t6*t7*t17*t18*t21*9.814+mpy6*t5*t6*t7*t17*t18*t22*9.814+mpy6*t5*t15*t16*t17*t18*t20*9.814-mpy6*t6*t14*t16*t17*t18*t20*9.814-mpy6*t7*t14*t15*t17*t18*t20*9.814-mpy6*t5*t15*t16*t17*t18*t21*9.814+mpy6*t6*t14*t16*t17*t18*t21*9.814+mpy6*t7*t14*t15*t17*t18*t21*9.814-mpy6*t5*t15*t16*t17*t18*t22*9.814+mpy6*t6*t14*t16*t17*t18*t22*9.814+mpy6*t7*t14*t15*t17*t18*t22*9.814;
marcodesilva 0:46ef60040f6a 856 A0(3) = t29+t31+t32+t34+t35+t36+t37+t39+t41+t43+t44+t45+t47+t48+t49+t50+t52+t53+t54+t55+t58+t59+t60+t61+t65+t66+t67+t68+t71+t74+t77+t78+t80+t81+t83+t84+t86+t87+t94+t96+t98+t102+t104+t105+t107+t108+t113+t115+t116+t124+t126+t127+t128+t129+t133+t134+t135+t137+t138+t143+t144+t147+t148+t150+t151+t152+t153+t156+t157+t158+t159+t161+t162+t163+t164+t166+t168+t170+t171+t172+t174+t175+t179+t180+t181+t182+t183+t184+t185+t187+t188+t189+t190+t225+t226+t232+t233+t234+t237+t242+t248+t251+t252+t268+t271+t272+t273+t278+t281+t290+t291+t293+mpx2*t5*t26+mpx2*t5*t27+mpx2*t14*t24+mpx2*t14*t25+mpy2*t14*t26+mpy2*t14*t27+a4*mass3*t5*t26+a4*mass3*t5*t27+a4*mass4*t5*t26+a4*mass4*t5*t27+a4*mass5*t5*t26+a4*mass5*t5*t27+a4*mass6*t5*t26+a4*mass6*t5*t27+a4*mass3*t14*t24+a4*mass3*t14*t25+a4*mass4*t14*t24+a4*mass4*t14*t25+a4*mass5*t14*t24+a4*mass5*t14*t25+a4*mass6*t14*t24+a4*mass6*t14*t25-mpy2*t5*t12*t19*9.814-mpy2*t2*t3*t5*t10*9.814-mpy3*t5*t12*t15*t19*9.814-mpx2*t2*t5*t10*t12*t13*9.814-mpx3*t4*t6*t10*t11*t14*9.814-mpx3*t3*t6*t13*t14*t19*9.814-mpx4*t5*t12*t15*t16*t19*9.814-mpy3*t2*t3*t5*t10*t15*9.814-mpy2*t2*t10*t12*t13*t14*9.814-a5i*mass4*t6*t12*t14*t19*9.814-a5i*mass5*t6*t12*t14*t19*9.814-a5i*mass6*t6*t12*t14*t19*9.814-a4*mass3*t2*t5*t10*t12*t13*9.814-a4*mass4*t2*t5*t10*t12*t13*9.814-a4*mass5*t2*t5*t10*t12*t13*9.814-a4*mass6*t2*t5*t10*t12*t13*9.814-a5*mass4*t4*t6*t10*t11*t14*9.814-a5*mass5*t4*t6*t10*t11*t14*9.814-a5*mass6*t4*t6*t10*t11*t14*9.814-a5*mass4*t3*t6*t13*t14*t19*9.814-a5*mass5*t3*t6*t13*t14*t19*9.814-a5*mass6*t3*t6*t13*t14*t19*9.814-a6*mass5*t5*t12*t15*t16*t19*9.814-a6*mass6*t5*t12*t15*t16*t19*9.814-a5i*mass4*t2*t3*t6*t10*t14*9.814-a5i*mass4*t4*t5*t6*t10*t11*9.814-a5i*mass5*t2*t3*t6*t10*t14*9.814-a5i*mass5*t4*t5*t6*t10*t11*9.814-a5i*mass6*t2*t3*t6*t10*t14*9.814-a5i*mass6*t4*t5*t6*t10*t11*9.814-a5i*mass4*t3*t5*t6*t13*t19*9.814-a5i*mass5*t3*t5*t6*t13*t19*9.814-a5i*mass6*t3*t5*t6*t13*t19*9.814-a5i*mass4*t4*t10*t11*t14*t15*9.814-a5i*mass5*t4*t10*t11*t14*t15*9.814-a5i*mass6*t4*t10*t11*t14*t15*9.814-a5i*mass4*t3*t13*t14*t15*t19*9.814-a5i*mass5*t3*t13*t14*t15*t19*9.814-a5i*mass6*t3*t13*t14*t15*t19*9.814-d7*mass6*t5*t6*t12*t16*t19*9.814-d7*mass6*t5*t7*t12*t15*t19*9.814-d7*mass6*t12*t14*t15*t16*t19*9.814-mpx4*t2*t3*t5*t10*t15*t16*9.814-mpx4*t4*t6*t7*t10*t11*t14*9.814-mpx3*t2*t5*t10*t12*t13*t15*9.814-mpx4*t3*t6*t7*t13*t14*t19*9.814-mpy3*t2*t5*t6*t10*t12*t13*9.814-mpy3*t2*t10*t12*t13*t14*t15*9.814-mpy6*t6*t7*t9*t12*t14*t19*9.814-a6*mass5*t2*t3*t5*t10*t15*t16*9.814-a6*mass5*t4*t6*t7*t10*t11*t14*9.814-a6*mass6*t2*t3*t5*t10*t15*t16*9.814-a6*mass6*t4*t6*t7*t10*t11*t14*9.814-a5*mass4*t2*t5*t10*t12*t13*t15*9.814-a5*mass5*t2*t5*t10*t12*t13*t15*9.814-a5*mass6*t2*t5*t10*t12*t13*t15*9.814-a6*mass5*t3*t6*t7*t13*t14*t19*9.814-a6*mass6*t3*t6*t7*t13*t14*t19*9.814-d7*mass6*t2*t3*t5*t6*t10*t16*9.814-d7*mass6*t2*t3*t5*t7*t10*t15*9.814-d7*mass6*t2*t3*t10*t14*t15*t16*9.814-d7*mass6*t4*t5*t10*t11*t15*t16*9.814-d7*mass6*t3*t5*t13*t15*t16*t19*9.814-mpx4*t2*t5*t6*t10*t12*t13*t16*9.814-mpx4*t2*t5*t7*t10*t12*t13*t15*9.814-mpx4*t2*t10*t12*t13*t14*t15*t16*9.814-mpy6*t2*t3*t6*t7*t9*t10*t14*9.814-mpy6*t4*t5*t6*t7*t9*t10*t11*9.814-mpy6*t3*t5*t6*t7*t9*t13*t19*9.814-mpy6*t4*t6*t9*t10*t11*t14*t16*9.814-mpy6*t4*t7*t9*t10*t11*t14*t15*9.814-mpy6*t3*t6*t9*t13*t14*t16*t19*9.814-mpy6*t3*t7*t9*t13*t14*t15*t19*9.814-mpy6*t5*t6*t7*t12*t17*t18*t19*9.814-mpy6*t6*t12*t14*t16*t17*t18*t19*9.814-mpy6*t7*t12*t14*t15*t17*t18*t19*9.814-a6*mass5*t2*t5*t6*t10*t12*t13*t16*9.814-a6*mass5*t2*t5*t7*t10*t12*t13*t15*9.814-a6*mass6*t2*t5*t6*t10*t12*t13*t16*9.814-a6*mass6*t2*t5*t7*t10*t12*t13*t15*9.814-a6*mass5*t2*t10*t12*t13*t14*t15*t16*9.814-a6*mass6*t2*t10*t12*t13*t14*t15*t16*9.814-d7*mass6*t2*t5*t6*t7*t10*t12*t13*9.814-d7*mass6*t2*t6*t10*t12*t13*t14*t16*9.814-d7*mass6*t2*t7*t10*t12*t13*t14*t15*9.814-mpy6*t2*t3*t5*t6*t7*t10*t17*t18*9.814-mpy6*t2*t5*t9*t10*t12*t13*t15*t16*9.814-mpy6*t2*t3*t6*t10*t14*t16*t17*t18*9.814-mpy6*t2*t3*t7*t10*t14*t15*t17*t18*9.814-mpy6*t4*t5*t6*t10*t11*t16*t17*t18*9.814-mpy6*t4*t5*t7*t10*t11*t15*t17*t18*9.814-mpy6*t3*t5*t6*t13*t16*t17*t18*t19*9.814-mpy6*t3*t5*t7*t13*t15*t17*t18*t19*9.814-mpy6*t4*t10*t11*t14*t15*t16*t17*t18*9.814-mpy6*t3*t13*t14*t15*t16*t17*t18*t19*9.814-mpy6*t2*t6*t7*t10*t12*t13*t14*t17*t18*9.814;
marcodesilva 0:46ef60040f6a 857 A0(4) = t30+t38+t40+t42+t46+t51+t56+t57+t62+t63+t64+t69+t70+t72+t73+t75+t76+t79+t82+t85+t88+t89+t90+t91+t92+t93+t95+t97+t99+t100+t101+t103+t106+t109+t110+t111+t112+t114+t117+t118+t119+t120+t121+t122+t123+t125+t130+t131+t132+t136+t139+t140+t141+t142+t145+t146+t149+t154+t155+t160+t165+t167+t169+t173+t176+t177+t178+t186+t191+t192+t193+t194+t195+t196+t197+t198+t199+t200+t201+t202+t203+t204+t205+t206+t207+t208+t209+t210+t211+t212+t213+t214+t215+t216+t217+t218+t219+t220+t221+t222+t223+t224+t227+t228+t229+t230+t231+t235+t236+t238+t239+t240+t241+t243+t244+t245+t246+t247+t249+t250+t253+t254+t255+t256+t257+t258+t259+t260+t261+t262+t263+t265+t266+t267+t269+t270+t274+t275+t276+t277+t279+t280+t282+t283+t284+t285+t286+t287+t288+t289+t292+t295+t296+t297-mpx3*t5*t6*t12*t19*9.814-mpx3*t12*t14*t15*t19*9.814-mpy3*t6*t12*t14*t19*9.814-mpx3*t2*t3*t5*t6*t10*9.814-mpx3*t2*t3*t10*t14*t15*9.814-mpx3*t4*t5*t10*t11*t15*9.814-mpx3*t3*t5*t13*t15*t19*9.814-mpy3*t2*t3*t6*t10*t14*9.814-mpy3*t4*t5*t6*t10*t11*9.814-mpy3*t3*t5*t6*t13*t19*9.814-mpy3*t4*t10*t11*t14*t15*9.814-mpy3*t3*t13*t14*t15*t19*9.814-a5*mass4*t5*t6*t12*t19*9.814-a5*mass5*t5*t6*t12*t19*9.814-a5*mass6*t5*t6*t12*t19*9.814-a5*mass4*t12*t14*t15*t19*9.814-a5*mass5*t12*t14*t15*t19*9.814-a5*mass6*t12*t14*t15*t19*9.814-a5i*mass4*t5*t12*t15*t19*9.814-a5i*mass5*t5*t12*t15*t19*9.814-a5i*mass6*t5*t12*t15*t19*9.814-a5*mass4*t2*t3*t5*t6*t10*9.814-a5*mass5*t2*t3*t5*t6*t10*9.814-a5*mass6*t2*t3*t5*t6*t10*9.814-a5*mass4*t2*t3*t10*t14*t15*9.814-a5*mass4*t4*t5*t10*t11*t15*9.814-a5*mass5*t2*t3*t10*t14*t15*9.814-a5*mass5*t4*t5*t10*t11*t15*9.814-a5*mass6*t2*t3*t10*t14*t15*9.814-a5*mass6*t4*t5*t10*t11*t15*9.814-a5*mass4*t3*t5*t13*t15*t19*9.814-a5*mass5*t3*t5*t13*t15*t19*9.814-a5*mass6*t3*t5*t13*t15*t19*9.814-a5i*mass4*t2*t3*t5*t10*t15*9.814-a5i*mass5*t2*t3*t5*t10*t15*9.814-a5i*mass6*t2*t3*t5*t10*t15*9.814-mpx3*t2*t6*t10*t12*t13*t14*9.814-a5*mass4*t2*t6*t10*t12*t13*t14*9.814-a5*mass5*t2*t6*t10*t12*t13*t14*9.814-a5*mass6*t2*t6*t10*t12*t13*t14*9.814-a5i*mass4*t2*t5*t6*t10*t12*t13*9.814-a5i*mass5*t2*t5*t6*t10*t12*t13*9.814-a5i*mass6*t2*t5*t6*t10*t12*t13*9.814-a5i*mass4*t2*t10*t12*t13*t14*t15*9.814-a5i*mass5*t2*t10*t12*t13*t14*t15*9.814-a5i*mass6*t2*t10*t12*t13*t14*t15*9.814;
marcodesilva 0:46ef60040f6a 858 A0(5) = t57+t62+t63+t64+t100+t101+t103+t106+t112+t117+t118+t119+t120+t121+t122+t123+t125+t130+t136+t139+t140+t141+t142+t145+t146+t149+t154+t155+t160+t173+t176+t177+t178+t186+t191+t192+t193+t194+t195+t196+t197+t198+t199+t200+t201+t202+t203+t204+t205+t206+t207+t208+t209+t210+t211+t212+t213+t214+t215+t216+t217+t218+t219+t220+t221+t222+t223+t224+t227+t228+t229+t230+t231+t235+t236+t238+t239+t240+t241+t243+t244+t245+t246+t247+t249+t250+t253+t254+t255+t256+t257+t258+t259+t260+t261+t262+t263+t265+t266+t267+t269+t270+t274+t275+t276+t277+t279+t280+t282+t283+t284+t285+t286+t287+t288+t289+t292+t295+t296+t297;
marcodesilva 0:46ef60040f6a 859 A0(6) = mpy6*t18*(t17*t264*9.814-t8*t315);
marcodesilva 0:46ef60040f6a 860 A0(7) = -mpy6*(t18*(t7*(t307+t6*(t298-t303))+t16*t311)+t9*(t8*t264*9.814+t17*t315));
marcodesilva 0:46ef60040f6a 861
marcodesilva 0:46ef60040f6a 862 return A0;
marcodesilva 0:46ef60040f6a 863
marcodesilva 0:46ef60040f6a 864 }
marcodesilva 0:46ef60040f6a 865
marcodesilva 0:46ef60040f6a 866 Vector6f ARAP180_WITH_ROVER::wrenchGr(Vector8f q, Vector3f droneOrientation){
marcodesilva 0:46ef60040f6a 867
marcodesilva 0:46ef60040f6a 868 Vector6f A0 = Vector6f::Zero();
marcodesilva 0:46ef60040f6a 869 volatile float q1 = 0.0; //q[0]; Per evitare che il drone si sposti nel calcolo della gravità rispetto alla terna drone fissa sul tubo
marcodesilva 0:46ef60040f6a 870 volatile float q2 = q[1];
marcodesilva 0:46ef60040f6a 871 volatile float q3 = q[2];
marcodesilva 0:46ef60040f6a 872 volatile float q4 = q[3];
marcodesilva 0:46ef60040f6a 873 volatile float q5 = q[4];
marcodesilva 0:46ef60040f6a 874 volatile float q6 = q[5];
marcodesilva 0:46ef60040f6a 875 volatile float q7 = q[6];
marcodesilva 0:46ef60040f6a 876 volatile float q8 = q[7];
marcodesilva 0:46ef60040f6a 877
marcodesilva 0:46ef60040f6a 878 float r_b_d = -droneOrientation[0];
marcodesilva 0:46ef60040f6a 879 float p_b_d = -droneOrientation[1];
marcodesilva 0:46ef60040f6a 880
marcodesilva 0:46ef60040f6a 881 float t2 = cos(p_b_d);
marcodesilva 0:46ef60040f6a 882 float t3 = cos(q2);
marcodesilva 0:46ef60040f6a 883 float t4 = cos(q3);
marcodesilva 0:46ef60040f6a 884 float t5 = cos(q4);
marcodesilva 0:46ef60040f6a 885 float t6 = cos(q5);
marcodesilva 0:46ef60040f6a 886 float t7 = cos(q6);
marcodesilva 0:46ef60040f6a 887 float t8 = cos(q7);
marcodesilva 0:46ef60040f6a 888 float t9 = cos(q8);
marcodesilva 0:46ef60040f6a 889 float t10 = cos(r_b_d);
marcodesilva 0:46ef60040f6a 890 float t11 = sin(p_b_d);
marcodesilva 0:46ef60040f6a 891 float t12 = sin(q2);
marcodesilva 0:46ef60040f6a 892 float t13 = sin(q3);
marcodesilva 0:46ef60040f6a 893 float t14 = sin(q4);
marcodesilva 0:46ef60040f6a 894 float t15 = sin(q5);
marcodesilva 0:46ef60040f6a 895 float t16 = sin(q6);
marcodesilva 0:46ef60040f6a 896 float t17 = sin(q7);
marcodesilva 0:46ef60040f6a 897 float t18 = sin(q8);
marcodesilva 0:46ef60040f6a 898 float t19 = sin(r_b_d);
marcodesilva 0:46ef60040f6a 899 float t20 = mass1+mass2+mass3+mass4+mass5+mass6+massr;
marcodesilva 0:46ef60040f6a 900 A0(0) = t10*t11*t20*9.814;
marcodesilva 0:46ef60040f6a 901 A0(1) = t19*t20*(-9.814);
marcodesilva 0:46ef60040f6a 902 A0(2) = t2*t10*t20*9.814;
marcodesilva 0:46ef60040f6a 903 A0(3) = mpxr*t12*t19*9.814+mpy1*t3*t19*9.814+mpyr*t3*t19*9.814-mpx2*t3*t5*t19*9.814-mpx1*t12*t13*t19*9.814+mpxr*t2*t3*t10*9.814-mpy1*t2*t10*t12*9.814-mpy2*t3*t14*t19*9.814-mpyr*t2*t10*t12*9.814+mass1*t12*t19*x_0_r2*9.814+mass2*t12*t19*x_0_r2*9.814+mass3*t12*t19*x_0_r2*9.814+mass4*t12*t19*x_0_r2*9.814+mass5*t12*t19*x_0_r2*9.814+mass6*t12*t19*x_0_r2*9.814+mass1*t3*t19*y_0_r2*9.814+mass2*t3*t19*y_0_r2*9.814+mass3*t3*t19*y_0_r2*9.814+mass4*t3*t19*y_0_r2*9.814+mass5*t3*t19*y_0_r2*9.814+mass6*t3*t19*y_0_r2*9.814-a4*mass3*t3*t5*t19*9.814-a4*mass4*t3*t5*t19*9.814-a4*mass5*t3*t5*t19*9.814-a4*mass6*t3*t5*t19*9.814-a3*mass2*t12*t13*t19*9.814-a3*mass3*t12*t13*t19*9.814-a3*mass4*t12*t13*t19*9.814-a3*mass5*t12*t13*t19*9.814-a3*mass6*t12*t13*t19*9.814-mpx1*t2*t3*t10*t13*9.814+mpx2*t2*t5*t10*t12*9.814-mpx3*t3*t5*t15*t19*9.814+mpx3*t3*t6*t14*t19*9.814-mpx2*t12*t13*t14*t19*9.814-mpy3*t3*t5*t6*t19*9.814+mpy2*t2*t10*t12*t14*9.814+mpy2*t5*t12*t13*t19*9.814-mpy3*t3*t14*t15*t19*9.814+mass1*t2*t3*t10*x_0_r2*9.814+mass2*t2*t3*t10*x_0_r2*9.814+mass3*t2*t3*t10*x_0_r2*9.814+mass4*t2*t3*t10*x_0_r2*9.814+mass5*t2*t3*t10*x_0_r2*9.814+mass6*t2*t3*t10*x_0_r2*9.814-mass1*t2*t10*t12*y_0_r2*9.814-mass2*t2*t10*t12*y_0_r2*9.814-mass3*t2*t10*t12*y_0_r2*9.814-mass4*t2*t10*t12*y_0_r2*9.814-mass5*t2*t10*t12*y_0_r2*9.814-mass6*t2*t10*t12*y_0_r2*9.814-mpx2*t2*t3*t10*t13*t14*9.814+mpx3*t2*t5*t10*t12*t15*9.814-mpx3*t2*t6*t10*t12*t14*9.814-mpx4*t3*t5*t6*t16*t19*9.814-mpx4*t3*t5*t7*t15*t19*9.814+mpx4*t3*t6*t7*t14*t19*9.814-mpx3*t5*t6*t12*t13*t19*9.814-mpx4*t3*t14*t15*t16*t19*9.814-mpx3*t12*t13*t14*t15*t19*9.814+mpy2*t2*t3*t5*t10*t13*9.814+mpy3*t2*t5*t6*t10*t12*9.814+mpy3*t2*t10*t12*t14*t15*9.814+mpy3*t5*t12*t13*t15*t19*9.814-mpy3*t6*t12*t13*t14*t19*9.814-mpy6*t4*t8*t12*t18*t19*9.814-a3*mass2*t2*t3*t10*t13*9.814-a3*mass3*t2*t3*t10*t13*9.814-a3*mass4*t2*t3*t10*t13*9.814-a3*mass5*t2*t3*t10*t13*9.814+a4*mass3*t2*t5*t10*t12*9.814-a3*mass6*t2*t3*t10*t13*9.814+a4*mass4*t2*t5*t10*t12*9.814+a4*mass5*t2*t5*t10*t12*9.814+a4*mass6*t2*t5*t10*t12*9.814-a5*mass4*t3*t5*t15*t19*9.814+a5*mass4*t3*t6*t14*t19*9.814-a5*mass5*t3*t5*t15*t19*9.814+a5*mass5*t3*t6*t14*t19*9.814-a5*mass6*t3*t5*t15*t19*9.814+a5*mass6*t3*t6*t14*t19*9.814-a4*mass3*t12*t13*t14*t19*9.814-a4*mass4*t12*t13*t14*t19*9.814-a4*mass5*t12*t13*t14*t19*9.814-a4*mass6*t12*t13*t14*t19*9.814+a5i*mass4*t3*t5*t6*t19*9.814+a5i*mass5*t3*t5*t6*t19*9.814+a5i*mass6*t3*t5*t6*t19*9.814+a5i*mass4*t3*t14*t15*t19*9.814+a5i*mass5*t3*t14*t15*t19*9.814+a5i*mass6*t3*t14*t15*t19*9.814-a4*mass3*t2*t3*t10*t13*t14*9.814-a4*mass4*t2*t3*t10*t13*t14*9.814-a4*mass5*t2*t3*t10*t13*t14*9.814-a4*mass6*t2*t3*t10*t13*t14*9.814+a5*mass4*t2*t5*t10*t12*t15*9.814-a5*mass4*t2*t6*t10*t12*t14*9.814+a5*mass5*t2*t5*t10*t12*t15*9.814-a5*mass5*t2*t6*t10*t12*t14*9.814+a5*mass6*t2*t5*t10*t12*t15*9.814-a5*mass6*t2*t6*t10*t12*t14*9.814-a6*mass5*t3*t5*t6*t16*t19*9.814-a6*mass5*t3*t5*t7*t15*t19*9.814+a6*mass5*t3*t6*t7*t14*t19*9.814-a6*mass6*t3*t5*t6*t16*t19*9.814-a6*mass6*t3*t5*t7*t15*t19*9.814+a6*mass6*t3*t6*t7*t14*t19*9.814-a5*mass4*t5*t6*t12*t13*t19*9.814-a5*mass5*t5*t6*t12*t13*t19*9.814-a5*mass6*t5*t6*t12*t13*t19*9.814-a6*mass5*t3*t14*t15*t16*t19*9.814-a6*mass6*t3*t14*t15*t16*t19*9.814-a5*mass4*t12*t13*t14*t15*t19*9.814-a5*mass5*t12*t13*t14*t15*t19*9.814-a5*mass6*t12*t13*t14*t15*t19*9.814-a5i*mass4*t2*t5*t6*t10*t12*9.814-a5i*mass5*t2*t5*t6*t10*t12*9.814-a5i*mass6*t2*t5*t6*t10*t12*9.814-a5i*mass4*t2*t10*t12*t14*t15*9.814-a5i*mass5*t2*t10*t12*t14*t15*9.814-a5i*mass6*t2*t10*t12*t14*t15*9.814-a5i*mass4*t5*t12*t13*t15*t19*9.814+a5i*mass4*t6*t12*t13*t14*t19*9.814-a5i*mass5*t5*t12*t13*t15*t19*9.814+a5i*mass5*t6*t12*t13*t14*t19*9.814-a5i*mass6*t5*t12*t13*t15*t19*9.814+a5i*mass6*t6*t12*t13*t14*t19*9.814-d7*mass6*t3*t5*t6*t7*t19*9.814+d7*mass6*t3*t5*t15*t16*t19*9.814-d7*mass6*t3*t6*t14*t16*t19*9.814-d7*mass6*t3*t7*t14*t15*t19*9.814-mpx3*t2*t3*t5*t6*t10*t13*9.814+mpx4*t2*t5*t6*t10*t12*t16*9.814+mpx4*t2*t5*t7*t10*t12*t15*9.814-mpx4*t2*t6*t7*t10*t12*t14*9.814-mpx3*t2*t3*t10*t13*t14*t15*9.814-mpx4*t5*t6*t7*t12*t13*t19*9.814+mpx4*t2*t10*t12*t14*t15*t16*9.814+mpx4*t5*t12*t13*t15*t16*t19*9.814-mpx4*t6*t12*t13*t14*t16*t19*9.814-mpx4*t7*t12*t13*t14*t15*t19*9.814+mpy3*t2*t3*t5*t10*t13*t15*9.814-mpy3*t2*t3*t6*t10*t13*t14*9.814-mpy6*t2*t3*t4*t8*t10*t18*9.814+mpy6*t3*t5*t6*t7*t9*t19*9.814-mpy6*t3*t5*t9*t15*t16*t19*9.814+mpy6*t3*t6*t9*t14*t16*t19*9.814+mpy6*t3*t7*t9*t14*t15*t19*9.814-a5*mass4*t2*t3*t5*t6*t10*t13*9.814-a5*mass5*t2*t3*t5*t6*t10*t13*9.814-a5*mass6*t2*t3*t5*t6*t10*t13*9.814+a6*mass5*t2*t5*t6*t10*t12*t16*9.814+a6*mass5*t2*t5*t7*t10*t12*t15*9.814-a6*mass5*t2*t6*t7*t10*t12*t14*9.814+a6*mass6*t2*t5*t6*t10*t12*t16*9.814+a6*mass6*t2*t5*t7*t10*t12*t15*9.814-a6*mass6*t2*t6*t7*t10*t12*t14*9.814-a5*mass4*t2*t3*t10*t13*t14*t15*9.814-a5*mass5*t2*t3*t10*t13*t14*t15*9.814-a5*mass6*t2*t3*t10*t13*t14*t15*9.814-a6*mass5*t5*t6*t7*t12*t13*t19*9.814-a6*mass6*t5*t6*t7*t12*t13*t19*9.814+a6*mass5*t2*t10*t12*t14*t15*t16*9.814+a6*mass6*t2*t10*t12*t14*t15*t16*9.814+a6*mass5*t5*t12*t13*t15*t16*t19*9.814-a6*mass5*t6*t12*t13*t14*t16*t19*9.814-a6*mass5*t7*t12*t13*t14*t15*t19*9.814+a6*mass6*t5*t12*t13*t15*t16*t19*9.814-a6*mass6*t6*t12*t13*t14*t16*t19*9.814-a6*mass6*t7*t12*t13*t14*t15*t19*9.814-a5i*mass4*t2*t3*t5*t10*t13*t15*9.814+a5i*mass4*t2*t3*t6*t10*t13*t14*9.814-a5i*mass5*t2*t3*t5*t10*t13*t15*9.814+a5i*mass5*t2*t3*t6*t10*t13*t14*9.814-a5i*mass6*t2*t3*t5*t10*t13*t15*9.814+a5i*mass6*t2*t3*t6*t10*t13*t14*9.814+d7*mass6*t2*t5*t6*t7*t10*t12*9.814-d7*mass6*t2*t5*t10*t12*t15*t16*9.814+d7*mass6*t2*t6*t10*t12*t14*t16*9.814+d7*mass6*t2*t7*t10*t12*t14*t15*9.814+d7*mass6*t5*t6*t12*t13*t16*t19*9.814+d7*mass6*t5*t7*t12*t13*t15*t19*9.814-d7*mass6*t6*t7*t12*t13*t14*t19*9.814+d7*mass6*t12*t13*t14*t15*t16*t19*9.814-mpx4*t2*t3*t5*t6*t7*t10*t13*9.814+mpx4*t2*t3*t5*t10*t13*t15*t16*9.814-mpx4*t2*t3*t6*t10*t13*t14*t16*9.814-mpx4*t2*t3*t7*t10*t13*t14*t15*9.814-mpy6*t2*t5*t6*t7*t9*t10*t12*9.814+mpy6*t2*t5*t9*t10*t12*t15*t16*9.814-mpy6*t2*t6*t9*t10*t12*t14*t16*9.814-mpy6*t2*t7*t9*t10*t12*t14*t15*9.814-mpy6*t5*t6*t9*t12*t13*t16*t19*9.814-mpy6*t5*t7*t9*t12*t13*t15*t19*9.814+mpy6*t6*t7*t9*t12*t13*t14*t19*9.814+mpy6*t3*t5*t6*t16*t17*t18*t19*9.814+mpy6*t3*t5*t7*t15*t17*t18*t19*9.814-mpy6*t3*t6*t7*t14*t17*t18*t19*9.814-mpy6*t9*t12*t13*t14*t15*t16*t19*9.814+mpy6*t3*t14*t15*t16*t17*t18*t19*9.814-a6*mass5*t2*t3*t5*t6*t7*t10*t13*9.814-a6*mass6*t2*t3*t5*t6*t7*t10*t13*9.814+a6*mass5*t2*t3*t5*t10*t13*t15*t16*9.814-a6*mass5*t2*t3*t6*t10*t13*t14*t16*9.814-a6*mass5*t2*t3*t7*t10*t13*t14*t15*9.814+a6*mass6*t2*t3*t5*t10*t13*t15*t16*9.814-a6*mass6*t2*t3*t6*t10*t13*t14*t16*9.814-a6*mass6*t2*t3*t7*t10*t13*t14*t15*9.814+d7*mass6*t2*t3*t5*t6*t10*t13*t16*9.814+d7*mass6*t2*t3*t5*t7*t10*t13*t15*9.814-d7*mass6*t2*t3*t6*t7*t10*t13*t14*9.814+d7*mass6*t2*t3*t10*t13*t14*t15*t16*9.814-mpy6*t2*t3*t5*t6*t9*t10*t13*t16*9.814-mpy6*t2*t3*t5*t7*t9*t10*t13*t15*9.814+mpy6*t2*t3*t6*t7*t9*t10*t13*t14*9.814-mpy6*t2*t3*t9*t10*t13*t14*t15*t16*9.814-mpy6*t2*t5*t6*t10*t12*t16*t17*t18*9.814-mpy6*t2*t5*t7*t10*t12*t15*t17*t18*9.814+mpy6*t2*t6*t7*t10*t12*t14*t17*t18*9.814+mpy6*t5*t6*t7*t12*t13*t17*t18*t19*9.814-mpy6*t2*t10*t12*t14*t15*t16*t17*t18*9.814-mpy6*t5*t12*t13*t15*t16*t17*t18*t19*9.814+mpy6*t6*t12*t13*t14*t16*t17*t18*t19*9.814+mpy6*t7*t12*t13*t14*t15*t17*t18*t19*9.814+mpy6*t2*t3*t5*t6*t7*t10*t13*t17*t18*9.814-mpy6*t2*t3*t5*t10*t13*t15*t16*t17*t18*9.814+mpy6*t2*t3*t6*t10*t13*t14*t16*t17*t18*9.814+mpy6*t2*t3*t7*t10*t13*t14*t15*t17*t18*9.814;
marcodesilva 0:46ef60040f6a 904 A0(4) = t10*(mpzr*t2+mass1*q1*t2+mass2*q1*t2+mass3*q1*t2+mass4*q1*t2+mass5*q1*t2+mass6*q1*t2+massr*q1*t2+mpx1*t2*t4-mpxr*t11*t12-mpy1*t3*t11-mpyr*t3*t11+mass1*t2*z_0_r2+mass2*t2*z_0_r2+mass3*t2*z_0_r2+mass4*t2*z_0_r2+mass5*t2*z_0_r2+mass6*t2*z_0_r2+a3*mass2*t2*t4+a3*mass3*t2*t4+a3*mass4*t2*t4+a3*mass5*t2*t4+a3*mass6*t2*t4+mpx2*t3*t5*t11+mpx2*t2*t4*t14+mpx1*t11*t12*t13-mpy2*t2*t4*t5+mpy2*t3*t11*t14-mass1*t11*t12*x_0_r2-mass2*t11*t12*x_0_r2-mass3*t11*t12*x_0_r2-mass4*t11*t12*x_0_r2-mass5*t11*t12*x_0_r2-mass6*t11*t12*x_0_r2-mass1*t3*t11*y_0_r2-mass2*t3*t11*y_0_r2-mass3*t3*t11*y_0_r2-mass4*t3*t11*y_0_r2-mass5*t3*t11*y_0_r2-mass6*t3*t11*y_0_r2+a4*mass3*t3*t5*t11+a4*mass3*t2*t4*t14+a4*mass4*t3*t5*t11+a4*mass4*t2*t4*t14+a4*mass5*t3*t5*t11+a4*mass5*t2*t4*t14+a4*mass6*t3*t5*t11+a4*mass6*t2*t4*t14+a3*mass2*t11*t12*t13+a3*mass3*t11*t12*t13+a3*mass4*t11*t12*t13+a3*mass5*t11*t12*t13+a3*mass6*t11*t12*t13+mpx3*t2*t4*t5*t6+mpx3*t3*t5*t11*t15-mpx3*t3*t6*t11*t14+mpx3*t2*t4*t14*t15+mpx2*t11*t12*t13*t14+mpy3*t3*t5*t6*t11-mpy3*t2*t4*t5*t15+mpy3*t2*t4*t6*t14-mpy2*t5*t11*t12*t13+mpy3*t3*t11*t14*t15-mpy6*t2*t8*t13*t18+mpx4*t2*t4*t5*t6*t7+mpx4*t3*t5*t6*t11*t16+mpx4*t3*t5*t7*t11*t15-mpx4*t3*t6*t7*t11*t14-mpx4*t2*t4*t5*t15*t16+mpx4*t2*t4*t6*t14*t16+mpx4*t2*t4*t7*t14*t15+mpx3*t5*t6*t11*t12*t13+mpx4*t3*t11*t14*t15*t16+mpx3*t11*t12*t13*t14*t15-mpy3*t5*t11*t12*t13*t15+mpy3*t6*t11*t12*t13*t14+mpy6*t4*t8*t11*t12*t18+a5*mass4*t2*t4*t5*t6+a5*mass5*t2*t4*t5*t6+a5*mass6*t2*t4*t5*t6+a5*mass4*t3*t5*t11*t15-a5*mass4*t3*t6*t11*t14+a5*mass4*t2*t4*t14*t15+a5*mass5*t3*t5*t11*t15-a5*mass5*t3*t6*t11*t14+a5*mass5*t2*t4*t14*t15+a5*mass6*t3*t5*t11*t15-a5*mass6*t3*t6*t11*t14+a5*mass6*t2*t4*t14*t15+a4*mass3*t11*t12*t13*t14+a4*mass4*t11*t12*t13*t14+a4*mass5*t11*t12*t13*t14+a4*mass6*t11*t12*t13*t14-a5i*mass4*t3*t5*t6*t11+a5i*mass4*t2*t4*t5*t15-a5i*mass4*t2*t4*t6*t14-a5i*mass5*t3*t5*t6*t11+a5i*mass5*t2*t4*t5*t15-a5i*mass5*t2*t4*t6*t14-a5i*mass6*t3*t5*t6*t11+a5i*mass6*t2*t4*t5*t15-a5i*mass6*t2*t4*t6*t14-a5i*mass4*t3*t11*t14*t15-a5i*mass5*t3*t11*t14*t15-a5i*mass6*t3*t11*t14*t15+a6*mass5*t2*t4*t5*t6*t7+a6*mass6*t2*t4*t5*t6*t7+a6*mass5*t3*t5*t6*t11*t16+a6*mass5*t3*t5*t7*t11*t15-a6*mass5*t3*t6*t7*t11*t14-a6*mass5*t2*t4*t5*t15*t16+a6*mass5*t2*t4*t6*t14*t16+a6*mass5*t2*t4*t7*t14*t15+a6*mass6*t3*t5*t6*t11*t16+a6*mass6*t3*t5*t7*t11*t15-a6*mass6*t3*t6*t7*t11*t14-a6*mass6*t2*t4*t5*t15*t16+a6*mass6*t2*t4*t6*t14*t16+a6*mass6*t2*t4*t7*t14*t15+a5*mass4*t5*t6*t11*t12*t13+a5*mass5*t5*t6*t11*t12*t13+a5*mass6*t5*t6*t11*t12*t13+a6*mass5*t3*t11*t14*t15*t16+a6*mass6*t3*t11*t14*t15*t16+a5*mass4*t11*t12*t13*t14*t15+a5*mass5*t11*t12*t13*t14*t15+a5*mass6*t11*t12*t13*t14*t15+a5i*mass4*t5*t11*t12*t13*t15-a5i*mass4*t6*t11*t12*t13*t14+a5i*mass5*t5*t11*t12*t13*t15-a5i*mass5*t6*t11*t12*t13*t14+a5i*mass6*t5*t11*t12*t13*t15-a5i*mass6*t6*t11*t12*t13*t14+d7*mass6*t3*t5*t6*t7*t11-d7*mass6*t2*t4*t5*t6*t16-d7*mass6*t2*t4*t5*t7*t15+d7*mass6*t2*t4*t6*t7*t14-d7*mass6*t3*t5*t11*t15*t16+d7*mass6*t3*t6*t11*t14*t16+d7*mass6*t3*t7*t11*t14*t15-d7*mass6*t2*t4*t14*t15*t16+mpx4*t5*t6*t7*t11*t12*t13-mpx4*t5*t11*t12*t13*t15*t16+mpx4*t6*t11*t12*t13*t14*t16+mpx4*t7*t11*t12*t13*t14*t15-mpy6*t3*t5*t6*t7*t9*t11+mpy6*t2*t4*t5*t6*t9*t16+mpy6*t2*t4*t5*t7*t9*t15-mpy6*t2*t4*t6*t7*t9*t14+mpy6*t3*t5*t9*t11*t15*t16-mpy6*t3*t6*t9*t11*t14*t16-mpy6*t3*t7*t9*t11*t14*t15+mpy6*t2*t4*t9*t14*t15*t16+a6*mass5*t5*t6*t7*t11*t12*t13+a6*mass6*t5*t6*t7*t11*t12*t13-a6*mass5*t5*t11*t12*t13*t15*t16+a6*mass5*t6*t11*t12*t13*t14*t16+a6*mass5*t7*t11*t12*t13*t14*t15-a6*mass6*t5*t11*t12*t13*t15*t16+a6*mass6*t6*t11*t12*t13*t14*t16+a6*mass6*t7*t11*t12*t13*t14*t15-d7*mass6*t5*t6*t11*t12*t13*t16-d7*mass6*t5*t7*t11*t12*t13*t15+d7*mass6*t6*t7*t11*t12*t13*t14-d7*mass6*t11*t12*t13*t14*t15*t16-mpy6*t2*t4*t5*t6*t7*t17*t18+mpy6*t5*t6*t9*t11*t12*t13*t16+mpy6*t5*t7*t9*t11*t12*t13*t15-mpy6*t6*t7*t9*t11*t12*t13*t14-mpy6*t3*t5*t6*t11*t16*t17*t18-mpy6*t3*t5*t7*t11*t15*t17*t18+mpy6*t3*t6*t7*t11*t14*t17*t18+mpy6*t2*t4*t5*t15*t16*t17*t18-mpy6*t2*t4*t6*t14*t16*t17*t18-mpy6*t2*t4*t7*t14*t15*t17*t18+mpy6*t9*t11*t12*t13*t14*t15*t16-mpy6*t3*t11*t14*t15*t16*t17*t18-mpy6*t5*t6*t7*t11*t12*t13*t17*t18+mpy6*t5*t11*t12*t13*t15*t16*t17*t18-mpy6*t6*t11*t12*t13*t14*t16*t17*t18-mpy6*t7*t11*t12*t13*t14*t15*t17*t18)*(-9.814);
marcodesilva 0:46ef60040f6a 905 A0(5) = mpzr*t19*(-9.814)-mass1*q1*t19*9.814-mass2*q1*t19*9.814-mass3*q1*t19*9.814-mass4*q1*t19*9.814-mass5*q1*t19*9.814-mass6*q1*t19*9.814-massr*q1*t19*9.814-mpx1*t4*t19*9.814-mass1*t19*z_0_r2*9.814-mass2*t19*z_0_r2*9.814-mass3*t19*z_0_r2*9.814-mass4*t19*z_0_r2*9.814-mass5*t19*z_0_r2*9.814-mass6*t19*z_0_r2*9.814-a3*mass2*t4*t19*9.814-a3*mass3*t4*t19*9.814-a3*mass4*t4*t19*9.814-a3*mass5*t4*t19*9.814-a3*mass6*t4*t19*9.814-mpx2*t4*t14*t19*9.814-mpxr*t3*t10*t11*9.814+mpy2*t4*t5*t19*9.814+mpy1*t10*t11*t12*9.814+mpyr*t10*t11*t12*9.814-a4*mass3*t4*t14*t19*9.814-a4*mass4*t4*t14*t19*9.814-a4*mass5*t4*t14*t19*9.814-a4*mass6*t4*t14*t19*9.814-mpx3*t4*t5*t6*t19*9.814+mpx1*t3*t10*t11*t13*9.814-mpx2*t5*t10*t11*t12*9.814-mpx3*t4*t14*t15*t19*9.814+mpy3*t4*t5*t15*t19*9.814-mpy3*t4*t6*t14*t19*9.814-mpy2*t10*t11*t12*t14*9.814+mpy6*t8*t13*t18*t19*9.814-mass1*t3*t10*t11*x_0_r2*9.814-mass2*t3*t10*t11*x_0_r2*9.814-mass3*t3*t10*t11*x_0_r2*9.814-mass4*t3*t10*t11*x_0_r2*9.814-mass5*t3*t10*t11*x_0_r2*9.814-mass6*t3*t10*t11*x_0_r2*9.814+mass1*t10*t11*t12*y_0_r2*9.814+mass2*t10*t11*t12*y_0_r2*9.814+mass3*t10*t11*t12*y_0_r2*9.814+mass4*t10*t11*t12*y_0_r2*9.814+mass5*t10*t11*t12*y_0_r2*9.814+mass6*t10*t11*t12*y_0_r2*9.814-mpx4*t4*t5*t6*t7*t19*9.814+mpx2*t3*t10*t11*t13*t14*9.814-mpx3*t5*t10*t11*t12*t15*9.814+mpx3*t6*t10*t11*t12*t14*9.814+mpx4*t4*t5*t15*t16*t19*9.814-mpx4*t4*t6*t14*t16*t19*9.814-mpx4*t4*t7*t14*t15*t19*9.814-mpy2*t3*t5*t10*t11*t13*9.814-mpy3*t5*t6*t10*t11*t12*9.814-mpy3*t10*t11*t12*t14*t15*9.814+a3*mass2*t3*t10*t11*t13*9.814+a3*mass3*t3*t10*t11*t13*9.814-a5*mass4*t4*t5*t6*t19*9.814+a3*mass4*t3*t10*t11*t13*9.814-a5*mass5*t4*t5*t6*t19*9.814+a3*mass5*t3*t10*t11*t13*9.814-a4*mass3*t5*t10*t11*t12*9.814-a5*mass6*t4*t5*t6*t19*9.814+a3*mass6*t3*t10*t11*t13*9.814-a4*mass4*t5*t10*t11*t12*9.814-a4*mass5*t5*t10*t11*t12*9.814-a4*mass6*t5*t10*t11*t12*9.814-a5*mass4*t4*t14*t15*t19*9.814-a5*mass5*t4*t14*t15*t19*9.814-a5*mass6*t4*t14*t15*t19*9.814-a5i*mass4*t4*t5*t15*t19*9.814+a5i*mass4*t4*t6*t14*t19*9.814-a5i*mass5*t4*t5*t15*t19*9.814+a5i*mass5*t4*t6*t14*t19*9.814-a5i*mass6*t4*t5*t15*t19*9.814+a5i*mass6*t4*t6*t14*t19*9.814-a6*mass5*t4*t5*t6*t7*t19*9.814-a6*mass6*t4*t5*t6*t7*t19*9.814+a4*mass3*t3*t10*t11*t13*t14*9.814+a4*mass4*t3*t10*t11*t13*t14*9.814+a4*mass5*t3*t10*t11*t13*t14*9.814+a4*mass6*t3*t10*t11*t13*t14*9.814-a5*mass4*t5*t10*t11*t12*t15*9.814+a5*mass4*t6*t10*t11*t12*t14*9.814-a5*mass5*t5*t10*t11*t12*t15*9.814+a5*mass5*t6*t10*t11*t12*t14*9.814-a5*mass6*t5*t10*t11*t12*t15*9.814+a5*mass6*t6*t10*t11*t12*t14*9.814+a6*mass5*t4*t5*t15*t16*t19*9.814-a6*mass5*t4*t6*t14*t16*t19*9.814-a6*mass5*t4*t7*t14*t15*t19*9.814+a6*mass6*t4*t5*t15*t16*t19*9.814-a6*mass6*t4*t6*t14*t16*t19*9.814-a6*mass6*t4*t7*t14*t15*t19*9.814+a5i*mass4*t5*t6*t10*t11*t12*9.814+a5i*mass5*t5*t6*t10*t11*t12*9.814+a5i*mass6*t5*t6*t10*t11*t12*9.814+a5i*mass4*t10*t11*t12*t14*t15*9.814+a5i*mass5*t10*t11*t12*t14*t15*9.814+a5i*mass6*t10*t11*t12*t14*t15*9.814+d7*mass6*t4*t5*t6*t16*t19*9.814+d7*mass6*t4*t5*t7*t15*t19*9.814-d7*mass6*t4*t6*t7*t14*t19*9.814+d7*mass6*t4*t14*t15*t16*t19*9.814+mpx3*t3*t5*t6*t10*t11*t13*9.814-mpx4*t5*t6*t10*t11*t12*t16*9.814-mpx4*t5*t7*t10*t11*t12*t15*9.814+mpx4*t6*t7*t10*t11*t12*t14*9.814+mpx3*t3*t10*t11*t13*t14*t15*9.814-mpx4*t10*t11*t12*t14*t15*t16*9.814-mpy3*t3*t5*t10*t11*t13*t15*9.814+mpy3*t3*t6*t10*t11*t13*t14*9.814+mpy6*t3*t4*t8*t10*t11*t18*9.814-mpy6*t4*t5*t6*t9*t16*t19*9.814-mpy6*t4*t5*t7*t9*t15*t19*9.814+mpy6*t4*t6*t7*t9*t14*t19*9.814-mpy6*t4*t9*t14*t15*t16*t19*9.814+a5*mass4*t3*t5*t6*t10*t11*t13*9.814+a5*mass5*t3*t5*t6*t10*t11*t13*9.814+a5*mass6*t3*t5*t6*t10*t11*t13*9.814-a6*mass5*t5*t6*t10*t11*t12*t16*9.814-a6*mass5*t5*t7*t10*t11*t12*t15*9.814+a6*mass5*t6*t7*t10*t11*t12*t14*9.814-a6*mass6*t5*t6*t10*t11*t12*t16*9.814-a6*mass6*t5*t7*t10*t11*t12*t15*9.814+a6*mass6*t6*t7*t10*t11*t12*t14*9.814+a5*mass4*t3*t10*t11*t13*t14*t15*9.814+a5*mass5*t3*t10*t11*t13*t14*t15*9.814+a5*mass6*t3*t10*t11*t13*t14*t15*9.814-a6*mass5*t10*t11*t12*t14*t15*t16*9.814-a6*mass6*t10*t11*t12*t14*t15*t16*9.814+a5i*mass4*t3*t5*t10*t11*t13*t15*9.814-a5i*mass4*t3*t6*t10*t11*t13*t14*9.814+a5i*mass5*t3*t5*t10*t11*t13*t15*9.814-a5i*mass5*t3*t6*t10*t11*t13*t14*9.814+a5i*mass6*t3*t5*t10*t11*t13*t15*9.814-a5i*mass6*t3*t6*t10*t11*t13*t14*9.814-d7*mass6*t5*t6*t7*t10*t11*t12*9.814+d7*mass6*t5*t10*t11*t12*t15*t16*9.814-d7*mass6*t6*t10*t11*t12*t14*t16*9.814-d7*mass6*t7*t10*t11*t12*t14*t15*9.814+mpx4*t3*t5*t6*t7*t10*t11*t13*9.814-mpx4*t3*t5*t10*t11*t13*t15*t16*9.814+mpx4*t3*t6*t10*t11*t13*t14*t16*9.814+mpx4*t3*t7*t10*t11*t13*t14*t15*9.814+mpy6*t5*t6*t7*t9*t10*t11*t12*9.814+mpy6*t4*t5*t6*t7*t17*t18*t19*9.814-mpy6*t5*t9*t10*t11*t12*t15*t16*9.814+mpy6*t6*t9*t10*t11*t12*t14*t16*9.814+mpy6*t7*t9*t10*t11*t12*t14*t15*9.814-mpy6*t4*t5*t15*t16*t17*t18*t19*9.814+mpy6*t4*t6*t14*t16*t17*t18*t19*9.814+mpy6*t4*t7*t14*t15*t17*t18*t19*9.814+a6*mass5*t3*t5*t6*t7*t10*t11*t13*9.814+a6*mass6*t3*t5*t6*t7*t10*t11*t13*9.814-a6*mass5*t3*t5*t10*t11*t13*t15*t16*9.814+a6*mass5*t3*t6*t10*t11*t13*t14*t16*9.814+a6*mass5*t3*t7*t10*t11*t13*t14*t15*9.814-a6*mass6*t3*t5*t10*t11*t13*t15*t16*9.814+a6*mass6*t3*t6*t10*t11*t13*t14*t16*9.814+a6*mass6*t3*t7*t10*t11*t13*t14*t15*9.814-d7*mass6*t3*t5*t6*t10*t11*t13*t16*9.814-d7*mass6*t3*t5*t7*t10*t11*t13*t15*9.814+d7*mass6*t3*t6*t7*t10*t11*t13*t14*9.814-d7*mass6*t3*t10*t11*t13*t14*t15*t16*9.814+mpy6*t3*t5*t6*t9*t10*t11*t13*t16*9.814+mpy6*t3*t5*t7*t9*t10*t11*t13*t15*9.814-mpy6*t3*t6*t7*t9*t10*t11*t13*t14*9.814+mpy6*t3*t9*t10*t11*t13*t14*t15*t16*9.814+mpy6*t5*t6*t10*t11*t12*t16*t17*t18*9.814+mpy6*t5*t7*t10*t11*t12*t15*t17*t18*9.814-mpy6*t6*t7*t10*t11*t12*t14*t17*t18*9.814+mpy6*t10*t11*t12*t14*t15*t16*t17*t18*9.814-mpy6*t3*t5*t6*t7*t10*t11*t13*t17*t18*9.814+mpy6*t3*t5*t10*t11*t13*t15*t16*t17*t18*9.814-mpy6*t3*t6*t10*t11*t13*t14*t16*t17*t18*9.814-mpy6*t3*t7*t10*t11*t13*t14*t15*t17*t18*9.814;
marcodesilva 0:46ef60040f6a 906
marcodesilva 0:46ef60040f6a 907 return A0;
marcodesilva 0:46ef60040f6a 908 }
marcodesilva 0:46ef60040f6a 909
marcodesilva 0:46ef60040f6a 910 Vector6f ARAP180_WITH_ROVER::wrenchBattery(float battery_position, Vector3f droneOrientation){
marcodesilva 0:46ef60040f6a 911
marcodesilva 0:46ef60040f6a 912 Vector6f A0 = Vector6f::Zero();
marcodesilva 0:46ef60040f6a 913
marcodesilva 0:46ef60040f6a 914 float r_b_w = -droneOrientation[0];
marcodesilva 0:46ef60040f6a 915 float p_b_w = -droneOrientation[1];
marcodesilva 0:46ef60040f6a 916 float q1 = battery_position;
marcodesilva 0:46ef60040f6a 917
marcodesilva 0:46ef60040f6a 918 float t2 = cos(p_b_w);
marcodesilva 0:46ef60040f6a 919 float t3 = cos(r_b_w);
marcodesilva 0:46ef60040f6a 920 float t4 = sin(p_b_w);
marcodesilva 0:46ef60040f6a 921 float t5 = sin(r_b_w);
marcodesilva 0:46ef60040f6a 922 A0(0) = mass_battery*t3*t4*9.814;
marcodesilva 0:46ef60040f6a 923 A0(1) = mass_battery*t5*(-9.814);
marcodesilva 0:46ef60040f6a 924 A0(2) = mass_battery*t2*t3*9.814;
marcodesilva 0:46ef60040f6a 925 A0(3) = mpx_battery*t5*9.814;
marcodesilva 0:46ef60040f6a 926 A0(4) = mpx_battery*t3*t4*9.814+mpz_battery*t2*t3*9.814+mass_battery*q1*t2*t3*9.814;
marcodesilva 0:46ef60040f6a 927 A0(5) = mpz_battery*t5*9.814+mass_battery*q1*t5*9.814;
marcodesilva 0:46ef60040f6a 928
marcodesilva 0:46ef60040f6a 929 return A0;
marcodesilva 0:46ef60040f6a 930 }
marcodesilva 0:46ef60040f6a 931
marcodesilva 0:46ef60040f6a 932 float ARAP180_WITH_ROVER::batteryPos_from_wrench(Vector6f desired_wrench, Vector3f droneOrientation){
marcodesilva 0:46ef60040f6a 933
marcodesilva 0:46ef60040f6a 934 float A0 = 0.0;
marcodesilva 0:46ef60040f6a 935
marcodesilva 0:46ef60040f6a 936 float w5 = desired_wrench[4];
marcodesilva 0:46ef60040f6a 937 float w6 = desired_wrench[5];
marcodesilva 0:46ef60040f6a 938
marcodesilva 0:46ef60040f6a 939 float r_b_w = -droneOrientation[0];
marcodesilva 0:46ef60040f6a 940 float p_b_w = -droneOrientation[1];
marcodesilva 0:46ef60040f6a 941 float t2 = cos(p_b_w);
marcodesilva 0:46ef60040f6a 942 float t3 = cos(r_b_w);
marcodesilva 0:46ef60040f6a 943 float t4 = sin(r_b_w);
marcodesilva 0:46ef60040f6a 944 float t5 = t2*t2;
marcodesilva 0:46ef60040f6a 945 float t6 = t3*t3;
marcodesilva 0:46ef60040f6a 946 A0 = ((t4*w6*-5.0E+2+mpz_battery*(t4*t4)*4.907E+3+mpz_battery*t5*t6*4.907E+3-t2*t3*w5*5.0E+2+mpx_battery*t2*t6*sin(p_b_w)*4.907E+3)*(-2.037905033625433E-4))/(mass_battery*(-t6+t5*t6+1.0));
marcodesilva 0:46ef60040f6a 947 return A0;
marcodesilva 0:46ef60040f6a 948 }
marcodesilva 0:46ef60040f6a 949
marcodesilva 0:46ef60040f6a 950 // Inertia matrix B
marcodesilva 0:46ef60040f6a 951
marcodesilva 0:46ef60040f6a 952
marcodesilva 0:46ef60040f6a 953
marcodesilva 0:46ef60040f6a 954