PRISMA Lab
Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of
Hyfliers_Completo_testato
by 
Marco De Silva
Revision 9:03e93b86a922, committed 2021-12-20
- Comitter:
- marcodesilva
- Date:
- Mon Dec 20 10:30:08 2021 +0000
- Parent:
- 8:4d44a9695adf
- Child:
- 10:62e9b61ed1ad
- Commit message:
- 20/12/2021 Funzionante completo
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ARM.lib Mon Dec 20 10:30:08 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/marcodesilva/code/ARM/#8d9a79e18fb9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ROVER.lib Mon Dec 20 10:30:08 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/marcodesilva/code/ROVER/#65943c77d1dc
--- a/Robots/ARM/ARAP180_with_rover.cpp Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,954 +0,0 @@
-#include "ARAP180_with_rover.h"
-#include "ARM_parameters.h"
-#include <iostream>
-
-//*****************************************************************************
-
-ARAP180_WITH_ROVER::ARAP180_WITH_ROVER(){
- dxl_port = new UARTSerial_half(PE_8,PE_7,PE_10, 1000000);
- //tool_com = new UARTSerial_mio(tool_tx, tool_rx);
- //tool_com->set_baud(460800);
-
- mx_MotorChain = new MX(ID, sizeof(ID)/sizeof(int), 1000000, dxl_port);
-
- ID[0] = 1;
- ID[1] = 2;
- ID[2] = 3;
- ID[3] = 4;
- ID[4] = 5;
- ID[5] = 6;
-
- motorSign(0) = 1;
- motorSign(1) = 1;
- motorSign(2) = 1;
- motorSign(3) = -1;
- motorSign(4) = 1;
- motorSign(5) = 1;
- motorSign(6) = -1;
- motorSign(7) = 1;
-
- toolGearRatio = 12.4553;
-
-
- offsets(0) = 0.0;
- offsets(1) = 0.0;
- offsets(2) = -22.5;
- offsets(3) = 0.0;
- offsets(4) = 0.0;
- offsets(5) = 0.0;
- offsets(6) = 0.0;
- offsets(7) = -360.0/toolGearRatio;
-
- extendedSelect[0] = 0;
- extendedSelect[1] = 0;
- extendedSelect[2] = 0;
- extendedSelect[3] = 0;
- extendedSelect[4] = 0;
- extendedSelect[5] = 1;
-
-
- a3 = 0.029;
- a4 = 0.340;
- a5 = 0.029;
- a5i = 0.335;
- a6 = 0.084;
- d7 = -0.218;
- // drone distances
- x_0_r2 = 0.0;
- y_0_r2 = -0.3;
- z_0_r2 = 0.36;
-
-
-}
-
-
-void ARAP180_WITH_ROVER::setParameters(VectorXd armParameters){
-
- a3 = armParameters(0);// 0.036
- a4 = armParameters(1);// 0.36836
- a5 = armParameters(2);// -0.029
- a5i = armParameters(3);
- a6 = armParameters(4);
- d7 = armParameters(5);// 0.370
- x_0_r2 = armParameters(6);
- y_0_r2 = armParameters(7);
- z_0_r2 = armParameters(8);// 0.53378
-
- massr = armParameters(9);
- mpxr = armParameters(10);
- mpyr = armParameters(11);
- mpzr = armParameters(12);
- mass1 = armParameters(13);
- mpx1 = armParameters(14);
- mpy1 = armParameters(15);
- mpz1 = armParameters(16);
- mass2 = armParameters(17);
- mpx2 = armParameters(18);
- mpy2 = armParameters(19);
- mpz2 = armParameters(20);
- mass3 = armParameters(21);
- mpx3 = armParameters(22);
- mpy3 = armParameters(23);
- mpz3 = armParameters(24);
- mass4 = armParameters(25);
- mpx4 = armParameters(26);
- mpy4 = armParameters(27);
- mpz4 = armParameters(28);
- mass5 = armParameters(29);
- mpx5 = armParameters(30);
- mpy5 = armParameters(31);
- mpz5 = armParameters(32);
- mass6 = armParameters(33);
- mpx6 = armParameters(34);
- mpy6 = armParameters(35);
- mpz6 = armParameters(36);
-
- mass_battery = armParameters(37);
- mpx_battery = armParameters(38);
- mpz_battery = armParameters(39);
-
- std::cout << "armParameters: " << armParameters << std::endl;
-}
-
-
-
-void ARAP180_WITH_ROVER::setOperatingMode(int modeArm, int modeTool){ //mode = 3 for position control
-
- int operatingMode[6];
-
- operatingMode[0] = modeArm;
- operatingMode[1] = modeArm;
- operatingMode[2] = modeArm;
- operatingMode[3] = modeArm;
- operatingMode[4] = modeArm;
- operatingMode[5] = modeTool;
-
- mx_MotorChain->SyncOperatingMode(operatingMode, -1);
-}
-
-void ARAP180_WITH_ROVER::enableMotors(int enable){
- bool enableVal[6];
- enableVal[0] = enable;
- enableVal[1] = enable;
- enableVal[2] = enable;
- enableVal[3] = enable;
- enableVal[4] = enable;
- enableVal[5] = enable;
-
- mx_MotorChain->SyncTorqueEnable(enableVal, -1);
-}
-
-void ARAP180_WITH_ROVER::setMaxVel(float maxVel){
-
- maxVel = 695.5*maxVel;//conversione velocità
- float Vel_profile[6] = {maxVel,maxVel,maxVel,maxVel,maxVel,maxVel};
- mx_MotorChain->SyncProfileVel(Vel_profile);
-
-}
-
-
-void ARAP180_WITH_ROVER::setMaxAcc(float maxAcc){
-
- maxAcc = 8.9*maxAcc; //8.9=60*200/(6.28*214) //742
- float Acc_profile[6] = {maxAcc,maxAcc,maxAcc,maxAcc,maxAcc,maxAcc};
- mx_MotorChain->SyncProfileAccel(Acc_profile);
-
-}
-
-void ARAP180_WITH_ROVER::setJointPos(Vector8f q){
-
-
- float Goal_position[6];
-
- //For pololu control
-
- /*union Packet{
- float f;
- char c[8];
- };
- Packet p;*/
-
- Goal_position[0] = utilities::rad2deg(q(2)) * motorSign(2) + offsets(2);
- Goal_position[1] = utilities::rad2deg(q(3)) * motorSign(3) + offsets(3);
- Goal_position[2] = utilities::rad2deg(q(4)) * motorSign(4) + offsets(4);
- Goal_position[3] = utilities::rad2deg(q(5)) * motorSign(5) + offsets(5);
- Goal_position[4] = utilities::rad2deg(q(6)) * motorSign(6) + offsets(6);
-
- Goal_position[5] = toolGearRatio*(utilities::rad2deg(q(7)) * motorSign(7) + offsets(7));
-
- mx_MotorChain->SyncSetGoalExtended(Goal_position, extendedSelect);
-
- //For pololu control
- /*mx_MotorChain->SyncSetGoal(Goal_position);
-
- p.f = (q(7) * motorSign(7) + offsets(7));
-
- tool_com->write(p.c,8);*/
-
-}
-
-
-
-Vector6f ARAP180_WITH_ROVER::getJointPos(bool checkData){
-
- Vector6f getPosConverted = Vector6f::Zero();
- float getPos[6];
- bool dataCheck;
-
- if(checkData){
- while(!mx_MotorChain->SyncGetExtendedPosition(getPos, extendedSelect)){
- ThisThread::sleep_for(0.1);
- //printf("wait for arm position!\n");
- }
- }else{
- mx_MotorChain->SyncGetExtendedPosition(getPos, extendedSelect);
- }
-
- getPosConverted(0) = utilities::deg2rad(getPos[0] - offsets(2)) * motorSign(2);
- getPosConverted(1) = utilities::deg2rad(getPos[1] - offsets(3)) * motorSign(3);
- getPosConverted(2) = utilities::deg2rad(getPos[2] - offsets(4)) * motorSign(4);
- getPosConverted(3) = utilities::deg2rad(getPos[3] - offsets(5)) * motorSign(5);
- getPosConverted(4) = utilities::deg2rad(getPos[4] - offsets(6)) * motorSign(6);
- getPosConverted(5) = utilities::deg2rad(getPos[5]/toolGearRatio - offsets(7)) * motorSign(7);
-
- return getPosConverted;
-
-}
-
-
-//Dyrect Kinematics Matrix Te
-Matrix4f ARAP180_WITH_ROVER::forwardKinematics(Vector8f q){
-
- Matrix4f A0 = Matrix4f::Zero();
- volatile float q1 = q[0];
- volatile float q2 = q[1];
- volatile float q3 = q[2];
- volatile float q4 = q[3];
- volatile float q5 = q[4];
- volatile float q6 = q[5];
- volatile float q7 = q[6];
- volatile float q8 = q[7];
-
- float t2 = cos(q2);
- float t3 = cos(q3);
- float t4 = cos(q4);
- float t5 = cos(q5);
- float t6 = cos(q6);
- float t7 = cos(q7);
- float t8 = cos(q8);
- float t9 = sin(q2);
- float t10 = sin(q3);
- float t11 = sin(q4);
- float t12 = sin(q5);
- float t13 = sin(q6);
- float t14 = sin(q7);
- float t15 = sin(q8);
- float t16 = q4*-1.0;
- float t17 = q4*1.0;
- float t18 = q5*-1.0;
- float t19 = q5*1.0;
- float t20 = -q4;
- float t21 = t2*t4;
- float t22 = t2*t11;
- float t23 = t4*t9;
- float t24 = t7*t10;
- float t25 = t9*t11;
- float t26 = q4+t18;
- float t27 = q5+q6+t16;
- float t30 = q5+q6+t20;
- float t34 = t2*t3*t7*-1.0;
- float t35 = t2*t3*t7*1.0;
- float t36 = t3*t7*t9*-1.0;
- float t37 = t3*t7*t9*1.0;
- float t28 = sin(t26);
- float t29 = t10*t25;
- float t31 = cos(t27);
- float t32 = sin(t27);
- float t33 = t10*t21;
- float t38 = t10*t22*-1.0;
- float t39 = t10*t22*1.0;
- float t40 = t10*t23*-1.0;
- float t41 = t10*t23*1.0;
- float t42 = t3*t14*t31;
- float t43 = t21+t29;
- float t44 = t25+t33;
- float t45 = t22+t40;
- float t46 = t23+t38;
- float t47 = t5*t43;
- float t48 = t12*t43;
- float t49 = t5*t44*1.0;
- float t50 = t12*t45;
- float t51 = t12*t44*1.0;
- float t52 = t5*t45*-1.0;
- float t53 = t5*t45*1.0;
- float t54 = t5*t46*1.0;
- float t55 = t12*t46*-1.0;
- float t56 = t12*t46*1.0;
- float t57 = t24+t42;
- float t58 = t47+t50;
- float t59 = t48+t52;
- float t60 = t49+t55;
- float t61 = t51+t54;
- float t62 = t6*t58;
- float t63 = t13*t58;
- float t64 = t6*t59;
- float t65 = t13*t59*-1.0;
- float t66 = t13*t59*1.0;
- float t67 = t13*t60*1.0;
- float t68 = t13*t61*-1.0;
- float t69 = t13*t61*1.0;
- float t70 = t6*t60*1.0;
- float t71 = t6*t61*1.0;
- float t72 = t63+t64;
- float t73 = t62+t65;
- float t75 = t68+t70;
- float t76 = t67+t71;
- float t74 = t14*t72;
- float t77 = t14*t75*1.0;
- float t78 = t36+t74;
- float t79 = t34+t77;
- A0(0,0) = t8*t57+t3*t15*t32;
- A0(0,1) = t15*t57*-1.0+t3*t8*t32;
- A0(0,2) = t10*t14*-1.0+t3*t7*cos(t30);
- 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);
- A0(1,0) = t8*t79*-1.0-t15*t76*1.0;
- A0(1,1) = t8*t76*-1.0+t15*t79;
- A0(1,2) = t7*t75*-1.0-t2*t3*t14*1.0;
- 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;
- A0(2,0) = t8*t78*-1.0+t15*t73;
- A0(2,1) = t8*t73+t15*t78;
- A0(2,2) = t7*t72*-1.0-t3*t9*t14*1.0;
- 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;
- A0(3,3) = 1.0;
-
-
-return A0;
-}
-
-//Jacobian matrix J
-Matrix<float,6,8> ARAP180_WITH_ROVER::jacobianMatrix(Vector8f q){
-
- Matrix<float,6,8> A0 = Matrix<float,6,8>::Zero();
- volatile float q1 = q[0];
- volatile float q2 = q[1];
- volatile float q3 = q[2];
- volatile float q4 = q[3];
- volatile float q5 = q[4];
- volatile float q6 = q[5];
- volatile float q7 = q[6];
- volatile float q8 = q[7];
-
-
- float t2 = cos(q2);
- float t3 = cos(q3);
- float t4 = cos(q4);
- float t5 = cos(q5);
- float t6 = cos(q6);
- float t7 = cos(q7);
- float t8 = sin(q2);
- float t9 = sin(q3);
- float t10 = sin(q4);
- float t11 = sin(q5);
- float t12 = sin(q6);
- float t13 = sin(q7);
- float t14 = q5*-1.0;
- float t15 = q5*1.0;
- float t17 = -q4;
- float t16 = a3*t3;
- float t18 = t2*t3;
- float t19 = t2*t4;
- float t20 = t3*t8;
- float t21 = t2*t10;
- float t22 = t4*t8;
- float t23 = t8*t10;
- float t24 = q4+t14;
- float t28 = a4*t3*t10;
- float t30 = q5+q6+t17;
- float t32 = a3*t2*t9*1.0;
- float t33 = a3*t8*t9*1.0;
- float t40 = a5*t3*t4*t5;
- float t41 = a5*t3*t10*t11;
- float t25 = cos(t24);
- float t26 = sin(t24);
- float t27 = a4*t22;
- float t29 = t9*t23;
- float t31 = a4*t19*1.0;
- float t34 = t9*t19;
- float t35 = t9*t21;
- float t38 = t9*t22*-1.0;
- float t39 = t9*t22*1.0;
- float t42 = sin(t30);
- float t36 = t35*-1.0;
- float t37 = t35*1.0;
- float t45 = a4*t29*1.0;
- float t46 = -t35;
- float t47 = a5i*t3*t26*-1.0;
- float t48 = a5i*t3*t26*1.0;
- float t49 = a6*t3*t6*t25;
- float t50 = a6*t3*t12*t26;
- float t51 = d7*t3*t42*-1.0;
- float t52 = d7*t3*t42*1.0;
- float t53 = t19+t29;
- float t54 = t23+t34;
- float t55 = t21+t38;
- float t43 = a4*t36;
- float t44 = a4*t37;
- float t56 = t22+t36;
- float t57 = t22+t46;
- float t58 = t5*t53;
- float t59 = t11*t53;
- float t60 = t11*t54;
- float t62 = t5*t54*1.0;
- float t63 = t11*t55;
- float t65 = t5*t55*-1.0;
- float t66 = t5*t55*1.0;
- float t70 = a5*t5*t55;
- float t71 = a5*t5*t54*-1.0;
- float t89 = t49+t50+t51;
- float t61 = t5*t56;
- float t64 = t60*1.0;
- float t68 = t11*t56*-1.0;
- float t69 = t11*t56*1.0;
- float t72 = a5*t62;
- float t73 = a5*t11*t56;
- float t74 = a5*t59*1.0;
- float t75 = t5*t57;
- float t76 = a5*t11*t57;
- float t77 = -t70;
- float t79 = t58+t63;
- float t81 = t59+t65;
- float t104 = t40+t41+t47+t89;
- float t67 = t61*1.0;
- float t78 = -t76;
- float t80 = t60+t61;
- float t82 = t60+t75;
- float t83 = t62+t68;
- float t85 = a5i*t79;
- float t88 = t6*t79;
- float t91 = t12*t81;
- float t98 = a6*t12*t79*1.0;
- float t99 = a6*t6*t81*1.0;
- float t105 = t28+t104;
- float t84 = t64+t67;
- float t86 = a5i*t80*-1.0;
- float t87 = a5i*t80*1.0;
- float t90 = t88*1.0;
- float t92 = a5i*t82*1.0;
- float t93 = -t85;
- float t95 = t12*t83;
- float t96 = t91*-1.0;
- float t97 = t91*1.0;
- float t101 = -t91;
- float t102 = a6*t6*t83*-1.0;
- float t103 = a6*t6*t83*1.0;
- float t106 = t16+t105;
- float t94 = t6*t84;
- float t100 = a6*t12*t84;
- float t107 = t88+t96;
- float t108 = t90+t101;
- float t109 = t94+t95;
- float t110 = d7*t107*1.0;
- float t111 = d7*t109;
- float t112 = t98+t99+t110;
- float t113 = t74+t77+t93+t112;
- float t114 = t71+t73+t86+t100+t102+t111;
- float t117 = t73+t78+t86+t92+t100+t102+t111;
- float t115 = t27+t43+t114;
- float t116 = t31+t45+t113;
- A0(0,0) = 1.0;
- A0(0,2) = -t8*(t33+t116)-t2*(-t27+t32+t44+t72-t73+t87-t100+t103-t111)*1.0;
- A0(0,3) = t18*t116*1.0+t20*t115*1.0;
- A0(0,4) = t20*(t72-t73+t87-t100+t103-t111)-t18*t113;
- A0(0,5) = -t18*t112-t20*t117;
- A0(0,6) = t107*t111*-1.0+t108*t111;
- 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;
- A0(1,2) = t2*t106*-1.0;
- A0(1,3) = t9*t116*-1.0-t20*t105*1.0;
- A0(1,4) = t9*t113+t20*t104;
- A0(1,5) = t20*t89+t9*t112;
- A0(1,6) = t51*t107+t52*t108;
- 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;
- A0(2,2) = -t8*t106;
- A0(2,3) = t18*t105*1.0-t9*t115*1.0;
- A0(2,4) = -t9*(t72-t73+t87-t100+t103-t111)-t18*t104;
- A0(2,5) = -t18*t89+t9*t117;
- A0(3,1) = 1.0;
- A0(3,3) = t9*-1.0;
- A0(3,4) = t9;
- A0(3,5) = t9;
- A0(3,6) = t3*t42*-1.0;
- A0(3,7) = t9*t13*-1.0+t3*t7*cos(t30);
- A0(4,2) = t8;
- A0(4,3) = t18*-1.0;
- A0(4,4) = t18;
- A0(4,5) = t18;
- A0(4,6) = t109;
- A0(4,7) = t7*(t6*t83*1.0-t12*t84*1.0)*-1.0-t13*t18*1.0;
- A0(5,2) = t2*-1.0;
- A0(5,3) = t20*-1.0;
- A0(5,4) = t20;
- A0(5,5) = t20;
- A0(5,6) = -t90+t91;
- A0(5,7) = t13*t20*-1.0-t7*(t6*t81+t12*t79)*1.0;
- return A0;
-}
-
-
-//Jacobian matrix J
-Matrix<float,6,8> ARAP180_WITH_ROVER::jacobianTimeDerivativeMatrix(Vector8f q, Vector8f dq){
- volatile float q1 = q[0];
- volatile float q2 = q[1];
- volatile float q3 = q[2];
- volatile float q4 = q[3];
- volatile float q5 = q[4];
- volatile float q6 = q[5];
- volatile float q7 = q[6];
- volatile float q8 = q[7];
-
- volatile float dq1 = dq[0];
- volatile float dq2 = dq[1];
- volatile float dq3 = dq[2];
- volatile float dq4 = dq[3];
- volatile float dq5 = dq[4];
- volatile float dq6 = dq[5];
- volatile float dq7 = dq[6];
- volatile float dq8 = dq[7];
-
- Matrix<float,6,8> A0 = Matrix<float,6,8>::Zero();
-
-
-return A0;
-}
-
-
-Vector6f ARAP180_WITH_ROVER::Gr(Vector8f q, Vector3f droneOrientation){
-
- Vector6f A0 = Vector6f::Zero();
- 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
- volatile float q2 = q[1];
- volatile float q3 = q[2];
- volatile float q4 = q[3];
- volatile float q5 = q[4];
- volatile float q6 = q[5];
- volatile float q7 = q[6];
- volatile float q8 = q[7];
-
- float r_b_d = -droneOrientation[0];
- float p_b_d = -droneOrientation[1];
-
- float t2 = cos(p_b_d);
- float t3 = cos(q2);
- float t4 = cos(q3);
- float t5 = cos(q4);
- float t6 = cos(q5);
- float t7 = cos(q6);
- float t8 = cos(q7);
- float t9 = cos(q8);
- float t10 = cos(r_b_d);
- float t11 = sin(p_b_d);
- float t12 = sin(q2);
- float t13 = sin(q3);
- float t14 = sin(q4);
- float t15 = sin(q5);
- float t16 = sin(q6);
- float t17 = sin(q7);
- float t18 = sin(q8);
- float t19 = sin(r_b_d);
- float t20 = t3*t4*t19;
- float t21 = t10*t11*t13;
- float t22 = t2*t4*t10*t12;
- float t24 = t12*t19*9.814;
- float t25 = t2*t3*t10*9.814;
- float t26 = t4*t10*t11*9.814;
- float t27 = t3*t13*t19*9.814;
- float t28 = t2*t10*t12*t13*9.814;
- float t62 = mpx4*t5*t6*t7*t12*t19*(-9.814);
- float t63 = mpx4*t6*t12*t14*t16*t19*(-9.814);
- float t64 = mpx4*t7*t12*t14*t15*t19*(-9.814);
- float t117 = a6*mass5*t5*t6*t7*t12*t19*(-9.814);
- float t118 = a6*mass6*t5*t6*t7*t12*t19*(-9.814);
- float t119 = d7*mass6*t6*t7*t12*t14*t19*(-9.814);
- float t120 = a6*mass5*t6*t12*t14*t16*t19*(-9.814);
- float t121 = a6*mass5*t7*t12*t14*t15*t19*(-9.814);
- float t122 = a6*mass6*t6*t12*t14*t16*t19*(-9.814);
- float t123 = a6*mass6*t7*t12*t14*t15*t19*(-9.814);
- float t140 = mpx4*t4*t10*t11*t14*t15*t16*(-9.814);
- float t141 = mpx4*t3*t13*t14*t15*t16*t19*(-9.814);
- float t142 = mpy6*t9*t12*t14*t15*t16*t19*(-9.814);
- float t191 = mpx4*t2*t3*t5*t6*t7*t10*(-9.814);
- float t192 = mpx4*t2*t3*t6*t10*t14*t16*(-9.814);
- float t193 = mpx4*t2*t3*t7*t10*t14*t15*(-9.814);
- float t194 = mpx4*t4*t5*t6*t10*t11*t16*(-9.814);
- float t195 = mpx4*t4*t5*t7*t10*t11*t15*(-9.814);
- float t196 = mpx4*t3*t5*t6*t13*t16*t19*(-9.814);
- float t197 = mpx4*t3*t5*t7*t13*t15*t19*(-9.814);
- float t198 = mpy6*t5*t6*t9*t12*t16*t19*(-9.814);
- float t199 = mpy6*t5*t7*t9*t12*t15*t19*(-9.814);
- float t200 = a6*mass5*t2*t3*t5*t6*t7*t10*(-9.814);
- float t201 = a6*mass6*t2*t3*t5*t6*t7*t10*(-9.814);
- float t202 = d7*mass6*t2*t3*t6*t7*t10*t14*(-9.814);
- float t203 = d7*mass6*t4*t5*t6*t7*t10*t11*(-9.814);
- float t204 = a6*mass5*t2*t3*t6*t10*t14*t16*(-9.814);
- float t205 = a6*mass5*t2*t3*t7*t10*t14*t15*(-9.814);
- float t206 = a6*mass5*t4*t5*t6*t10*t11*t16*(-9.814);
- float t207 = a6*mass5*t4*t5*t7*t10*t11*t15*(-9.814);
- float t208 = a6*mass6*t2*t3*t6*t10*t14*t16*(-9.814);
- float t209 = a6*mass6*t2*t3*t7*t10*t14*t15*(-9.814);
- float t210 = a6*mass6*t4*t5*t6*t10*t11*t16*(-9.814);
- float t211 = a6*mass6*t4*t5*t7*t10*t11*t15*(-9.814);
- float t212 = d7*mass6*t3*t5*t6*t7*t13*t19*(-9.814);
- float t213 = a6*mass5*t3*t5*t6*t13*t16*t19*(-9.814);
- float t214 = a6*mass5*t3*t5*t7*t13*t15*t19*(-9.814);
- float t215 = a6*mass6*t3*t5*t6*t13*t16*t19*(-9.814);
- float t216 = a6*mass6*t3*t5*t7*t13*t15*t19*(-9.814);
- float t217 = d7*mass6*t4*t6*t10*t11*t14*t16*(-9.814);
- float t218 = d7*mass6*t4*t7*t10*t11*t14*t15*(-9.814);
- float t219 = a6*mass5*t4*t10*t11*t14*t15*t16*(-9.814);
- float t220 = a6*mass6*t4*t10*t11*t14*t15*t16*(-9.814);
- float t221 = d7*mass6*t3*t6*t13*t14*t16*t19*(-9.814);
- float t222 = d7*mass6*t3*t7*t13*t14*t15*t19*(-9.814);
- float t223 = a6*mass5*t3*t13*t14*t15*t16*t19*(-9.814);
- float t224 = a6*mass6*t3*t13*t14*t15*t16*t19*(-9.814);
- float t257 = mpy6*t2*t3*t5*t6*t9*t10*t16*(-9.814);
- float t258 = mpy6*t2*t3*t5*t7*t9*t10*t15*(-9.814);
- float t259 = mpx4*t2*t6*t7*t10*t12*t13*t14*(-9.814);
- float t260 = mpy6*t2*t3*t9*t10*t14*t15*t16*(-9.814);
- float t261 = mpy6*t4*t5*t9*t10*t11*t15*t16*(-9.814);
- float t262 = mpy6*t3*t5*t9*t13*t15*t16*t19*(-9.814);
- float t263 = mpy6*t5*t12*t15*t16*t17*t18*t19*(-9.814);
- float t265 = a6*mass5*t2*t6*t7*t10*t12*t13*t14*(-9.814);
- float t266 = a6*mass6*t2*t6*t7*t10*t12*t13*t14*(-9.814);
- float t267 = d7*mass6*t2*t5*t10*t12*t13*t15*t16*(-9.814);
- float t284 = mpy6*t2*t5*t6*t7*t9*t10*t12*t13*(-9.814);
- float t285 = mpy6*t2*t6*t9*t10*t12*t13*t14*t16*(-9.814);
- float t286 = mpy6*t2*t7*t9*t10*t12*t13*t14*t15*(-9.814);
- float t287 = mpy6*t2*t3*t5*t10*t15*t16*t17*t18*(-9.814);
- float t288 = mpy6*t4*t6*t7*t10*t11*t14*t17*t18*(-9.814);
- float t289 = mpy6*t3*t6*t7*t13*t14*t17*t18*t19*(-9.814);
- float t295 = mpy6*t2*t5*t6*t10*t12*t13*t16*t17*t18*(-9.814);
- float t296 = mpy6*t2*t5*t7*t10*t12*t13*t15*t17*t18*(-9.814);
- float t297 = mpy6*t2*t10*t12*t13*t14*t15*t16*t17*t18*(-9.814);
- float t23 = -t20;
- float t29 = mpx3*t5*t6*t24;
- float t30 = mpy3*t5*t15*t24;
- float t31 = mpy3*t6*t14*t24;
- float t32 = mpx3*t14*t15*t24;
- float t33 = -t28;
- float t34 = a5*mass4*t5*t6*t24;
- float t35 = a5*mass5*t5*t6*t24;
- float t36 = a5*mass6*t5*t6*t24;
- float t37 = a5i*mass4*t5*t15*t24;
- float t38 = a5i*mass4*t6*t14*t24;
- float t39 = a5i*mass5*t5*t15*t24;
- float t40 = a5i*mass5*t6*t14*t24;
- float t41 = a5i*mass6*t5*t15*t24;
- float t42 = a5i*mass6*t6*t14*t24;
- float t43 = a5*mass4*t14*t15*t24;
- float t44 = a5*mass5*t14*t15*t24;
- float t45 = a5*mass6*t14*t15*t24;
- float t46 = mpy3*t5*t15*t25;
- float t47 = mpy3*t6*t14*t25;
- float t48 = mpy3*t5*t6*t26;
- float t49 = mpx3*t14*t15*t25;
- float t50 = mpx3*t5*t15*t26;
- float t51 = mpx3*t6*t14*t26;
- float t52 = mpx4*t5*t6*t7*t24;
- float t53 = mpy3*t5*t6*t27;
- float t54 = mpy3*t14*t15*t26;
- float t55 = mpx3*t5*t15*t27;
- float t56 = mpx3*t6*t14*t27;
- float t57 = mpx4*t5*t15*t16*t24;
- float t58 = mpx4*t6*t14*t16*t24;
- float t59 = mpx4*t7*t14*t15*t24;
- float t60 = mpy3*t14*t15*t27;
- float t61 = mpx3*t5*t6*t25;
- float t65 = a5*mass4*t5*t6*t25;
- float t66 = a5*mass5*t5*t6*t25;
- float t67 = a5*mass6*t5*t6*t25;
- float t68 = a5i*mass4*t5*t15*t25;
- float t69 = a5i*mass4*t6*t14*t25;
- float t70 = a5i*mass4*t5*t6*t26;
- float t71 = a5i*mass5*t5*t15*t25;
- float t72 = a5i*mass5*t6*t14*t25;
- float t73 = a5i*mass5*t5*t6*t26;
- float t74 = a5i*mass6*t5*t15*t25;
- float t75 = a5i*mass6*t6*t14*t25;
- float t76 = a5i*mass6*t5*t6*t26;
- float t77 = a5*mass4*t14*t15*t25;
- float t78 = a5*mass4*t5*t15*t26;
- float t79 = a5*mass4*t6*t14*t26;
- float t80 = a5*mass5*t14*t15*t25;
- float t81 = a5*mass5*t5*t15*t26;
- float t82 = a5*mass5*t6*t14*t26;
- float t83 = a5*mass6*t14*t15*t25;
- float t84 = a5*mass6*t5*t15*t26;
- float t85 = a5*mass6*t6*t14*t26;
- float t86 = a6*mass5*t5*t6*t7*t24;
- float t87 = a6*mass6*t5*t6*t7*t24;
- float t88 = a5i*mass4*t5*t6*t27;
- float t89 = a5i*mass5*t5*t6*t27;
- float t90 = a5i*mass6*t5*t6*t27;
- float t91 = a5i*mass4*t14*t15*t26;
- float t92 = a5i*mass5*t14*t15*t26;
- float t93 = a5i*mass6*t14*t15*t26;
- float t94 = a5*mass4*t5*t15*t27;
- float t95 = a5*mass4*t6*t14*t27;
- float t96 = a5*mass5*t5*t15*t27;
- float t97 = a5*mass5*t6*t14*t27;
- float t98 = a5*mass6*t5*t15*t27;
- float t99 = a5*mass6*t6*t14*t27;
- float t100 = d7*mass6*t5*t6*t16*t24;
- float t101 = d7*mass6*t5*t7*t15*t24;
- float t102 = d7*mass6*t6*t7*t14*t24;
- float t103 = a6*mass5*t5*t15*t16*t24;
- float t104 = a6*mass5*t6*t14*t16*t24;
- float t105 = a6*mass5*t7*t14*t15*t24;
- float t106 = a6*mass6*t5*t15*t16*t24;
- float t107 = a6*mass6*t6*t14*t16*t24;
- float t108 = a6*mass6*t7*t14*t15*t24;
- float t109 = a5i*mass4*t14*t15*t27;
- float t110 = a5i*mass5*t14*t15*t27;
- float t111 = a5i*mass6*t14*t15*t27;
- float t112 = d7*mass6*t14*t15*t16*t24;
- float t113 = mpx4*t14*t15*t16*t26;
- float t114 = mpy3*t14*t15*t28;
- float t115 = mpx4*t14*t15*t16*t27;
- float t116 = mpy6*t9*t14*t15*t16*t24;
- float t124 = mpx4*t5*t6*t7*t25;
- float t125 = mpx4*t5*t15*t16*t25;
- float t126 = mpx4*t6*t14*t16*t25;
- float t127 = mpx4*t7*t14*t15*t25;
- float t128 = mpx4*t5*t6*t16*t26;
- float t129 = mpx4*t5*t7*t15*t26;
- float t130 = mpx4*t6*t7*t14*t26;
- float t131 = mpy3*t5*t6*t28;
- float t132 = mpx3*t5*t15*t28;
- float t133 = mpx3*t6*t14*t28;
- float t134 = mpx4*t5*t6*t16*t27;
- float t135 = mpx4*t5*t7*t15*t27;
- float t136 = mpx4*t6*t7*t14*t27;
- float t137 = mpy6*t5*t6*t9*t16*t24;
- float t138 = mpy6*t5*t7*t9*t15*t24;
- float t139 = mpy6*t6*t7*t9*t14*t24;
- float t143 = a6*mass5*t5*t6*t7*t25;
- float t144 = a6*mass6*t5*t6*t7*t25;
- float t145 = d7*mass6*t5*t6*t16*t25;
- float t146 = d7*mass6*t5*t7*t15*t25;
- float t147 = d7*mass6*t6*t7*t14*t25;
- float t148 = d7*mass6*t5*t6*t7*t26;
- float t149 = a6*mass5*t5*t15*t16*t25;
- float t150 = a6*mass5*t6*t14*t16*t25;
- float t151 = a6*mass5*t7*t14*t15*t25;
- float t152 = a6*mass5*t5*t6*t16*t26;
- float t153 = a6*mass5*t5*t7*t15*t26;
- float t154 = a6*mass5*t6*t7*t14*t26;
- float t155 = a6*mass6*t5*t15*t16*t25;
- float t156 = a6*mass6*t6*t14*t16*t25;
- float t157 = a6*mass6*t7*t14*t15*t25;
- float t158 = a6*mass6*t5*t6*t16*t26;
- float t159 = a6*mass6*t5*t7*t15*t26;
- float t160 = a6*mass6*t6*t7*t14*t26;
- float t161 = a5i*mass4*t5*t6*t28;
- float t162 = a5i*mass5*t5*t6*t28;
- float t163 = a5i*mass6*t5*t6*t28;
- float t164 = d7*mass6*t5*t6*t7*t27;
- float t165 = a5*mass4*t5*t15*t28;
- float t166 = a5*mass4*t6*t14*t28;
- float t167 = a5*mass5*t5*t15*t28;
- float t168 = a5*mass5*t6*t14*t28;
- float t169 = a5*mass6*t5*t15*t28;
- float t170 = a5*mass6*t6*t14*t28;
- float t171 = a6*mass5*t5*t6*t16*t27;
- float t172 = a6*mass5*t5*t7*t15*t27;
- float t173 = a6*mass5*t6*t7*t14*t27;
- float t174 = a6*mass6*t5*t6*t16*t27;
- float t175 = a6*mass6*t5*t7*t15*t27;
- float t176 = a6*mass6*t6*t7*t14*t27;
- float t177 = d7*mass6*t14*t15*t16*t25;
- float t178 = d7*mass6*t5*t15*t16*t26;
- float t179 = d7*mass6*t6*t14*t16*t26;
- float t180 = d7*mass6*t7*t14*t15*t26;
- float t181 = a6*mass5*t14*t15*t16*t26;
- float t182 = a6*mass6*t14*t15*t16*t26;
- float t183 = a5i*mass4*t14*t15*t28;
- float t184 = a5i*mass5*t14*t15*t28;
- float t185 = a5i*mass6*t14*t15*t28;
- float t186 = d7*mass6*t5*t15*t16*t27;
- float t187 = d7*mass6*t6*t14*t16*t27;
- float t188 = d7*mass6*t7*t14*t15*t27;
- float t189 = a6*mass5*t14*t15*t16*t27;
- float t190 = a6*mass6*t14*t15*t16*t27;
- float t225 = mpy6*t5*t6*t9*t16*t25;
- float t226 = mpy6*t5*t7*t9*t15*t25;
- float t227 = mpy6*t6*t7*t9*t14*t25;
- float t228 = mpy6*t5*t6*t7*t9*t26;
- float t229 = mpy6*t5*t6*t7*t9*t27;
- float t230 = mpx4*t5*t6*t16*t28;
- float t231 = mpx4*t5*t7*t15*t28;
- float t232 = mpx4*t6*t7*t14*t28;
- float t233 = mpy6*t9*t14*t15*t16*t25;
- float t234 = mpy6*t5*t9*t15*t16*t26;
- float t235 = mpy6*t6*t9*t14*t16*t26;
- float t236 = mpy6*t7*t9*t14*t15*t26;
- float t237 = mpy6*t5*t9*t15*t16*t27;
- float t238 = mpy6*t6*t9*t14*t16*t27;
- float t239 = mpy6*t7*t9*t14*t15*t27;
- float t240 = mpy6*t5*t6*t7*t17*t18*t24;
- float t241 = mpx4*t14*t15*t16*t28;
- float t242 = mpy6*t5*t15*t16*t17*t18*t24;
- float t243 = mpy6*t6*t14*t16*t17*t18*t24;
- float t244 = mpy6*t7*t14*t15*t17*t18*t24;
- float t245 = d7*mass6*t5*t6*t7*t28;
- float t246 = a6*mass5*t5*t6*t16*t28;
- float t247 = a6*mass5*t5*t7*t15*t28;
- float t248 = a6*mass5*t6*t7*t14*t28;
- float t249 = a6*mass6*t5*t6*t16*t28;
- float t250 = a6*mass6*t5*t7*t15*t28;
- float t251 = a6*mass6*t6*t7*t14*t28;
- float t252 = d7*mass6*t5*t15*t16*t28;
- float t253 = d7*mass6*t6*t14*t16*t28;
- float t254 = d7*mass6*t7*t14*t15*t28;
- float t255 = a6*mass5*t14*t15*t16*t28;
- float t256 = a6*mass6*t14*t15*t16*t28;
- float t268 = mpy6*t5*t6*t7*t9*t28;
- float t269 = mpy6*t5*t6*t7*t17*t18*t25;
- float t270 = mpy6*t5*t9*t15*t16*t28;
- float t271 = mpy6*t6*t9*t14*t16*t28;
- float t272 = mpy6*t7*t9*t14*t15*t28;
- float t273 = mpy6*t5*t15*t16*t17*t18*t25;
- float t274 = mpy6*t6*t14*t16*t17*t18*t25;
- float t275 = mpy6*t7*t14*t15*t17*t18*t25;
- float t276 = mpy6*t5*t6*t16*t17*t18*t26;
- float t277 = mpy6*t5*t7*t15*t17*t18*t26;
- float t278 = mpy6*t6*t7*t14*t17*t18*t26;
- float t279 = mpy6*t5*t6*t16*t17*t18*t27;
- float t280 = mpy6*t5*t7*t15*t17*t18*t27;
- float t281 = mpy6*t6*t7*t14*t17*t18*t27;
- float t282 = mpy6*t14*t15*t16*t17*t18*t26;
- float t283 = mpy6*t14*t15*t16*t17*t18*t27;
- float t290 = mpy6*t5*t6*t16*t17*t18*t28;
- float t291 = mpy6*t5*t7*t15*t17*t18*t28;
- float t292 = mpy6*t6*t7*t14*t17*t18*t28;
- float t293 = mpy6*t14*t15*t16*t17*t18*t28;
- float t294 = t24+t25;
- float t264 = t21+t22+t23;
- float t298 = t5*t294;
- float t299 = t14*t294;
- float t301 = t26+t27+t33;
- float t300 = -t298;
- float t302 = t5*t301;
- float t303 = t14*t301;
- float t304 = t299+t302;
- float t305 = t300+t303;
- float t308 = -t6*(t298-t303);
- float t309 = -t15*(t298-t303);
- float t306 = t6*t304;
- float t307 = t15*t304;
- float t310 = -t307;
- float t311 = t306+t309;
- float t314 = -t16*(t307+t6*(t298-t303));
- float t312 = t308+t310;
- float t313 = t7*t311;
- float t315 = t313+t314;
- A0(0) = t10*t11*(mass1+mass2+mass3+mass4+mass5+mass6+massr)*9.814;
- 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;
- 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;
- 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;
- 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;
- 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;
- A0(6) = mpy6*t18*(t17*t264*9.814-t8*t315);
- A0(7) = -mpy6*(t18*(t7*(t307+t6*(t298-t303))+t16*t311)+t9*(t8*t264*9.814+t17*t315));
-
-return A0;
-
-}
-
-Vector6f ARAP180_WITH_ROVER::wrenchGr(Vector8f q, Vector3f droneOrientation){
-
- Vector6f A0 = Vector6f::Zero();
- 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
- volatile float q2 = q[1];
- volatile float q3 = q[2];
- volatile float q4 = q[3];
- volatile float q5 = q[4];
- volatile float q6 = q[5];
- volatile float q7 = q[6];
- volatile float q8 = q[7];
-
- float r_b_d = -droneOrientation[0];
- float p_b_d = -droneOrientation[1];
-
- float t2 = cos(p_b_d);
- float t3 = cos(q2);
- float t4 = cos(q3);
- float t5 = cos(q4);
- float t6 = cos(q5);
- float t7 = cos(q6);
- float t8 = cos(q7);
- float t9 = cos(q8);
- float t10 = cos(r_b_d);
- float t11 = sin(p_b_d);
- float t12 = sin(q2);
- float t13 = sin(q3);
- float t14 = sin(q4);
- float t15 = sin(q5);
- float t16 = sin(q6);
- float t17 = sin(q7);
- float t18 = sin(q8);
- float t19 = sin(r_b_d);
- float t20 = mass1+mass2+mass3+mass4+mass5+mass6+massr;
- A0(0) = t10*t11*t20*9.814;
- A0(1) = t19*t20*(-9.814);
- A0(2) = t2*t10*t20*9.814;
- 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;
- 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);
- 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;
-
-return A0;
-}
-
-Vector6f ARAP180_WITH_ROVER::wrenchBattery(float battery_position, Vector3f droneOrientation){
-
- Vector6f A0 = Vector6f::Zero();
-
- float r_b_w = -droneOrientation[0];
- float p_b_w = -droneOrientation[1];
- float q1 = battery_position;
-
- float t2 = cos(p_b_w);
- float t3 = cos(r_b_w);
- float t4 = sin(p_b_w);
- float t5 = sin(r_b_w);
- A0(0) = mass_battery*t3*t4*9.814;
- A0(1) = mass_battery*t5*(-9.814);
- A0(2) = mass_battery*t2*t3*9.814;
- A0(3) = mpx_battery*t5*9.814;
- A0(4) = mpx_battery*t3*t4*9.814+mpz_battery*t2*t3*9.814+mass_battery*q1*t2*t3*9.814;
- A0(5) = mpz_battery*t5*9.814+mass_battery*q1*t5*9.814;
-
- return A0;
-}
-
-float ARAP180_WITH_ROVER::batteryPos_from_wrench(Vector6f desired_wrench, Vector3f droneOrientation){
-
- float A0 = 0.0;
-
- float w5 = desired_wrench[4];
- float w6 = desired_wrench[5];
-
- float r_b_w = -droneOrientation[0];
- float p_b_w = -droneOrientation[1];
- float t2 = cos(p_b_w);
- float t3 = cos(r_b_w);
- float t4 = sin(r_b_w);
- float t5 = t2*t2;
- float t6 = t3*t3;
- 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));
- return A0;
-}
-
-// Inertia matrix B
-
-
-
-
--- a/Robots/ARM/ARAP180_with_rover.h Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,156 +0,0 @@
-/*This file is part of dvrk-dynamics package.
- * Copyright (C) 2017, Giuseppe Andrea Fontanelli
-
- * Email id : giuseppeandrea.fontanelli@unina.it
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * * Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
-* This code will subscriber integer values from demo_topic_publisher
-*/
-
-#ifndef _ARAP180_WITH_ROVER_H
-#define _ARAP180_WITH_ROVER_H
-#include "mbed.h"
-#include "Core.h"
-#include <Eigen/Dense.h>
-#include "utilities.hpp"
-#include "UARTSerial_half.h"
-#include "UARTSerial_mio.h"
-
-
-#include "MX.h"
-
-#define tool_tx PD_5
-#define tool_rx PD_6
-
-
-using namespace Eigen;
-using namespace std;
-
-typedef Matrix<double, 6, 1> Vector6d;
-typedef Matrix<double, 7, 1> Vector7d;
-typedef Matrix<double, 6, 6> Matrix6d;
-typedef Matrix<float, 5, 1> Vector5f;
-typedef Matrix<float, 6, 1> Vector6f;
-typedef Matrix<float, 7, 1> Vector7f;
-typedef Matrix<float, 8, 1> Vector8f;
-
-typedef Matrix<float, 6, 6> Matrix6f;
-typedef Matrix<float, 5, 5> Matrix5f;
-
-
-class ARAP180_WITH_ROVER
-{
- private:
-
- UARTSerial_half *dxl_port;
- UARTSerial_mio *tool_com;
-
-
- int ID[6];
-
- MX *mx_MotorChain;
-
- Vector8f offsets;
- Vector8f motorSign;
-
- float a3;
- float a4;
- float a5;
- float a5i;
- float a6;
- float d7;
- // drone distances
- float x_0_r2;
- float y_0_r2;
- float z_0_r2;
-
- float massr;
- float mpxr ;
- float mpyr ;
- float mpzr ;
- float mass1;
- float mpx1 ;
- float mpy1 ;
- float mpz1 ;
- float mass2;
- float mpx2 ;
- float mpy2 ;
- float mpz2 ;
- float mass3;
- float mpx3 ;
- float mpy3 ;
- float mpz3 ;
- float mass4;
- float mpx4 ;
- float mpy4 ;
- float mpz4 ;
- float mass5;
- float mpx5 ;
- float mpy5 ;
- float mpz5 ;
- float mass6;
- float mpx6 ;
- float mpy6 ;
- float mpz6 ;
-
- float mass_battery;
- float mpx_battery;
- float mpz_battery;
-
- int extendedSelect[6];
- float toolGearRatio;
-
-
- public:
-
- /* brief Constructor. */
- ARAP180_WITH_ROVER();
- VectorXf get_parameters();
-
- Matrix<float,6,8> jacobianMatrix(Vector8f q); //Jacobian matrix
- Matrix<float,6,8> jacobianTimeDerivativeMatrix(Vector8f q, Vector8f dq); //Jacobian matrix
-
- Matrix4f forwardKinematics(Vector8f q); //Dyrect kinematics
-
- Vector6f wrenchGr(Vector8f q, Vector3f droneOrientation);
- float batteryPos_from_wrench(Vector6f desired_wrench, Vector3f droneOrientation);
- Vector6f wrenchBattery(float battery_position, Vector3f droneOrientation);
- Vector6f Gr(Vector8f q, Vector3f droneOrientation);
-
-
- void setOperatingMode(int modeArm, int modeTool); //mode = 3 for position control
- void enableMotors(int enable);
-
- Vector6f getJointPos(bool checkData);
- void setJointPos(Vector8f q);
-
- void setMaxVel(float maxVel);
- void setMaxAcc(float maxAcc);
-
- void setParameters(VectorXd armParameters);
- void printParameters();
-
-};
-
-#endif // _ARMHyfliers_H
-
--- a/Robots/ARM/ARM_parameters.h Wed Sep 29 11:20:56 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,41 +0,0 @@ -//Kinematic param -/*#define a3 0.029 -#define a4 0.340 -#define a5 0.029 -#define a5i 0.335 -#define a6 0.084 -#define d7 -0.218 - -#define x_0_r2 0.0 -#define y_0_r2 -0.3 -#define z_0_r2 0.36*/ - -//Dynamic param -/*#define massr 6.0 -#define mpxr 0 -#define mpyr -1.2 -#define mpzr 0 -#define mass1 0.2 -#define mpx1 0.2 -#define mpy1 0 -#define mpz1 0 -#define mass2 0.2 -#define mpx2 0.2 -#define mpy2 0 -#define mpz2 0 -#define mass3 0.15 -#define mpx3 0.1 -#define mpy3 0 -#define mpz3 0 -#define mass4 0.1 -#define mpx4 0.1 -#define mpy4 0 -#define mpz4 0 -#define mass5 0.1 -#define mpx5 0.0 -#define mpy5 0 -#define mpz5 0.1 -#define mass6 0.2 -#define mpx6 0 -#define mpy6 0 -#define mpz6 0.1*/
--- a/Robots/ARM/ArmControl/FollowFilter.cpp Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,113 +0,0 @@
-#include "FollowFilter.h"
-
-FollowFilter::FollowFilter(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos){
- omega = _omega;
- zita = _zita;
- maxJerk = _maxJerk;
- maxAcc = _maxAcc;
- maxVel = _maxVel;
- maxPos = _maxPos;
- minPos = _minPos;
- dq_filtered = VectorXf::Zero( _omega.size(),1);
- ddq_filtered = VectorXf::Zero( _omega.size(),1);
- q_filtered = VectorXf::Zero( _omega.size(),1);
-}
-
-void FollowFilter::setFollowFilterParameters(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos){
-
- omega = _omega;
- zita = _zita;
- maxJerk = _maxJerk;
- maxAcc = _maxAcc;
- maxVel = _maxVel;
- maxPos = _maxPos;
- minPos = _minPos;
-
-
-}
-
-void FollowFilter::initFollowFilter(VectorXf _first_q){
-
- first_q = _first_q;
- dq_filtered = VectorXf::Zero( _first_q.size(),1);
- ddq_filtered = VectorXf::Zero( _first_q.size(),1);
- q_filtered = VectorXf::Zero( _first_q.size(),1);
- q_filtered = first_q;
-
-
-}
-
-void FollowFilter::getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd){
- q_cmd.resize(q_filtered.size(),1);
- dq_cmd.resize(q_filtered.size(),1);
- ddq_cmd.resize(q_filtered.size(),1);
- q_cmd = q_filtered;
- dq_cmd = dq_filtered;
- ddq_cmd = ddq_filtered;
-
-}
-
-
-void FollowFilter::updateFollowFilter(VectorXf q_in, float dT ) {
-
- float ddq = 0.0;
- float dq = 0.0;
- float posError = 0.0;
- float jerk = 0.0;
-
- for(int j = 0; j<q_in.size(); j++){
- posError = q_in(j) - q_filtered(j);
-
- ddq = omega(j) * omega(j) * posError - 2.0 * zita(j) * omega(j) * dq_filtered(j);
-
- //Jerk
- /*jerk = (ddq - ddq_filtered(j))/dT;
-
- //---jerk saturation
- if( fabs( jerk ) > maxJerk(j) ) {
- if( jerk > 0.0 )
- jerk = (maxJerk(j) ); // / jerk[j_id] );
- else
- jerk = -(maxJerk(j) ); // / jerk[j_id] );
- }
- //---
-
- //acceleration
- ddq = ddq_filtered(j) + jerk*dT;*/
-
- //---acc saturation
- if( fabs( ddq ) > maxAcc(j) ) {
- if( ddq > 0.0 )
- ddq_filtered(j) = maxAcc(j); // / ddq[j_id];
- else
- ddq_filtered(j) = -maxAcc(j); // / ddq[j_id];
- }else {
- ddq_filtered(j) = ddq;
- }
- //---
-
- //velocity
- dq = dq_filtered(j) + ddq_filtered(j) * dT;
-
- //---veloticyt saturation
- if( fabs( dq ) > maxVel(j) ) {
- if( dq > 0.0 )
- dq_filtered(j) = maxVel(j);
- else
- dq_filtered(j) = -maxVel(j);
- }else {
- dq_filtered(j) = dq;
- }
- //---
-
- //Aggiornamento posizione
- q_filtered(j) += dq_filtered(j) * dT;
-
- if(q_filtered(j) > maxPos(j)){ q_filtered(j) = maxPos(j);}
- if(q_filtered(j) < minPos(j)){ q_filtered(j) = minPos(j);}
-
-
-
- }
-
-}
--- a/Robots/ARM/ArmControl/FollowFilter.h Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-#ifndef FOLLOW_FILTER_H
-#define FOLLOW_FILTER_H
-
-#include "mbed.h"
-#include <Eigen/Dense.h>
-
-using namespace Eigen;
-
-class FollowFilter
-{
-public:
- FollowFilter(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos);
- void setFollowFilterParameters(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos);
- void initFollowFilter(VectorXf _first_q);
- void updateFollowFilter(VectorXf q_in, float deltaT);
- void getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd);
-
-private:
-
- VectorXf q_filtered;
- VectorXf dq_filtered;
- VectorXf ddq_filtered;
- VectorXf first_q;
- VectorXf omega;
- VectorXf zita;
- VectorXf maxJerk;
- VectorXf maxAcc;
- VectorXf maxVel;
- VectorXf maxPos;
- VectorXf minPos;
-
-};
-
-#endif
--- a/Robots/ARM/ArmControl/InverseKinematicsControl.cpp Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,267 +0,0 @@
-#include "InverseKinematicsControl.h"
-
-InverseKinematicsControl::InverseKinematicsControl(ARAP180_WITH_ROVER *_arm ,Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError){
-
- arm = _arm;
- maxAcc = _maxAcc;
- maxVel = _maxVel;
- maxPos = _maxPos;
- minPos = _minPos;
-
- K_first = _K_first;
- K_second = _K_second;
-
- D = _D;
-
- n_max_iter = _n_max_iter;
- max_pError = _max_pError;
- max_oError = _max_oError;
-
- q = Vector8f::Zero();
- dq = Vector8f::Zero();
- ddq = Vector8f::Zero();
-
- ddx = Vector6f::Zero();
-
- Te = Matrix4f::Identity();
-
- J = Matrix<float,6,8>::Zero();
- pinvJ = Matrix<float,8,6>::Zero();
- J_T = Matrix<float,8,6>::Zero();
- L = Matrix3f::Zero();
- pinvL = Matrix3f::Zero();
-
- Jp = Matrix<float,6,8>::Zero();
-
- Vector8f W_vec;
- W_vec << 10,1000,1,1,1,1,1,1;
-
- W = W_vec.cast<float>().asDiagonal();
-}
-
-void InverseKinematicsControl::setInverseKinematicsControlParameters(Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f WeightVector, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError){
-
- maxAcc = _maxAcc;
- maxVel = _maxVel;
- maxPos = _maxPos;
- minPos = _minPos;
-
- K_first = _K_first;
- K_second = _K_second;
-
- n_max_iter = _n_max_iter;
- max_pError = _max_pError;
- max_oError = _max_oError;
-
- W = WeightVector.cast<float>().asDiagonal();
-
-}
-
-
-void InverseKinematicsControl::initInverseKinematicsControl(Vector8f _first_q){
-
- q = _first_q;
- dq = Vector8f::Zero();
- ddq = Vector8f::Zero();
- ddx = Vector6f::Zero();
-
-}
-
-void InverseKinematicsControl::getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd){
- q_cmd.resize(q.size(),1);
- dq_cmd.resize(q.size(),1);
- ddq_cmd.resize(q.size(),1);
- q_cmd = q;
- dq_cmd = dq;
- ddq_cmd = ddq;
-
-}
-
-void InverseKinematicsControl::getDebugVector(Matrix<float, 24, 1> &_debugVector){
- _debugVector = debugVector;
-
-}
-
-
-
-void InverseKinematicsControl::updateInverseKinematicsControl_firstOrder(Matrix4f Td, Vector6f dx_d, Vector8f dq_null_space, float deltaT) {
-
- Matrix3f Kp = Matrix3f::Zero();
- Matrix3f Ko = Matrix3f::Zero();
- Kp = K_first.block(0,0,3,1).cast<float>().asDiagonal();
- Ko = K_first.block(3,0,3,1).cast<float>().asDiagonal();
-
- //Matrix6f W_inv = Matrix6f::Identity();
-
- Vector3f dxp = Vector3f::Zero();
- Vector3f dxo = Vector3f::Zero();
- dx = Vector6f::Zero();
-
- int n_iter = 0;
-
- float pError = 0.0;
- float oError = 0.0;
-
- Te = arm->forwardKinematics(q);
- J = arm->jacobianMatrix(q);
-
- ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);
- eo = utilities::rotationMatrixError(Td, Te);
-
- pError = ep.norm(); //L_2 norm of vector
- oError = eo.norm();
-
- //while(n_iter<n_max_iter && (pError>max_pError || oError>max_oError)){
-
- L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3));
-
- pinvL = L.inverse();
-
- dxp = dx_d.block(0,0,3,1) + Kp*ep;
-
- dxo = pinvL*(L.transpose()*dx_d.block(3,0,3,1) + Ko*eo);
-
- dx << dxp[0], dxp[1], dxp[2], dxo[0], dxo[1], dxo[2];
-
- J_T = J.transpose();
-
- // Pseudo inversa a destra perchè ho più giunti che gradi di libertà
- FullPivLU<Matrix6f> Core_J(J*W.inverse()*J_T);
- pinvJ = W.inverse()*J_T*Core_J.inverse();
-
- //pinvJ = J.inverse();
-
- dq = pinvJ*dx + (Matrix<float,8,8>::Identity() - pinvJ*J)*dq_null_space;
-
- /*for(int j = 0; j<dq.size(); j++){
- if(dq(j) > maxVel(j)){ dq(j) = maxVel(j);}
- if(dq(j) < -maxVel(j)){ dq(j) = -maxVel(j);}
- }*/
-
-
- q += dq*deltaT;
-
- for(int j = 0; j<q.size(); j++){
- if(q(j) > maxPos(j)){ q(j) = maxPos(j);}
- if(q(j) < minPos(j)){ q(j) = minPos(j);}
- }
-
-
- /*Te = arm->forwardKinematics(q);
- J = arm->jacobianMatrix(q);
-
- ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);
- eo = utilities::rotationMatrixError(Td, Te);
-
- pError = ep.norm(); //L_2 norm of vector
- oError = eo.norm();
-
- debugVector.block(0,0,6,1) << ep[0], ep[1], ep[2], eo[0], eo[1], eo[2];
- debugVector.block(6,0,6,1) << de;
- //debugVector.block(12,0,6,1) << dq;
- //debugVector.block(18,0,6,1) << ddq;
-
- n_iter++;*/
-
- //}
-
-}
-
-
-//SECOND ORDER CLICK
-
-void InverseKinematicsControl::updateInverseKinematicsControl_secondOrder(Matrix4f Td, Vector6f dx_d, Vector6f ddx_d, float deltaT) {
-
-
- Matrix6f K_mat = Matrix6f::Zero();
- Matrix6f D_mat = Matrix6f::Zero();
- K_mat = K_second.cast<float>().asDiagonal();
- D_mat = D.cast<float>().asDiagonal();
-
- int n_iter = 0;
-
- float pError = 0.0;
- float oError = 0.0;
-
- Te = arm->forwardKinematics(q);
- J = arm->jacobianMatrix(q);
- L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3));
-
- ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);
- eo = utilities::rotationMatrixError(Td, Te);
-
- dep = dx_d.block(0,0,3,1) - J.block(0,0,3,8)*dq; //dq deve entrare
- deo = L.transpose()*dx_d.block(3,0,3,1) - L*J.block(3,0,3,8)*dq; //
- de << dep[0], dep[1], dep[2], deo[0], deo[1], deo[2];
- e << ep[0], ep[1], ep[2], eo[0], eo[1], eo[2];
-
- pError = ep.norm(); //L_2 norm of vector
- oError = eo.norm();
- Matrix<float,6,8> J_dot;
-
- while(n_iter<n_max_iter && (pError>max_pError || oError>max_oError)){
-
-
- J_dot = (J - Jp)/deltaT;
- Jp = J;
-
- ddx = ddx_d + D_mat*de + K_mat*e - J_dot*dq; //arm->jacobianTimeDerivativeMatrix(q, dq)*dq
-
- J_T = J.transpose();
-
- FullPivLU<Matrix6f> Core_J(J*W.inverse()*J_T);
- pinvJ = W.inverse()*J_T*Core_J.inverse();
-
-
- //pinvJ = J.inverse();
-
- ddq = pinvJ*ddx;
-
-
- /*for(int j = 0; j<ddq.size(); j++){
- if(ddq(j) > maxAcc(j)){ ddq(j) = maxAcc(j);}
- if(ddq(j) < -maxAcc(j)){ ddq(j) = -maxAcc(j);}
- }*/
-
- dq += ddq*deltaT;
-
- /*for(int j = 0; j<dq.size(); j++){
- if(dq(j) > maxVel(j)){ dq(j) = maxVel(j);}
- if(dq(j) < -maxVel(j)){ dq(j) = -maxVel(j);}
- }*/
-
- q += dq*deltaT;
-
- for(int j = 0; j<q.size(); j++){
- if(q(j) > maxPos(j)){ q(j) = maxPos(j);}
- if(q(j) < minPos(j)){ q(j) = minPos(j);}
- }
-
-
- Te = arm->forwardKinematics(q);
- J = arm->jacobianMatrix(q);
- L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3));
-
-
- ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);
- eo = utilities::rotationMatrixError(Td, Te);
-
- dep = dx_d.block(0,0,3,1) - J.block(0,0,3,8)*dq; //dq deve entrare
- deo = L.transpose()*dx_d.block(3,0,3,1) - L*J.block(3,0,3,8)*dq; //
- de << dep[0], dep[1], dep[2], deo[0], deo[1], deo[2];
- e << ep[0], ep[1], ep[2], eo[0], eo[1], eo[2];
-
- pError = ep.norm(); //L_2 norm of vector
- oError = eo.norm();
-
- debugVector.block(0,0,6,1) << e;
- debugVector.block(6,0,6,1) << de;
- //debugVector.block(12,0,6,1) << dq;
- //debugVector.block(18,0,6,1) << ddq;
-
- n_iter++;
-
- }
-
-}
-
--- a/Robots/ARM/ArmControl/InverseKinematicsControl.h Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,80 +0,0 @@
-#ifndef INVERSE_KINEMATICS_CONTROL_H
-#define INVERSE_KINEMATICS_CONTROL_H
-
-#include "mbed.h"
-#include <Eigen/Dense.h>
-#include "utilities.hpp"
-#include "ARAP180_with_rover.h"
-
-using namespace Eigen;
-
-class InverseKinematicsControl
-{
-public:
- InverseKinematicsControl(ARAP180_WITH_ROVER *_arm ,Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError);
- void setInverseKinematicsControlParameters(Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f WeightVector, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError);
- void initInverseKinematicsControl(Vector8f _first_q);
-
- void updateInverseKinematicsControl_firstOrder(Matrix4f Td, Vector6f dx_d, Vector8f dq_null_space, float deltaT);
- void updateInverseKinematicsControl_secondOrder(Matrix4f Td, Vector6f dx_d, Vector6f ddx_d, float deltaT);
-
- void getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd);
- void getDebugVector(Matrix<float, 24, 1> &_debugVector);
-
-
-private:
-
- ARAP180_WITH_ROVER *arm;
-
- Vector8f q;
- Vector8f dq;
- Vector8f ddq;
- Vector6f h_s_s;
- Vector6f h_e_b;
-
- Vector6f ddx;
- Vector6f dx;
-
- Vector3f ep;
- Vector3f eo;
-
- Vector3f dep;
- Vector3f deo;
- Vector6f de;
- Vector6f e;
-
- Vector8f maxAcc;
- Vector8f maxVel;
- Vector8f maxPos;
- Vector8f minPos;
-
- Vector6f K_first;
- Vector6f K_second;
-
- Vector6f D;
-
- int n_max_iter;
- float max_pError;
- float max_oError;
-
- Matrix4f Te;
- Matrix4f T_5_b;
- Matrix6f Q_s_b;
- Matrix6f Q_b_s;
-
- Matrix<float,6,8> J;
- Matrix<float,8,6> pinvJ;
- Matrix<float,8,6> J_T;
-
- Matrix<float,8,8> W;
- Matrix3f L;
- Matrix3f pinvL;
-
- Matrix<float, 24, 1> debugVector;
-
- Matrix<float,6,8> Jp;
-
-
-};
-
-#endif
--- a/Robots/ARM/ArmControl/SafeCheck.cpp Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,56 +0,0 @@
-#include "SafeCheck.h"
-
-SafeCheck::SafeCheck(ARAP180_WITH_ROVER *_arm, VectorXf _maxJointPos,VectorXf _minJointPos, VectorXf _maxJointVel, VectorXf _maxJointAcc, Vector3f _maxCartPos, Vector3f _minCartPos){
-
- arm = _arm;
- maxJointPos = _maxJointPos;
- minJointPos = _minJointPos;
- maxJointVel = _maxJointVel;
- maxJointAcc = _maxJointAcc;
- maxCartPos = _maxCartPos;
- minCartPos = _minCartPos;
-}
-
-void SafeCheck::setSafeCheckParameters(VectorXf _maxJointPos,VectorXf _minJointPos, VectorXf _maxJointVel, VectorXf _maxJointAcc, Vector3f _maxCartPos, Vector3f _minCartPos){
-
- maxJointPos = _maxJointPos;
- minJointPos = _minJointPos;
- maxJointVel = _maxJointVel;
- maxJointAcc = _maxJointAcc;
- maxCartPos = _maxCartPos;
- minCartPos = _minCartPos;
-}
-
-void SafeCheck::initSafeCheck(){
- isSafe = true;
-}
-
-bool SafeCheck::check(VectorXf q, VectorXf dq, VectorXf ddq, checkMode mode){
-
-
- Matrix4f Te;
- Vector3f cartPos;
-
- for(int i = 0; i < q.size(); i++){
-
- if(q(i) > maxJointPos(i) || q(i) < minJointPos(i)) isSafe = false;
- if(fabs(dq(i)) > maxJointVel(i)) isSafe = false;
- if(fabs(ddq(i)) > maxJointAcc(i)) isSafe = false;
-
- }
-
-
- if(mode == CARTESIAN_MODE){
- Te = arm->forwardKinematics(q);
- cartPos = Te.block(0,3,3,1);
-
- for(int i = 0; i < cartPos.size(); i++){
- if(cartPos(i) > maxCartPos(i) || cartPos(i) < minCartPos(i)) isSafe = false;
- }
- }
-
- return isSafe;
-
-
-}
-
--- a/Robots/ARM/ArmControl/SafeCheck.h Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,41 +0,0 @@
-#ifndef SAFE_CHECK_H
-#define SAFE_CHECK_H
-
-#include "mbed.h"
-#include <Eigen/Dense.h>
-#include "ARAP180_with_rover.h"
-
-using namespace Eigen;
-
-class SafeCheck
-{
-public:
-
- enum checkMode {
- JOINT_MODE,
- CARTESIAN_MODE,
- };
-
- SafeCheck(ARAP180_WITH_ROVER *_arm, VectorXf _maxJointPos,VectorXf _minJointPos, VectorXf _maxJointVel, VectorXf _maxJointAcc, Vector3f _maxCartPos, Vector3f _minCartPos);
- void setSafeCheckParameters(VectorXf _maxJointPos,VectorXf _minJointPos, VectorXf _maxJointVel, VectorXf _maxJointAcc, Vector3f _maxCartPos, Vector3f _minCartPos);
- void initSafeCheck();
- bool check(VectorXf q, VectorXf dq, VectorXf ddq, checkMode mode);
-
-
-
-
-private:
- ARAP180_WITH_ROVER *arm;
-
- VectorXf maxJointPos;
- VectorXf minJointPos;
- VectorXf maxJointVel;
- VectorXf maxJointAcc;
- Vector3f maxCartPos;
- Vector3f minCartPos;
-
- bool isSafe;
-
-};
-
-#endif
--- a/Robots/ARM/MX64.lib Wed Sep 29 11:20:56 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/teams/PRISMA-Lab/code/MX64/#a2ed80abe394
--- a/Robots/ARM/utilities.hpp Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,382 +0,0 @@
-#ifndef _utilities_hpp
-#define _utilities_hpp
-
-
-#include "Core.h"
-#ifndef M_PI
-#define M_PI 3.14159265358979
-#endif
-
-using namespace std; //calling the standard directory
-using namespace Eigen;
-
-
-
-
-namespace utilities{
-
- inline Matrix3f rotx(float alpha){
- Matrix3f Rx;
- Rx << 1,0,0,
- 0,cos(alpha),-sin(alpha),
- 0,sin(alpha), cos(alpha);
- return Rx;
- }
-
- inline Matrix3f roty(float beta){
- Matrix3f Ry;
- Ry << cos(beta),0,sin(beta),
- 0,1,0,
- -sin(beta),0, cos(beta);
- return Ry;
- }
-
- inline Matrix3f rotz(float gamma){
- Matrix3f Rz;
- Rz << cos(gamma),-sin(gamma),0,
- sin(gamma),cos(gamma),0,
- 0,0, 1;
- return Rz;
- }
-
-
- inline Matrix4f rotx_T(float alpha){
- Matrix4f Tx = Matrix4f::Identity();
- Tx.block(0,0,3,3) = rotx(alpha);
- return Tx;
- }
-
- inline Matrix4f roty_T(float beta){
- Matrix4f Ty = Matrix4f::Identity();
- Ty.block(0,0,3,3) = roty(beta);
- return Ty;
- }
-
- inline Matrix4f rotz_T(float gamma){
- Matrix4f Tz = Matrix4f::Identity();
- Tz.block(0,0,3,3) = rotz(gamma);
- return Tz;
- }
-
- inline Matrix3f skew(Vector3f v)
- {
- Matrix3f S;
- S << 0, -v[2], v[1], //Skew-symmetric matrix
- v[2], 0, -v[0],
- -v[1], v[0], 0;
- return S;
- }
-
-
- inline Matrix3f L_matrix(Matrix3f R_d, Matrix3f R_e)
- {
- Matrix3f L = -0.5 * (skew(R_d.col(0))*skew(R_e.col(0)) + skew(R_d.col(1))*skew(R_e.col(1)) + skew(R_d.col(2))*skew(R_e.col(2)));
- return L;
- }
-
-
-
- inline Vector3f rotationMatrixError(Matrix4f Td, Matrix4f Te)
- {
-
- Matrix3f R_e = Te.block(0,0,3,3); //Matrix.slice<RowStart, ColStart, NumRows, NumCols>();
- Matrix3f R_d = Td.block(0,0,3,3);
-
- Vector3f eo = 0.5 * (skew(R_e.col(0))*R_d.col(0) + skew(R_e.col(1))*R_d.col(1) + skew(R_e.col(2))*R_d.col(2)) ;
- return eo;
- }
-
-
- inline Vector3f r2quat(Matrix3f R_iniz, float &eta)
- {
- Vector3f epsilon;
- int iu, iv, iw;
-
- if ( (R_iniz(0,0) >= R_iniz(1,1)) && (R_iniz(0,0) >= R_iniz(2,2)) )
- {
- iu = 0; iv = 1; iw = 2;
- }
- else if ( (R_iniz(1,1) >= R_iniz(0,0)) && (R_iniz(1,1) >= R_iniz(2,2)) )
- {
- iu = 1; iv = 2; iw = 0;
- }
- else
- {
- iu = 2; iv = 0; iw = 1;
- }
-
- float r = sqrt(1 + R_iniz(iu,iu) - R_iniz(iv,iv) - R_iniz(iw,iw));
- Vector3f q;
- q << 0,0,0;
- if (r>0)
- {
- float rr = 2*r;
- eta = (R_iniz(iw,iv)-R_iniz(iv,iw)/rr);
- epsilon[iu] = r/2;
- epsilon[iv] = (R_iniz(iu,iv)+R_iniz(iv,iu))/rr;
- epsilon[iw] = (R_iniz(iw,iu)+R_iniz(iu,iw))/rr;
- }
- else
- {
- eta = 1;
- epsilon << 0,0,0;
- }
- return epsilon;
- }
-
-
- inline Vector4f rot2quat(Matrix3f R){
-
- float m00, m01, m02, m10, m11, m12, m20, m21, m22;
-
- m00 = R(0,0);
- m01 = R(0,1);
- m02 = R(0,2);
- m10 = R(1,0);
- m11 = R(1,1);
- m12 = R(1,2);
- m20 = R(2,0);
- m21 = R(2,1);
- m22 = R(2,2);
-
- float tr = m00 + m11 + m22;
- float qw, qx, qy, qz, S;
- Vector4f quat;
-
- if (tr > 0) {
- S = sqrt(tr+1.0) * 2; // S=4*qw
- qw = 0.25 * S;
- qx = (m21 - m12) / S;
- qy = (m02 - m20) / S;
- qz = (m10 - m01) / S;
- } else if ((m00 > m11)&(m00 > m22)) {
- S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
- qw = (m21 - m12) / S;
- qx = 0.25 * S;
- qy = (m01 + m10) / S;
- qz = (m02 + m20) / S;
- } else if (m11 > m22) {
- S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
- qw = (m02 - m20) / S;
- qx = (m01 + m10) / S;
- qy = 0.25 * S;
- qz = (m12 + m21) / S;
- } else {
- S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
- qw = (m10 - m01) / S;
- qx = (m02 + m20) / S;
- qy = (m12 + m21) / S;
- qz = 0.25 * S;
- }
-
- quat << qw, qx, qy, qz;
- return quat;
-
- }
-
-
- //Matrix ortonormalization
- inline Matrix3f matrixOrthonormalization(Matrix3f R){
-
- SelfAdjointEigenSolver<Matrix3f> es(R.transpose()*R);
- Vector3f D = es.eigenvalues();
- Matrix3f V = es.eigenvectors();
- R = R*((1/sqrt(D(0)))*V.col(0)*V.col(0).transpose() + (1/sqrt(D(1)))*V.col(1)*V.col(1).transpose() + (1/sqrt(D(2)))*V.col(2)*V.col(2).transpose());
-
- return R;
- }
-
-
-
-
- //******************************************************************************
- inline Vector3f quaternionError(Matrix4f Tbe, Matrix4f Tbe_d)
- {
- float eta, eta_d;
- Matrix3f R = Tbe.block(0,0,3,3); //Matrix.slice<RowStart, ColStart, NumRows, NumCols>();
- Vector3f epsilon = r2quat(R, eta);
- Matrix3f R_d = Tbe_d.block(0,0,3,3);
- Vector3f epsilon_d = r2quat(R_d, eta_d);
- Matrix3f S = skew(epsilon_d);
- Vector3f eo = eta*epsilon_d-eta_d*epsilon-S*epsilon;
- return eo;
- }
-
-
- inline Vector3f versorError(Vector3f P_d, Matrix3f Tbe_e, float &theta)
- {
- Vector3f P_e = Tbe_e.block(0,3,3,1);
- Vector3f u_e = Tbe_e.block(0,0,3,1);
-
- Vector3f u_d = P_d - P_e;
- u_d = u_d/ u_d.norm();
-
- Vector3f r = skew(u_e)*u_d;
- float nr = r.norm();
-
- if (nr >0){
- r = r/r.norm();
-
-
- //be carefult to acos( > 1 )
- float u_e_d = u_e.transpose()*u_d;
- if( fabs(u_e_d) <= 1.0 ) {
- theta = acos(u_e.transpose()*u_d);
- }
- else {
- theta = 0.0;
- }
-
- Vector3f error = r*sin(theta);
- return error;
- }else{
- theta = 0.0;
- return Vector3f::Zero();
- }
- }
-
-
- /*Matrix3f utilities::rotation ( float theta, Vector3f r ) {
-
- Matrix3f R = Zeros;
-
- R[0][0] = r[0]*r[0]*(1-cos(theta)) + cos(theta);
- R[1][1] = r[1]*r[1]*(1-cos(theta)) + cos(theta);
- R[2][2] = r[2]*r[2]*(1-cos(theta)) + cos(theta);
-
- R[0][1] = r[0]*r[1]*(1-cos(theta)) - r[2]*sin(theta);
- R[1][0] = r[0]*r[1]*(1-cos(theta)) + r[2]*sin(theta);
-
- R[0][2] = r[0]*r[2]*(1-cos(theta)) + r[1]*sin(theta);
- R[2][0] = r[0]*r[2]*(1-cos(theta)) - r[1]*sin(theta);
-
- R[1][2] = r[1]*r[2]*(1-cos(theta)) - r[0]*sin(theta);
- R[2][1] = r[1]*r[2]*(1-cos(theta)) + r[0]*sin(theta);
-
-
- return R;
- }*/
-
-
- inline Matrix3f XYZ2R(Vector3f angles) {
-
- Matrix3f R = Matrix3f::Zero();
- Matrix3f R1 = Matrix3f::Zero();
- Matrix3f R2 = Matrix3f::Zero();
- Matrix3f R3 = Matrix3f::Zero();
-
- float cos_phi = cos(angles[0]);
- float sin_phi = sin(angles[0]);
- float cos_theta = cos(angles[1]);
- float sin_theta = sin(angles[1]);
- float cos_psi = cos(angles[2]);
- float sin_psi = sin(angles[2]);
-
- R1 << 1, 0 , 0,
- 0, cos_phi, -sin_phi,
- 0, sin_phi, cos_phi;
-
- R2 << cos_theta , 0, sin_theta,
- 0 , 1, 0 ,
- -sin_theta, 0, cos_theta;
-
- R3 << cos_psi, -sin_psi, 0,
- sin_psi, cos_psi , 0,
- 0 , 0 , 1;
-
- R = R1*R2*R3;
-
- return R;
- }
-
- // This method computes the XYZ Euler angles from the Rotational matrix R.
- inline Vector3f R2XYZ(Matrix3f R) {
- double phi=0.0, theta=0.0, psi=0.0;
- Vector3f XYZ = Vector3f::Zero();
-
- theta = asin(R(0,2));
-
- if(fabsf(cos(theta))>pow(10.0,-10.0))
- {
- phi=atan2(-R(1,2)/cos(theta), R(2,2)/cos(theta));
- psi=atan2(-R(0,1)/cos(theta), R(0,0)/cos(theta));
- }
- else
- {
- if(fabsf(theta-M_PI/2.0)<pow(10.0,-5.0))
- {
- psi = 0.0;
- phi = atan2(R(1,0), R(2,0));
- theta = M_PI/2.0;
- }
- else
- {
- psi = 0.0;
- phi = atan2(-R(1,0), R(2,0));
- theta = -M_PI/2.0;
- }
- }
-
- XYZ << phi,theta,psi;
- return XYZ;
- }
-
- inline Matrix3f angleAxis2Rot(Vector3f ri, float theta){
- Matrix3f R;
- R << ri[0]*ri[0] * (1 - cos(theta)) + cos(theta) , ri[0] * ri[1] * (1 - cos(theta)) - ri[2] * sin(theta) , ri[0] * ri[2] * (1 - cos(theta)) + ri[1] * sin(theta),
- ri[0] * ri[1] * (1 - cos(theta)) + ri[2] * sin(theta) , ri[1]*ri[1] * (1 - cos(theta)) + cos(theta) , ri[1] * ri[2] * (1 - cos(theta)) - ri[0] * sin(theta),
- ri[0] * ri[2] * (1 - cos(theta)) - ri[1] * sin(theta) , ri[1] * ri[2] * (1 - cos(theta)) + ri[0] * sin(theta) , ri[2]*ri[2] * (1 - cos(theta)) + cos(theta);
-
- return R;
-
- }
-
-
-
- inline Vector3f butt_filter(Vector3f x, Vector3f x1, Vector3f x2, float omega_n, float zita, float ctrl_T){
- //applico un filtro di Butterworth del secondo ordine (sfrutto Eulero all'indietro)
- return x1*(2.0 + 2.0*omega_n*zita*ctrl_T)/(omega_n*omega_n*ctrl_T*ctrl_T + 2.0*omega_n*zita*ctrl_T + 1.0) - x2/(omega_n*omega_n*ctrl_T*ctrl_T + 2.0*omega_n*zita*ctrl_T + 1.0) + x*(omega_n*omega_n*ctrl_T*ctrl_T)/(omega_n*omega_n*ctrl_T*ctrl_T + 2.0*omega_n*zita*ctrl_T + 1.0);
- }
-
-
-
- /// converts a rate in Hz to an integer period in ms.
- inline uint16_t rateToPeriod(const float & rate) {
- if (rate > 0)
- return static_cast<uint16_t> (1000.0 / rate);
- else
- return 0;
- }
-
-
-
-
- //Quaternion to rotration Matrix
- inline Matrix3f QuatToMat(Vector4f Quat){
- Matrix3f Rot;
- float s = Quat[0];
- float x = Quat[1];
- float y = Quat[2];
- float z = Quat[3];
- Rot << 1-2*(y*y+z*z),2*(x*y-s*z),2*(x*z+s*y),
- 2*(x*y+s*z),1-2*(x*x+z*z),2*(y*z-s*x),
- 2*(x*z-s*y),2*(y*z+s*x),1-2*(x*x+y*y);
- return Rot;
- }
-
-
- inline float rad2deg(float rad){
- float deg;
- deg = 180.0*rad/M_PI;
- return deg;
- }
-
- inline float deg2rad(float deg){
- float rad;
- rad = M_PI*deg/180.0;
- return rad;
- }
-}
-
-#endif
-
--- a/Robots/Rover/Rover.cpp Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,397 +0,0 @@
-#include "Rover.h"
-#include <iostream>
-
-Rover::Rover(){
-
- pipeDir = 0;
-
- S = Eigen::Matrix<float, 4, 3>::Zero();
-
- ionMcFront = new IONMcMotors(frontBoardAddress,ionMcBoudRate, PB_9, PB_8, frontMotorGearBoxRatio, frontEncoderPulse, frontKt, frontKt);
- ionMcRetro = new IONMcMotors(retroBoardAddress,ionMcBoudRate, PG_14, PG_9, retroMotorGearBoxRatio, retroEncoderPulse, retroKt, retroKt);
-
- frontActuonix = new Servo(PE_11);
- retroActuonix = new Servo(PE_9);
- mpu = new MPU6050(PF_15, PF_14);
-
- tofs = new TOFs();
-
- enableCurv = 1;
- g_x_old = 0.0;
-
-}
-
-void Rover::initializeTofs(){
-
- tofs->TOFs_init();
- ThisThread::sleep_for(1000);
- //tofs->TOFs_offset();
-
-
-}
-
-void Rover::setParameters(Eigen::VectorXd roverParameters){
- r_frontWheel = roverParameters(0);
- r_retroWheel = roverParameters(1);
- pipeCurve_I = roverParameters(2);
- pipeCurve_E = roverParameters(3);
- pipeCurve_M = roverParameters(4);
- retroFrontCentralDistance = roverParameters(5);
- wheelsCntactPointDistanceFromPipeCenter = roverParameters(6);
-
-}
-
-
-float Rover::computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd){
-
- vels_a = -Kp*M_PI*(pitch_d-pitch)/180 -Kd*M_PI*(0-pitch_vel)/180;
- return vels_a;
-
-}
-
-
-void Rover::acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance){
-
- tofs->TOFs_acquireFiltr();
- roverLaserFrontDx = tofs->getLaserFrontDx();
- roverLaserFrontSx = tofs->getLaserFrontSx();
- roverLaserRetroDx = tofs->getLaserRetroDx();
- roverLaserRetroSx = tofs->getLaserRetroSx();
-
- roverFrontDistance = tofs->getFrontDistance();
- roverRetroDistance = tofs->getRetroDistance();
-
-
-}
-
-void Rover::computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx){
-
- float frontDistance;
- float retroDistance;
-
- acquireTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx, roverFrontDistance, roverRetroDistance);
-
- frontDistance = roverFrontDistance;
- retroDistance = roverRetroDistance;
- float frontVel = 0.05*frontDistance;
- float retroVel = -0.04*retroDistance;
-
- if(enableCurv==1){
- frontPos = frontPos + frontVel*0.02;
- retroPos = retroPos + retroVel*0.02;
- }else{
- frontPos = 0.0;
- retroPos = 0.0;
- }
-
- if(frontPos > 0.5) frontPos=0.5;
- if(frontPos < -0.5) frontPos=-0.5;
- if(retroPos > 0.5) retroPos=0.5;
- if(retroPos < -0.5) retroPos=-0.5;
-
- setCentralJointsAngle(frontPos,retroPos);
-
-}
-
-
-void Rover::initializeImu(){
-
- printf("Initialize IMU \r\n");
- //IMU
-
- accBias[0] = 0.0;
- accBias[1] = 0.0;
- accBias[2] = 0.0;
- gyroBias[0] = 0.0;
- gyroBias[1] = 0.0;
- gyroBias[2] = 0.0;
-
- mpu->initialize();
- bool mpu6050TestResult = mpu->testConnection();
- if(mpu6050TestResult) {
- printf("MPU6050 test passed \r\n");
- } else {
- printf("MPU6050 test failed \r\n");
- }
-
-
- calibrateImu();
-}
-
-void Rover::setRoverVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){
-
- Eigen::Vector3f cartesianSpeed;
- Eigen::Vector4f wheelsSpeed;
-
- updateRoverKin(pipe_radius, pipeDir);
-
- cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed;
- wheelsSpeed = S*cartesianSpeed;
- //std::cout << "des: " << wheelsSpeed.transpose() << std::endl;
- ionMcFront->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration);
-
- ionMcFront->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration);
-
- ionMcRetro->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration);
-
- ionMcRetro->setSpeed(2,wheelsSpeed[3],maxWheelAcceleration);
-
-}
-
-
-
-void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){
-
- Eigen::Vector3f cartesianSpeed;
- Eigen::Vector4f wheelsSpeed;
-
- float speedM1 = 0.0;
- float speedM2 = 0.0;
- float speedM3 = 0.0;
- float speedM4 = 0.0;
-
- updateRoverKin(pipe_radius, pipeDir);
-
- getMotorsSpeed(speedM1, speedM2, speedM3, speedM4);
-
- wheelsSpeed << speedM1, speedM2, speedM3, speedM4;
-
- //td::cout << "mis: " << wheelsSpeed.transpose() << std::endl;
-
-
- cartesianSpeed = S_inv*wheelsSpeed;
-
- forward_speed = cartesianSpeed[0];
- stabilization_speed = cartesianSpeed[1];
- asset_correction_speed = cartesianSpeed[2];
-
-}
-
-void Rover::getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx){
-
- Eigen::Vector3f cartesianSpeed;
- Eigen::Vector4f wheelsSpeed;
-
- float speedM1 = 0.0;
- float speedM2 = 0.0;
- float speedM3 = 0.0;
- float speedM4 = 0.0;
-
- getMotorsSpeed(speedM1, speedM2, speedM3, speedM4);
-
- front_dx = speedM1;
- front_sx = speedM2;
- retro_dx = speedM3;
- retro_sx = speedM4;
-
-}
-
-void Rover::updateRoverKin(float pipe_radius, int pipeDir){
- float rf = r_frontWheel;
- float rr = r_retroWheel;
- float l = retroFrontCentralDistance;
- float L = wheelsCntactPointDistanceFromPipeCenter;
- float pr = pipe_radius;
- float R_dx = 0.0;
- float R_sx = 0.0;
- float R_m = 0.0;
- if(pipeDir ==0){
- R_dx = 1.0;
- R_sx = 1.0;
- R_m = 1.0;
- }else if(pipeDir == 1){
- R_dx = pipeCurve_I;
- R_sx = pipeCurve_E;
- R_m = pipeCurve_M;
- }else if(pipeDir == -1){
- R_dx = pipeCurve_E;
- R_sx = pipeCurve_I;
- R_m = pipeCurve_M;
-
- }
- //AGGIORNARE la S
- S << 1.0*R_dx/(rf*R_m), pr/rf, -(L+l)/rf,
- -1.0*R_sx/(rf*R_m), pr/rf, -(L+l)/rf,
- 1.0*R_dx/(rr*R_m), -pr/rr, -(L+l)/rr,
- -1.0*R_sx/(rr*R_m), -pr/rr, -(L+l)/rr;
-
-
- Eigen::FullPivLU<Matrix3f> Core_S(S.transpose()*S);
- S_inv = Core_S.inverse()*S.transpose();
-
- }
-
-void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4){
-
-
- torM1 = ionMcFront->getMotorTorque(1);
- torM2 = ionMcFront->getMotorTorque(2);
- torM3 = ionMcRetro->getMotorTorque(1);
- torM4 = ionMcRetro->getMotorTorque(2);
-}
-
-void Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4){
-
- speedM1 = ionMcFront->getMotorSpeed(1);
- speedM2 = ionMcFront->getMotorSpeed(2);
- speedM3 = ionMcRetro->getMotorSpeed(1);
- speedM4 = ionMcRetro->getMotorSpeed(2);
-
-}
-
-void Rover::setCentralJointsAngle(float jointFront, float jointRetro){
-
- float LmRestPosFront = (9.0)/1000; //9
- float LmRestPosRetro = (9.0-0.5)/1000; //9
- float maxLenght = 0.02;
-
- float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront);
- float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro);
-
- frontActuonix->write(1.0 - LmFront/maxLenght);
- retroActuonix->write(1.0 - LmRetro/maxLenght);
-
- if (jointFront > 3.14*5/180){
- pipeDir = -1;
- }else if (jointFront < -3.14*5/180){
- pipeDir = 1;
- }else{
- pipeDir = 0;
- }
-
- pipeDir = 0;/////////////////////////////////////////////////////////////
-
-
-}
-
-float Rover::centralJoint2actuonix(float jointAngle){
-
- float alpha = deg2rad(34.4);
- float L1 = 23.022/1000;
- float L2 = 62.037/1000;
- float L3 = 51.013/1000;
- float L4 = 4.939/1000;
-
- float l1 = L1*sin(alpha + jointAngle);
- float l2 = L1*cos(alpha + jointAngle);
- float h = sqrt( (L2 - l1)*(L2 - l1) + (l2 - L4)*(l2 - L4) );
- return L3 - h;
-
-}
-
-
-float Rover::deg2rad(float deg) {
- return deg * M_PI / 180.0;
-}
-
-
-
-void Rover::calcImuAngles(float& pitch, float& pitch_vel, float& roll, float dtImu)
-{
- int16_t ax = 0;
- int16_t ay = 0;
- int16_t az = 0;
- int16_t gx = 0;
- int16_t gy = 0;
- int16_t gz = 0;
- float pitchAcc = 0.0;
- float rollAcc = 0.0;
- float pitchAcc_p = 0.0;
- float rollAcc_p = 0.0;
- float pitch_integrated = 0.0;
- float roll_integrated = 0.0;
- mpu->getMotion6(ax, ay, az, gx, gy, gz); //acquisisco i dati dal sensore
- float a_x=float(ax)/FS_a - accBias[0]; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT)
- float a_y=float(ay)/FS_a - accBias[1];
- float a_z=float(az)/FS_a;
- float g_x=float(gx)/FS_g - gyroBias[0];
- float g_y=float(gy)/FS_g - gyroBias[1];
- float g_z=float(gz)/FS_g - gyroBias[2];
- if(abs(g_x) > 100.0){
- pitch_vel = 0;
- }else{
- pitch_vel = g_x;
- }
- /*g_x_old = g_x;*/
- //pitch_vel = g_x;
-
-
- pitch_integrated = g_x * dtImu; // Angle around the X-axis // Integrate the gyro data(deg/s) over time to get angle, usato g_x perchè movimento attorno all'asse x
- roll_integrated = g_y * dtImu; // Angle around the Y-axis //uso g_y perchè ho il rollio ruotando atttorno all'asse y
-
- pitchAcc = atan2f(a_y, sqrt(a_z*a_z + a_x*a_x))*180/M_PI; //considerare formule paper, calcolo i contrubiti dovuti dall'accelerometro
- rollAcc = -atan2f(a_x, sqrt(a_y*a_y + a_z*a_z))*180/M_PI; //
-
- if ( abs( pitchAcc - pitchAcc_p ) > 10){
- pitchAcc = pitch;
- }
-
- if ( abs( rollAcc - rollAcc_p ) > 10){
- rollAcc = roll;
- }
-
- // Apply Complementary Filter
- pitch = 0.95f*( pitch + pitch_integrated) + 0.05f*pitchAcc;
- roll = 0.95f*( roll + roll_integrated) + 0.05f*(rollAcc);
-
- pitchAcc_p = pitchAcc;
- rollAcc_p = rollAcc;
-
-
-}
-
-void Rover::calibrateImu()
-{
-
- //Imu variables
- int16_t ax = 0;
- int16_t ay = 0;
- int16_t az = 0;
- int16_t gx = 0;
- int16_t gy = 0;
- int16_t gz = 0;
- float a_x,a_y,a_z;
- float g_x, g_y, g_z;
-
- const int CALIBRATION = 500;
-
- for(int i=0;i<50;i++)
- {
- mpu->getMotion6(ax, ay, az, gx, gy, gz);
- }
-
-
- printf(" START CALIBRATION \r\n");
-
-
- for(int i=0;i<CALIBRATION;i++)
- {
- mpu->getMotion6(ax, ay, az, gx, gy, gz); //acquisisco i dati dal sensore
- a_x=(float)ax/FS_a; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT)
- a_y=(float)ay/FS_a;
- //a_z=(float)az/FS_a;
- g_x=(float)gx/FS_g;
- g_y=(float)gy/FS_g;;
- g_z=(float)gz/FS_g;;
-
- accBias[0] += a_x;
- accBias[1] += a_y;
- gyroBias[0] += g_x;
- gyroBias[1] += g_y;
- gyroBias[2] += g_z;
-
- }
-
- //BIAS
- accBias[0] = accBias[0]/CALIBRATION;
- accBias[1] = accBias[1]/CALIBRATION;
- gyroBias[0] = gyroBias[0]/CALIBRATION;
- gyroBias[1] = gyroBias[1]/CALIBRATION;
- gyroBias[2] = gyroBias[2]/CALIBRATION;
-
-
- printf(" END CALIBRATION \r\n");
-
- //printf("%f\t %f\t %f\t %f\t %f\t \r\n", accBias[0], accBias[1], gyroBias[0],gyroBias[1],gyroBias[2]); //stampo le misure
-
-}
--- a/Robots/Rover/Rover.h Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,110 +0,0 @@
-#ifndef ROVER_H
-#define ROVER_H
-#include "mbed.h"
-
-#include "controlDefineVariables.h"
-
-#include "IONMcMotors.h"
-#include "MPU6050.h"
-#include "TOFs.h"
-#include "Servo.h"
-#include "Core.h"
-#include <Eigen/Dense.h>
-#include "platform/PlatformMutex.h"
-
-typedef Eigen::Matrix<float, 4, 4> Matrix4f;
-typedef Eigen::Matrix<float, 3, 3> Matrix3f;
-
-class Rover
-{
-public:
- Rover();
-
- void setRoverVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration);
- void getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius);
- void getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4);
- void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4);
-
- void setParameters(Eigen::VectorXd roverParameters);
-
- void getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx);
-
- void setCentralJointsAngle(float act1, float act2);
- void calcImuAngles(float& pitch, float& pitch_vel, float& roll, float dtImu);
-
- void computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx);
-
- float computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd);
-
- void initializeImu();
-
- void initializeTofs();
-
- void acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance);
-
-
-
-private:
-
- IONMcMotors *ionMcFront;
- IONMcMotors *ionMcRetro;
-
- Servo *frontActuonix;
- Servo *retroActuonix;
-
- MPU6050 *mpu;
-
- TOFs *tofs;
-
- float r_frontWheel;
- float r_retroWheel;
- float pipeCurve_I;
- float pipeCurve_E;
- float pipeCurve_M;
- float retroFrontCentralDistance;
- float wheelsCntactPointDistanceFromPipeCenter;
-
- //Imu variables
-
- float accBias[3];
- float gyroBias[3];
- int pitch_d;
- float pitch;
- float g_x_old;
-
- int forward_vel;
-
- int jointFront;
- int jointRetro;
- int enableStab;
- int enableCurv;
-
- float time;
- float vels_a;
- float vels_m;
-
- int pipeDir;
-
- Eigen::Matrix<float, 4, 3> S;
- Eigen::Matrix<float, 3, 4> S_inv;
-
- float frontPos;
- float retroPos;
-
- //private functions
- float deg2rad(float deg);
- float centralJoint2actuonix(float jointAngle);
- void calibrateImu();
- void updateRoverKin(float pipe_radius, int pipeDir);
-
- void setState(int state);
-
- int get_forward_vel();
- //PlatformMutex eth_mutex;
-
- float roverFrontDistance;
- float roverRetroDistance;
-
-};
-
-#endif
\ No newline at end of file
--- a/Robots/Rover/old/Rover_omni_cpp.txt Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,487 +0,0 @@
-#include "Rover.h"
-
-Rover::Rover(){
-
- ionMcBoudRate = 460800;
- meccanumBoardAddress = 128;
- meccanumMotorGearBoxRatio = 103;
- meccanumEncoderPulse = 512;
- meccanumKt = 0.00667;
- meccanumTransmissionRatio = 1.0;
- omniTransmissionRatio = 1.0;
- omniBoardAddress = 129;
- omniMotorGearBoxRatio = 103;
- omniEncoderPulse = 512;
- omniKt = 0.00667;
-
- r_meccanumWheel = 0.03;
- r_omniWheel = 0.019;
-
- pipeCurve_I = 0.165;
- pipeCurve_E = 0.2925;
- pipeCurve_M = 0.228;
-
- pipeDir = 0;
-
- omniMeccanumCentralDistance = 0.25;
- wheelsCntactPointDistanceFromPipeCenter = 0.08;
-
- eth_time_out = 2.0;
-
- S = Eigen::Matrix3f::Zero();
-
-
- ionMcMeccanum = new IONMcMotors(meccanumBoardAddress,ionMcBoudRate, PG_14, PG_9, meccanumMotorGearBoxRatio, meccanumEncoderPulse, meccanumKt, meccanumKt);
- ionMcOmni = new IONMcMotors(omniBoardAddress,ionMcBoudRate, PB_9, PB_8, omniMotorGearBoxRatio, omniEncoderPulse, omniKt, omniKt);
- meccanumActuonix = new Servo(PE_11);
- omniActuonix = new Servo(PE_9);
- mpu = new MPU6050(PF_15, PF_14);
- eth_tcp = new Eth_tcp(3154, 500);
-
- distance_sensors = new distanceSensors();
-
-
- eth_state = -1;
-
- eth_status = false;
-
-
-}
-
-void Rover::initializeTofs(){
- distance_sensors->TOFs_init();
- distance_sensors->TOFs_offset();
-}
-
-void Rover::acquireTofs(float &frontDistance, float &retroDistance){
-
- distance_sensors->TOFs_acquireFiltr();
- frontDistance = distance_sensors->getFrontDistance();
- retroDistance = distance_sensors->getRetroDistance();
-
-
-
-}
-
-
-void Rover::computeCentralJointsFromTofs(){
-
- float frontDistance;
- float retroDistance;
-
- acquireTofs(frontDistance, retroDistance);
-
-
-
- float frontVel = 0.05*frontDistance;
- float retroVel = -0.04*retroDistance;
-
-
- if(enableCurv==1){
- frontPos = frontPos + frontVel*0.005;
- retroPos = retroPos + retroVel*0.005;
- }else{
- frontPos = 0.0;
- retroPos = 0.0;
- }
-
- if(frontPos > 0.5) frontPos=0.5;
- if(frontPos < -0.5) frontPos=-0.5;
- if(retroPos > 0.5) retroPos=0.5;
- if(retroPos < -0.5) retroPos=-0.5;
-
- setCentralJointsAngle(frontPos,retroPos);
-
-}
-
-
-void Rover::initializeImu(){
-
- printf("Initialize IMU \r\n");
- //IMU
- FS_a = 8192; //per avere letture in g, sarebbe 32768/4 perchè 4g è il fondo scala
- FS_g = 131.072; //per avere letture in gradi/s, sarebbe 32768/250 perchè 250 è il fondo scala
-
-
- accBias[0] = 0.0;
- accBias[1] = 0.0;
- accBias[2] = 0.0;
- gyroBias[0] = 0.0;
- gyroBias[1] = 0.0;
- gyroBias[2] = 0.0;
-
-
- pitchAcc = 0.0;
- rollAcc = 0.0;
-
- pitch_integrated = 0.0;
- roll_integrated = 0.0;
-
- mpu->initialize();
- bool mpu6050TestResult = mpu->testConnection();
- if(mpu6050TestResult) {
- printf("MPU6050 test passed \r\n");
- } else {
- printf("MPU6050 test failed \r\n");
- }
-
-
- calibrateImu();
-}
-
-void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){
-
- Eigen::Vector3f cartesianSpeed;
- Eigen::Vector3f wheelsSpeed;
-
- updateRoverKin(pipe_radius, pipeDir);
-
- cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed;
- wheelsSpeed = S*cartesianSpeed;
- ionMcMeccanum->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration);
- ionMcMeccanum->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration);
- ionMcOmni->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration);
-
-}
-
-
-int Rover::get_forward_vel(){
- int fv;
- eth_mutex.lock();
- fv = forward_vel;
- eth_mutex.unlock();
- return fv;
-
-}
-int Rover::get_pitch(){
- int pit;
- eth_mutex.lock();
- pit = pitch_d;
- eth_mutex.unlock();
- return pit;
-}
-
-int Rover::get_jointFront(){
- int jf;
- eth_mutex.lock();
- jf = jointFront;
- eth_mutex.unlock();
- return jf;
-}
-int Rover::get_jointRetro(){
- int jr;
- eth_mutex.lock();
- jr = jointRetro;
- eth_mutex.unlock();
- return jr;
-}
-
-void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){
-
- Eigen::Vector3f cartesianSpeed;
- Eigen::Vector3f wheelsSpeed;
-
- float speedM1 = 0.0;
- float speedM2 = 0.0;
- float speedM3 = 0.0;
-
- updateRoverKin(pipe_radius, pipeDir);
-
- getMotorsSpeed(speedM1, speedM2, speedM3);
-
- wheelsSpeed << speedM1, speedM2, speedM3;
-
- cartesianSpeed = S_inv*wheelsSpeed;
-
- forward_speed = cartesianSpeed[0];
- stabilization_speed = cartesianSpeed[1];
- asset_correction_speed = cartesianSpeed[2];
-
-}
-
-void Rover::getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro){
-
- Eigen::Vector3f cartesianSpeed;
- Eigen::Vector3f wheelsSpeed;
-
- float speedM1 = 0.0;
- float speedM2 = 0.0;
- float speedM3 = 0.0;
-
- getMotorsSpeed(speedM1, speedM2, speedM3);
-
- speed_wheel_dx = speedM1;
- speed_wheel_sx = speedM2;
- speed_wheels_retro = speedM3;
-
-
-}
-
-void Rover::updateRoverKin(float pipe_radius, int pipeDir){
- float rm = r_meccanumWheel;
- float ro = r_omniWheel;
- float l = omniMeccanumCentralDistance;
- float L = wheelsCntactPointDistanceFromPipeCenter;
- float pr = pipe_radius;
- float R_dx = 0.0;
- float R_sx = 0.0;
- float R_m = 0.0;
- if(pipeDir ==0){
- R_dx = 1.0;
- R_sx = 1.0;
- R_m = 1.0;
- }else if(pipeDir == 1){
- R_dx = pipeCurve_I;
- R_sx = pipeCurve_E;
- R_m = pipeCurve_M;
- }else if(pipeDir == -1){
- R_dx = pipeCurve_E;
- R_sx = pipeCurve_I;
- R_m = pipeCurve_M;
-
- }
-
- S << -1.0*R_dx/(rm*R_m), pr/rm, -L/rm,
- 1.0*R_sx/(rm*R_m), pr/rm, -L/rm,
- 0, pr/ro, l/ro;
-
- S_inv << -rm/2, rm/2, 0,
- l*rm/(2*pr*(L+l)), l*rm/(2*pr*(L+l)), L*ro/(pr*(L+l)),
- -rm/(2*(L+l)), -rm/(2*(L+l)), ro/(L+l);
- }
-
-void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3){
-
-
- torM1 = ionMcMeccanum->getMotorTorque(1);
- torM2 = ionMcMeccanum->getMotorTorque(2);
- torM3 = ionMcOmni->getMotorTorque(1);
-
-}
-
-void Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3){
-
- speedM1 = ionMcMeccanum->getMotorSpeed(1);
- speedM2 = ionMcMeccanum->getMotorSpeed(2);
- speedM3 = ionMcOmni->getMotorSpeed(1);
-
-}
-
-void Rover::setCentralJointsAngle(float jointFront, float jointRetro){
-
- float LmRestPosFront = (9.0+0.5)/1000; //9
- float LmRestPosRetro = (9.0-0.5)/1000; //9
- float maxLenght = 0.02;
-
- float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront);
- float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro);
-
- meccanumActuonix->write(1.0 - LmFront/maxLenght);
- omniActuonix->write(1.0 - LmRetro/maxLenght);
-
- if (jointFront > 3.14*5/180){
- pipeDir = -1;
- }else if (jointFront < -3.14*5/180){
- pipeDir = 1;
- }else{
- pipeDir = 0;
- }
-
-
-}
-
-float Rover::centralJoint2actuonix(float jointAngle){
-
- float alpha = deg2rad(34.4);
- float L1 = 23.022/1000;
- float L2 = 62.037/1000;
- float L3 = 51.013/1000;
- float L4 = 4.939/1000;
-
- float l1 = L1*sin(alpha + jointAngle);
- float l2 = L1*cos(alpha + jointAngle);
- float h = sqrt( (L2 - l1)*(L2 - l1) + (l2 - L4)*(l2 - L4) );
- return L3 - h;
-
-}
-
-
-float Rover::deg2rad(float deg) {
- return deg * M_PI / 180.0;
-}
-
-
-
-void Rover::calcImuAngles(float& pitch, float& roll, float dtImu)
-{
-
- mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore
- a_x=float(ax)/FS_a - accBias[0]; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT)
- a_y=float(ay)/FS_a - accBias[1];
- a_z=float(az)/FS_a;
- g_x=float(gx)/FS_g - gyroBias[0];
- g_y=float(gy)/FS_g - gyroBias[1];
- g_z=float(gz)/FS_g - gyroBias[2];
-
-
- pitch_integrated = g_x * dtImu; // Angle around the X-axis // Integrate the gyro data(deg/s) over time to get angle, usato g_x perchè movimento attorno all'asse x
- roll_integrated = g_y * dtImu; // Angle around the Y-axis //uso g_y perchè ho il rollio ruotando atttorno all'asse y
-
- pitchAcc = atan2f(a_y, sqrt(a_z*a_z + a_x*a_x))*180/M_PI; //considerare formule paper, calcolo i contrubiti dovuti dall'accelerometro
- rollAcc = -atan2f(a_x, sqrt(a_y*a_y + a_z*a_z))*180/M_PI; //
-
- if ( abs( pitchAcc - pitchAcc_p ) > 10){
- pitchAcc = pitch;
- }
-
- if ( abs( rollAcc - rollAcc_p ) > 10){
- rollAcc = roll;
- }
-
- // Apply Complementary Filter
- pitch = 0.95f*( pitch + pitch_integrated) + 0.05f*pitchAcc;
- roll = 0.95f*( roll + roll_integrated) + 0.05f*(rollAcc);
-
- pitchAcc_p = pitchAcc;
- rollAcc_p = rollAcc;
-
-
-}
-
-void Rover::calibrateImu()
-{
- const int CALIBRATION = 500;
- for(int i=0;i<50;i++)
- {
- mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
- }
-
-
- printf(" START CALIBRATION \r\n");
-
-
- for(int i=0;i<CALIBRATION;i++)
- {
- mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore
- a_x=(float)ax/FS_a; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT)
- a_y=(float)ay/FS_a;
- //a_z=(float)az/FS_a;
- g_x=(float)gx/FS_g;
- g_y=(float)gy/FS_g;;
- g_z=(float)gz/FS_g;;
-
- accBias[0] += a_x;
- accBias[1] += a_y;
- gyroBias[0] += g_x;
- gyroBias[1] += g_y;
- gyroBias[2] += g_z;
-
- }
-
- //BIAS
- accBias[0] = accBias[0]/CALIBRATION;
- accBias[1] = accBias[1]/CALIBRATION;
- gyroBias[0] = gyroBias[0]/CALIBRATION;
- gyroBias[1] = gyroBias[1]/CALIBRATION;
- gyroBias[2] = gyroBias[2]/CALIBRATION;
-
-
- printf(" END CALIBRATION \r\n");
-
- //printf("%f\t %f\t %f\t %f\t %f\t \r\n", accBias[0], accBias[1], gyroBias[0],gyroBias[1],gyroBias[2]); //stampo le misure
-
-
-}
-
-
-
-void Rover::startEthComunication(){
- printf("Avvio la comunicazione ethernet \r\n");
- comunicationTimer.start();
- eth_tcp->connect();
- setState(1);
-
-}
-
-void Rover::ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) {
-
- if( eth_tcp->is_connected()){
- setState(2);
- printf("ETH: connected!\r\n");
- sock_open = true;
- }else{
- setState(1);
- }
-
- eth_status = eth_tcp->recv_pkt(cmd, recv_vec, n_of_int);
-
- eth_time = comunicationTimer.read();
- //printf("%f\r\n", (double)(eth_time - eth_time_sample_received));
-
-
- if(eth_status){
- eth_time_sample_received = comunicationTimer.read();
- if(cmd == 's'){
- if(n_of_int == 4){
- eth_mutex.lock();
- forward_vel = recv_vec.get();
- pitch_d = recv_vec.get();
- //jointFront = recv_vec.get();
- //jointRetro = recv_vec.get();
- enableStab = recv_vec.get();
- enableCurv= recv_vec.get();
- eth_mutex.unlock();
- }
-
- eth_mutex.lock();
- vec_to_send.put(_time*1000);
- vec_to_send.put(_pitch*1000);
- vec_to_send.put(_vels_a*1000);
- vec_to_send.put(_vels_m*1000);
- vec_to_send.put(_velf_a*1000);
- vec_to_send.put(_velf_m*1000);
-
- eth_mutex.unlock();
- eth_tcp->send_vec_of_int(vec_to_send);
-
- vec_to_send.clear();
-
- cmd = ' ';
-
- }
- }
-
- if((double)(eth_time - eth_time_sample_received)>eth_time_out && sock_open == true){
- eth_tcp->reset_connection();
- sock_open = false;
- eth_mutex.lock();
- forward_vel = 0.0;
- pitch_d = 0.0;
- //jointFront = 0.0;
- //jointRetro = 0.0;
- enableCurv = 0;
- eth_mutex.unlock();
- }
-
-
-}
-
-int Rover::getEthState(){
- int state;
-
- eth_mutex.lock();
- state = eth_state;
- eth_mutex.unlock();
-
- return state;
-}
-
-void Rover::setState(int state){
-
- eth_mutex.lock();
- eth_state = state;
- eth_mutex.unlock();
-
-}
-
-
--- a/Robots/Rover/old/Rover_omni_h.txt Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,172 +0,0 @@
-#ifndef ROVER_H
-#define ROVER_H
-#include "mbed.h"
-#include "IONMcMotors.h"
-#include "Servo.h"
-#include "Core.h"
-#include "MPU6050.h"
-#include "platform/PlatformMutex.h"
-
-#include "TOFs.h"
-
-#include "Eth_tcp.h"
-
-
-class Rover
-{
-public:
- Rover();
-
- void setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration);
- void getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius);
- void getMotorsTorque(float &torM1, float &torM2, float &torM3);
- void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3);
-
- void getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro);
-
-
- void setCentralJointsAngle(float act1, float act2);
- void calcImuAngles(float& pitch, float& roll, float dtImu);
-
- int get_forward_vel();
- int get_pitch();
- int get_jointFront();
- int get_jointRetro();
-
- void ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) ;
-
- int getEthState(void);
-
-
- void startEthComunication();
-
- void initializeImu();
-
- void initializeTofs();
- void acquireTofs(float &frontDistance, float &retroDistance);
-
- void computeCentralJointsFromTofs();
-
-
-
-private:
-
- IONMcMotors *ionMcMeccanum;
- IONMcMotors *ionMcOmni;
-
- Servo *meccanumActuonix;
- Servo *omniActuonix;
-
- MPU6050 *mpu;
-
- Eth_tcp *eth_tcp;
-
- Thread EthThread;
-
- distanceSensors *distance_sensors;
-
- int ionMcBoudRate;
-
- int meccanumMotorGearBoxRatio;
- int omniMotorGearBoxRatio;
-
- int meccanumBoardAddress;
- int omniBoardAddress;
-
- float meccanumTransmissionRatio;
- float omniTransmissionRatio;
-
- int meccanumEncoderPulse;
- int omniEncoderPulse;
-
- float meccanumKt;
- float omniKt;
-
- float r_meccanumWheel;
- float r_omniWheel;
-
- float wheelsCntactPointDistanceFromPipeCenter;
- float omniMeccanumCentralDistance;
-
- //Imu variables
- int16_t ax, ay, az;
- int16_t gx, gy, gz;
-
- float a_x,a_y,a_z;
- float g_x, g_y, g_z;
-
- int16_t FS_a;
- float FS_g;
-
- PlatformMutex eth_mutex;
-
-
- float accBias[3];
- float gyroBias[3];
-
-
-
- float pitchAcc;
- float rollAcc;
- float pitchAcc_p;
- float rollAcc_p;
- float pitch_integrated;
- float roll_integrated;
-
-
- int forward_vel;
- int pitch_d;
-
- int jointFront;
- int jointRetro;
- int enableStab;
- int enableCurv;
-
- float time;
- float pitch;
- float vels_a;
- float vels_m;
-
- float pipeCurve_I;
- float pipeCurve_E;
- float pipeCurve_M;
- int pipeDir;
-
- double eth_time_out; //ms
-
- int eth_state;
- double eth_time;
- double eth_time_sample_received;
-
- Eigen::Matrix3f S;
- Eigen::Matrix3f S_inv;
- //private functions
- float deg2rad(float deg);
- float centralJoint2actuonix(float jointAngle);
- void calibrateImu();
- void updateRoverKin(float pipe_radius, int pipeDir);
-
- void setState(int state);
-
-
- Timer comunicationTimer;
-
- char cmd;
- MyBuffer <int> vec_to_send;
- MyBuffer <int> recv_vec;
- double t_p;
- double t;
- int n_of_int;
- bool eth_status;
-
- bool sock_open;
-
-
-
- float frontPos;
- float retroPos;
-
-
-};
-
-#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SystemControl.lib Mon Dec 20 10:30:08 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/marcodesilva/code/SystemControl/#f757110a016c
--- a/systemControl.cpp Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,833 +0,0 @@
-#include "systemControl.h"
-
-#include <iostream>
-#include <iomanip>
-
-SystemControl::SystemControl(){
-
- dtPassedStab = 0.0;
- dtPassedArm = 0.0;
- dtPassedNavig = 0.0;
- dtPassedEth = 0.0;
-
- wdTime = 0.2;
-
- t = 0.0;
-
- printSchedulerCounter = 0;
- motor_enabled = false;
-
- actualDtControlCycle = 0.0;
-
- q_cmd.resize(8,1);
- dq_cmd.resize(8,1);
- ddq_cmd.resize(8,1);
-
- q_cmd = Vector8f::Zero();
- dq_cmd = Vector8f::Zero();
- ddq_cmd = Vector8f::Zero();
- q_mis = Vector8f::Zero();
- drone_orientation = Vector3f::Zero();
-
- omega.resize(8,1);
- zita.resize(8,1);
- maxJerk.resize(8,1);
- maxAcc.resize(8,1);
- maxVel.resize(8,1);
- maxPos.resize(8,1);
- minPos.resize(8,1);
-
- dq_stab_int = 0.0;
- dq_stab = 0.0;
- q_stab = 0.0;
-
-
-
- debugVector = Matrix<float, 24, 1> ::Zero();
-
- debug = false;
-
- parametersConfigurated = false;
-
- ethDone = false;
-
- printThread = new Thread(osPriorityNormal, 1 * 1024 /*32K stack size*/);
-
- ledEth = new DigitalOut(PG_3);
- ledError = new DigitalOut(PG_2);
-
-}
-
-void SystemControl::initEthernet(){
- printf("INIT ETHERNET!! \r\n");
- eth_tcp = new Eth_tcp(3); // server timeout 1s
- robotStatus = eth_tcp->connect();
- wdTimer.start();
-
-}
-
-void SystemControl::initArm() {
- printf("Creo il braccio \r\n");
- arm = new ARAP180_WITH_ROVER();
-
- i2c_tool = new I2C_master(PC_9, PA_8, 0x54);
-
- K_firstOrder << 25,25,25,5,5,5;
- K_secondOrder << 1500,1500,1500,500,500,500;
-
- D_secondOrder << 20,20,20,15,15,15;
-
- invKin_n_max_iter = 1;
- invKin_max_pError = 0.001;
- invKin_max_oError = 0.001;
-
- omega << 3.0,3.0,3.0,3.0,3.0,3.0,3.0,3.0;
- zita << 0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8;
- maxJerk << 3,3,3,3,3,3,3,3;
- maxAcc << 20,100,100,100,100,100,100,100;
- maxVel << 1,3,3,3,3,3,3,3;
-
- minPos << -1000.0,-M_PI/4, -2*M_PI, -M_PI/2 + M_PI/8, 0.0, -3.1, -M_PI, -M_PI;
- maxPos << 1000.0, M_PI/4, 2*M_PI, M_PI - M_PI/6, 2*M_PI/3, 0.85, M_PI, M_PI;
-
- first_q.resize(8,1);
- first_q << 0.0,
- 0.0,
- 0.0,
- -M_PI/2 + M_PI/6,
- 0.75,
- -2.5,
- M_PI/2,
- 0.0;
-
- velAccSafetyFactor = 2;
- forceTorqueSafetyFactor = 100;
-
- minCartPos << 0.1,0.1,0.1;
- maxCartPos << 0.3,0.3,0.3;
-
- followFilter = new FollowFilter(omega, zita, maxJerk, maxAcc, maxVel, maxPos, minPos);
- inverseKinematicsControl = new InverseKinematicsControl(arm, K_firstOrder, K_secondOrder, D_secondOrder, maxAcc, maxVel, maxPos, minPos, invKin_n_max_iter, invKin_max_pError, invKin_max_oError);
- safeCheck = new SafeCheck(arm, maxPos, minPos, velAccSafetyFactor*maxVel, velAccSafetyFactor*maxAcc, maxCartPos, minCartPos);
- followFilter->initFollowFilter(first_q);
- inverseKinematicsControl->initInverseKinematicsControl(first_q);
- safeCheck->initSafeCheck();
-
- q_mis = first_q;
- q_cmd = q_mis;
-
- T_e_b_m = arm->forwardKinematics(first_q);
-
- batteryServo = new Servo(PA_3);
- batteryServo->write(0.0);
-
-}
-
-void SystemControl::initRover() {
- printf("Creo il rover \r\n");
-
- rover = new Rover();
- pitch_m = 0.0;
- roll_m = 0.0;
-
- pitch_d = 0.0;
- vels_d = 0.0;
- velf_d = 0.0;
- vela_d = 0.0;
- velf_m = 0.0;
- vels_m = 0.0;
- vela_m = 0.0;
- front_vel_dx = 0.0;
- front_vel_sx = 0.0;
- retro_vel_dx = 0.0;
- retro_vel_sx = 0.0;
- forward_vel = 0.0;
- jointFront_d = 0.0;
- jointRetro_d = 0.0;
- roverLaserFrontDx = 0.0;
- roverLaserFrontSx = 0.0;
- roverLaserRetroDx = 0.0;
- roverLaserRetroSx = 0.0;
- roverFrontDistance = 0.0;
- roverRetroDistance = 0.0;
-
- roverLongitudinalIntegratedPosition = 0.0;
-
- robotCmd.Data.roverForwardVel = 0.0;
- robotCmd.Data.roverZVel = 0.0;
- robotCmd.Data.roverFrontJointAngle = 0.0;
- robotCmd.Data.roverRetroJointAngle = 0.0;
-
- pipeRadious = 0.125;
- Kp_stabilization=1.2; //0.6
- wheelsAcceleration = 10000;
-
- rover->initializeImu();
- //rover->initializeTofs(); //in radiants (front, retro) ///////////////////////////////
- rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
- rover->setCentralJointsAngle(0.0,0.0); //in radiants (front, retro)
-
- printf("Creato il rover \r\n");
-
-
-
-}
-
-void SystemControl::startControlThread(){
-
- printf("\nControl thread started!\r\n");
-
- //stabilizationThread->start(this,&SystemControl::stabilizationThreadFunction);
- //navigationThread->start(this,&SystemControl::navigationThreadFunction);
- //armThread->start(this,&SystemControl::armThreadFunction);
- //printThread->start(this,&SystemControl::printThreadFunction);
- //schedulerThread->start(this,&SystemControl::schedulerThreadFunction);
-
-
-}
-
-
-void SystemControl::setConfigurationParameters() {
-
- Matrix<double, 7, 1> roverParameters;
- Matrix<double, 40, 1> armParameters;
-
- wdTime = robotCnf.Data.wdTime;
- debug = robotCnf.Data.debug;
-
- dtBoard = robotCnf.Data.dtBoard;
- r_frontWheel = robotCnf.Data.r_frontWheel;
- r_retroWheel = robotCnf.Data.r_retroWheel;
- pipeCurve_I = robotCnf.Data.pipeCurve_I;
- pipeCurve_E = robotCnf.Data.pipeCurve_E;
- pipeCurve_M = robotCnf.Data.pipeCurve_M;
- retroFrontCentralDistance = robotCnf.Data.retroFrontCentralDistance;
- wheelsCntactPointDistanceFromPipeCenter = robotCnf.Data.wheelsCntactPointDistanceFromPipeCenter;
- pipeRadious = robotCnf.Data.pipeRadious;
- Kp_stabilization = robotCnf.Data.Kp_stabilization;
- Kd_stabilization = robotCnf.Data.Kd_stabilization;
-
- Kp_zero_torque = robotCnf.Data.Kp_zero_torque;
- Ki_zero_torque = robotCnf.Data.Ki_zero_torque;
-
- std::cout << "Kp_stab: " << Kp_stabilization << " Kd_stab: " << Kd_stabilization << std::endl;
- std::cout << "Kp_zeroTorque: " << Kp_zero_torque << " Kd_zeroTorque: " << Ki_zero_torque << std::endl;
-
- wheelsAcceleration = robotCnf.Data.wheelsAcceleration;
-
- velAccSafetyFactor = robotCnf.Data.velAccSafetyFactor;
-
- maxCartPos << robotCnf.Data.maxCartPos_x, robotCnf.Data.maxCartPos_y, robotCnf.Data.maxCartPos_z;
- minCartPos << robotCnf.Data.minCartPos_x, robotCnf.Data.minCartPos_y, robotCnf.Data.minCartPos_z;
-
-
- roverParameters << r_frontWheel, r_retroWheel, pipeCurve_I, pipeCurve_E, pipeCurve_M, retroFrontCentralDistance, wheelsCntactPointDistanceFromPipeCenter;
-
- rover->setParameters(roverParameters);
-
- armParameters << robotCnf.Data.a3, robotCnf.Data.a4 , robotCnf.Data.a5, robotCnf.Data.a5i, robotCnf.Data.a6, robotCnf.Data.d7, robotCnf.Data.x_0_r2, robotCnf.Data.y_0_r2, robotCnf.Data.z_0_r2,
- robotCnf.Data.massr ,robotCnf.Data.mpxr ,robotCnf.Data.mpyr ,robotCnf.Data.mpzr ,robotCnf.Data.mass1, robotCnf.Data.mpx1 ,robotCnf.Data.mpy1 ,robotCnf.Data.mpz1 ,robotCnf.Data.mass2,robotCnf.Data.mpx2 ,robotCnf.Data.mpy2 ,robotCnf.Data.mpz2 ,robotCnf.Data.mass3,robotCnf.Data.mpx3 ,robotCnf.Data.mpy3 ,robotCnf.Data.mpz3 ,robotCnf.Data.mass4,robotCnf.Data.mpx4 ,robotCnf.Data.mpy4 ,robotCnf.Data.mpz4 ,robotCnf.Data.mass5,robotCnf.Data.mpx5 ,robotCnf.Data.mpy5 ,robotCnf.Data.mpz5 ,robotCnf.Data.mass6,robotCnf.Data.mpx6 ,robotCnf.Data.mpy6 ,robotCnf.Data.mpz6,robotCnf.Data.mass_battery ,robotCnf.Data.mpx_battery ,robotCnf.Data.mpz_battery;
-
- arm->setParameters(armParameters);
-
- float o = robotCnf.Data.jointsFilterOmega;
- float z = robotCnf.Data.jointsFilterZita;
- float ma = robotCnf.Data.jointsMaxAcc;
- float mv = robotCnf.Data.jointsMaxVel;
- omega << o,o,o,o,o,o,o,o;
- zita << z,z,z,z,z,z,z,z;
- maxJerk << 1000,1000,1000,1000,1000,1000,1000,1000;
- maxAcc << 20,ma,ma,ma,ma,ma,ma,ma;
- maxVel << 1,mv,mv,mv,mv,mv,mv,mv;
-
- invKin_n_max_iter = 1;
- invKin_max_pError = 0.001;
- invKin_max_oError = 0.001;
-
- WeightVector << robotCnf.Data.jacobianInversionWeight1,robotCnf.Data.jacobianInversionWeight2,robotCnf.Data.jacobianInversionWeight3,robotCnf.Data.jacobianInversionWeight4,robotCnf.Data.jacobianInversionWeight5,robotCnf.Data.jacobianInversionWeight6,robotCnf.Data.jacobianInversionWeight7,robotCnf.Data.jacobianInversionWeight8;
-
- K_firstOrder << robotCnf.Data.kp_first_order,robotCnf.Data.kp_first_order,robotCnf.Data.kp_first_order,robotCnf.Data.ko_first_order,robotCnf.Data.ko_first_order,robotCnf.Data.ko_first_order;
- K_secondOrder << robotCnf.Data.kp_second_order,robotCnf.Data.kp_second_order,robotCnf.Data.kp_second_order,robotCnf.Data.ko_second_order,robotCnf.Data.ko_second_order,robotCnf.Data.ko_second_order;
-
- D_secondOrder << robotCnf.Data.dp_second_order,robotCnf.Data.dp_second_order,robotCnf.Data.dp_second_order,robotCnf.Data.do_second_order,robotCnf.Data.do_second_order,robotCnf.Data.do_second_order;
-
-
- followFilter->setFollowFilterParameters(omega, zita, maxJerk, maxAcc, maxVel, maxPos, minPos);
- inverseKinematicsControl->setInverseKinematicsControlParameters(K_firstOrder, K_secondOrder, D_secondOrder, WeightVector, maxAcc, maxVel, maxPos, minPos, invKin_n_max_iter, invKin_max_pError, invKin_max_oError);
-
-
- safeCheck->setSafeCheckParameters(maxPos,minPos, velAccSafetyFactor*maxVel, velAccSafetyFactor*maxAcc, maxCartPos, minCartPos);
- safeCheck->initSafeCheck();
-
- std::cout << "K_firstOrder: " << K_firstOrder.transpose() << std::endl;
- std::cout << "K_secondOrder: " << K_secondOrder.transpose() << std::endl;
- std::cout << "D_secondOrder: " << D_secondOrder.transpose() << std::endl;
- std::cout << "WeightVector: " << WeightVector.transpose() << std::endl;
-
- dq_stab_int = 0.0;
- dq_stab = 0.0;
- q_stab = 0.0;
-
- if(!debug){
- arm_mutex.lock();
- if(!parametersConfigurated){
- if(!debug)arm->setOperatingMode(3,4);
- std::cout << "set operating mode" << std::endl;
- }
- if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
- arm_mutex.unlock();
- }else{
- q_mis = first_q;
- }
-
- q_cmd = q_mis;
-
-
-
-}
-
-
-void SystemControl::armEnableMotors() {
-
- if(!motor_enabled && parametersConfigurated) {
-
- arm_mutex.lock();
- if(!debug)arm->enableMotors(1);
- arm_mutex.unlock();
- motor_enabled = true;
-
- }
-
-}
-
-void SystemControl::armDisableMotors() {
- arm_mutex.lock();
- if(!debug)arm->enableMotors(0);
- arm_mutex.unlock();
- motor_enabled = false;
-}
-
-
-void SystemControl::setResponse() {
- data_mutex.lock();
-
- robotRsp.Data.estRoverJointPos1 = q_mis(0);
- robotRsp.Data.estRoverJointPos2 = q_mis(1);
- robotRsp.Data.misArmJointPos1 = q_mis(2);
- robotRsp.Data.misArmJointPos2 = q_mis(3);
- robotRsp.Data.misArmJointPos3 = q_mis(4);
- robotRsp.Data.misArmJointPos4 = q_mis(5);
- robotRsp.Data.misArmJointPos5 = q_mis(6);
- robotRsp.Data.misArmJointPos6 = q_mis(7);
-
- Vector4f quaternion_mis = utilities::rot2quat(T_e_b_m.block(0,0,3,3));
-
- robotRsp.Data.misTebX = T_e_b_m(0,3);
- robotRsp.Data.misTebY = T_e_b_m(1,3);
- robotRsp.Data.misTebZ = T_e_b_m(2,3);
-
- robotRsp.Data.misTebQ0 = quaternion_mis(0);
- robotRsp.Data.misTebQ1 = quaternion_mis(1);
- robotRsp.Data.misTebQ2 = quaternion_mis(2);
- robotRsp.Data.misTebQ3 = quaternion_mis(3);
-
-
- robotRsp.Data.droneFx = wrench_system(0);
- robotRsp.Data.droneFy = wrench_system(1);
- robotRsp.Data.droneFz = wrench_system(2);
- robotRsp.Data.droneTx = wrench_system(3);
- robotRsp.Data.droneTy = wrench_system(4);
- robotRsp.Data.droneTz = wrench_system(5);
-
- robotRsp.Data.roverPitchAngle = utilities::deg2rad(pitch_m);
- robotRsp.Data.roverStabVel = vels_d;
- robotRsp.Data.roverLaserFrontDx = roverLaserFrontDx;
- robotRsp.Data.roverLaserFrontSx = roverLaserFrontSx;
- robotRsp.Data.roverLaserRetroDx = roverLaserRetroDx;
- robotRsp.Data.roverLaserRetroSx = roverLaserRetroSx;
-
- robotRsp.Data.roverForwardVel = dq_cmd(0);
- robotRsp.Data.roverZVel = vela_m;
-
- robotRsp.StatusData = robotStatus;
-
- data_mutex.unlock();
-
-}
-
-
-
-void SystemControl::updateEthCommunication(){
-
- setResponse();
-
- data_mutex.lock();
- robotStatus = eth_tcp->updateEthCommunication(robotCnf, robotCmd, robotRsp);
- data_mutex.unlock();
-
- switch(robotStatus){
-
- case ETH_LOST_CONNECTION:
- //azioni da fare se la connessione si interrompe
- //armDisableMotors();
- //robotCmd.Cmd = DISABLE_MOTORS;
- rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
- dq_cmd(5) = 0.0;
- ematClosed = false;
- ledEth->write(0);
-
- break;
-
- case ETH_ERROR_SENDING_DATA:
- //azioni da fare se non riesco ad inviare la risposta
- break;
-
- case ETH_WRONG_RECV_SIZE:
- case ETH_CHECKSUM_ERROR:
- //azioni da fare se il pacchetto arrivato non è corretto
-
- break;
-
- case ETH_NEW_CMD:
- wdTimer.reset();
- break;
-
- case ETH_NEW_CMD_CURRENT_VAL_REQUEST:
- //azioni da fare se è arrivato un comando
-
- wdTimer.reset();
- break;
-
- case ETH_NEW_CNF:
- //azioni da fare se è arrivata una configurazione
- printf("E arrivata una configurazione!!!\n");
- setConfigurationParameters();
- parametersConfigurated = true;
- ledEth->write(1);
-
-
- break;
-
- default:
- //istruzioni di default
- break;
- }
-
- ethDone = true;
-
-
-
-}
-
-
-void SystemControl::stabilizationFunction() {
-
- if(parametersConfigurated){
- ComandMsg cmd_;
-
- data_mutex.lock();
- cmd_ = robotCmd;
- data_mutex.unlock();
-
-
- float torqueError = 0.0-wrench_system(3);
- dq_stab_int += Ki_zero_torque*torqueError*dtBoard;
-
- dq_stab = Kp_zero_torque*torqueError + dq_stab_int;
-
- rover->calcImuAngles(pitch_m, pitch_vel_m, roll_m, dtBoard);
- //std::cout << "pd: " << pitch_d << " pm: " << pitch_m << std::endl;
- vels_d = rover->computeStabilizationVel(pitch_d, pitch_m, pitch_vel_m, Kp_stabilization, Kd_stabilization);
-
- rover->setRoverVelocity(velf_d, vels_d , 0.0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
- }else{
-
- rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
- ledError->write(1);
- }
-
-}
-
-void SystemControl::navigationFunction() {
-
- if(parametersConfigurated && wdTimer.read() < wdTime){
- ComandMsg cmd_;
-
- data_mutex.lock();
- cmd_ = robotCmd;
- data_mutex.unlock();
- //rover->computeCentralJointsFromTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx);
-
-
- //rover->acquireTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx, roverFrontDistance, roverRetroDistance);;
- //if(!debug)rover->setCentralJointsAngle(cmd_.Data.roverFrontJointAngle,cmd_.Data.roverRetroJointAngle); //in radiants (front, retro)
- rover->setCentralJointsAngle(0.04,0.0); //in radiants (front, retro)
-
- rover->getRoverVelocity(velf_m, vels_m, vela_m, pipeRadious);
- roverLongitudinalIntegratedPosition -= velf_m*dtBoard;
-
-
- }else{
- rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
- ledError->write(0);
-
- }
-
-}
-
-Vector8f SystemControl::calcNullSpaceVelocity(){
- Vector8f dq_null_space;
-
- dq_null_space << 0.0,dq_stab,0.0,0.0,2.0*(M_PI/6-q_cmd(4)),0.0,0.0,0.0;
- return dq_null_space;
-}
-
-
-void SystemControl::armRoverKinematicControlFunction() {
-
- Matrix4f T_e_b = Matrix4f::Identity();
- Vector8f dq_null_space;
-
-
- Vector4f quaternion = Vector4f::Zero();
- data_mutex.lock();
- ComandMsg cmd_ = robotCmd;
- data_mutex.unlock();
-
- if(parametersConfigurated && wdTimer.read() < wdTime){
- switch (cmd_.Cmd)
- {
-
- case IDLE_STATE:
- robotControlMode = IDLE_MODE;
-
- break;
-
- case DISABLE_MOTORS:
- armDisableMotors();
- q_stab = 0.0;
- pitch_d = 0.0;
- velf_d = 0.0;
- break;
-
- case ENABLE_ARM:
- armEnableMotors();
- safeCheck->initSafeCheck();
- q_stab = 0.0;
- pitch_d = 0.0;
- velf_d = 0.0;
- //roverLongitudinalIntegratedPosition = 0.0; ///////////////////////////ricorda di togliere
-
- //roverLongitudinalIntegratedPosition = 0.0; ///////////////////////////ricorda di togliere
- //std::cout << "enable arm cmd" << std::endl;
- break;
-
- case GET_CURRENT_ROBOT_STATE:
- //std::cout << "Get robot state" << std::endl;
-
- if(debug){
- q_mis = q_cmd;
- }else{
- arm_mutex.lock();
- if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
- arm_mutex.unlock();
- }
- T_e_b_m = arm->forwardKinematics(q_mis); //Dyrect kinematics
- robotControlMode = GET_ROBOT_STATE_MODE;
-
- break;
-
- case ARM_JOINT_CONTROL:
- if(precCmd != ARM_JOINT_CONTROL){
- std::cout << "Joint cntrl" << std::endl;
- if(debug){
- q_mis = q_cmd;
- }else{
- arm_mutex.lock();
- std::cout << "get joint pos" << std::endl;
- //roverLongitudinalIntegratedPosition = 0.0;
- if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
- arm_mutex.unlock();
- }
- followFilter->initFollowFilter(q_mis);
-
- }
- robotControlMode = JOINT_CONTROL_MODE;
-
- q_cmd << cmd_.Data.roverLongitudinalPosition,0.0,cmd_.Data.jointPos1, cmd_.Data.jointPos2, cmd_.Data.jointPos3, cmd_.Data.jointPos4, cmd_.Data.jointPos5, cmd_.Data.toolOrientAroundPipeAxis;
- followFilter->updateFollowFilter(q_cmd, dtBoard);
- followFilter->getJointCmd(q_cmd, dq_cmd, ddq_cmd);
- //dq_cmd(0) = dq_stab;
- if(safeCheck->check(q_cmd,dq_cmd,ddq_cmd, safeCheck->JOINT_MODE)){
- arm_mutex.lock();
- if(!debug)arm->setJointPos(q_cmd);
- //q_stab += dq_stab*dtBoard;
- float torqueError = 0.0-wrench_system(3);
- q_cmd(1) = 0.04*torqueError;
- pitch_d = utilities::rad2deg(q_cmd(1));
- velf_d = -dq_cmd(0);
-
- arm_mutex.unlock();
- }else{
- printf("arm locked!\t");
- data_mutex.lock();
- robotStatus = ROBOT_SAFETY_MODE;
- pitch_d = 0.0;
- velf_d = 0.0;
- q_stab = 0.0;
-
- data_mutex.unlock();
- }
-
- break;
-
- case ARM_KINEMATIC_CONTROL_FIRST_ORDER:
-
- if(precCmd != ARM_KINEMATIC_CONTROL_FIRST_ORDER){
- std::cout << "Kinematic first order" << std::endl;
-
- if(debug){
- q_mis = q_cmd;
- }else{
- arm_mutex.lock();
- if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
- arm_mutex.unlock();
- }
- followFilter->initFollowFilter(q_mis);
- inverseKinematicsControl->initInverseKinematicsControl(q_mis);
- }
- robotControlMode = KINEMATIC_CONTROL_FIRST_ORDER_MODE;
-
- T_e_b(0,3) = cmd_.Data.armX; T_e_b(1,3) = cmd_.Data.armY; T_e_b(2,3) = cmd_.Data.armZ;
- quaternion << cmd_.Data.armQ0, cmd_.Data.armQ1, cmd_.Data.armQ2, cmd_.Data.armQ3;
-
- T_e_b.block(0,0,3,3) = utilities::matrixOrthonormalization( utilities::QuatToMat(quaternion) );
- dq_null_space = calcNullSpaceVelocity();
- inverseKinematicsControl->updateInverseKinematicsControl_firstOrder(T_e_b, Vector6f::Zero(),dq_null_space, dtBoard);
- inverseKinematicsControl->getJointCmd(q_cmd, dq_cmd, ddq_cmd);
- inverseKinematicsControl->getDebugVector(debugVector);
- if(safeCheck->check(q_cmd,dq_cmd,ddq_cmd,safeCheck->CARTESIAN_MODE)){
- arm_mutex.lock();
- if(!debug)arm->setJointPos(q_cmd);
- pitch_d = utilities::rad2deg(q_cmd(1));
- velf_d = -dq_cmd(0);
- arm_mutex.unlock();
- }else{
- printf("arm locked!\t");
- pitch_d = 0.0;
- velf_d = 0.0;
-
- data_mutex.lock();
- robotStatus = ROBOT_SAFETY_MODE;
- data_mutex.unlock();
- }
-
- break;
-
- case ARM_KINEMATIC_CONTROL_SECOND_ORDER:
-
- if(precCmd != ARM_KINEMATIC_CONTROL_SECOND_ORDER){
- std::cout << "Kinematic second order" << std::endl;
-
- if(debug){
- q_mis = q_cmd;
- }else{
- arm_mutex.lock();
- if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
- arm_mutex.unlock();
- }
- inverseKinematicsControl->initInverseKinematicsControl(q_mis);
- }
- robotControlMode = KINEMATIC_CONTROL_SECOND_ORDER_MODE;
-
-
- T_e_b(0,3) = cmd_.Data.armX; T_e_b(1,3) = cmd_.Data.armY; T_e_b(2,3) = cmd_.Data.armZ;
- quaternion << cmd_.Data.armQ0, cmd_.Data.armQ1, cmd_.Data.armQ2, cmd_.Data.armQ3;
-
- T_e_b.block(0,0,3,3) = utilities::matrixOrthonormalization( utilities::QuatToMat(quaternion) );
-
- inverseKinematicsControl->updateInverseKinematicsControl_secondOrder(T_e_b, Vector6f::Zero(), Vector6f::Zero(), dtBoard);
- inverseKinematicsControl->getJointCmd(q_cmd, dq_cmd, ddq_cmd);
- inverseKinematicsControl->getDebugVector(debugVector);
-
- if(safeCheck->check(q_cmd,dq_cmd,ddq_cmd,safeCheck->CARTESIAN_MODE)){
- arm_mutex.lock();
- if(!debug)arm->setJointPos(q_cmd);
- pitch_d = utilities::rad2deg(q_cmd(1));
- velf_d = -dq_cmd(0);
-
- arm_mutex.unlock();
- }else{
- printf("arm locked!\t");
- data_mutex.lock();
- robotStatus = ROBOT_SAFETY_MODE;
- pitch_d = 0.0;
- velf_d = 0.0;
- data_mutex.unlock();
- }
- break;
-
- default:
-
- break;
- }
-
- precCmd = cmd_.Cmd;
- drone_orientation << cmd_.Data.drone_roll, cmd_.Data.drone_pitch, 0.0;
- ematClosed = cmd_.Data.toolEmatClose;
-
- }else{
- precCmd = DISABLE_MOTORS;
- }
-}
-
-
-float SystemControl::calcDroneDynamics(){
- Vector8f q_forDyn;
- if(parametersConfigurated){
-
- q_forDyn << roverLongitudinalIntegratedPosition, utilities::deg2rad(pitch_m), q_cmd.block(2,0,6,1);
-
- //batteryServo->write(battery_position/0.14);
- drone_orientation << 0,0,0;
-
- wrench_armAndRover = arm->wrenchGr(q_cmd,drone_orientation);
-
- battery_pos = arm->batteryPos_from_wrench(-wrench_armAndRover, drone_orientation);
- //battery_pos = 0.0;
-
- batteryServo->write(battery_pos/0.14);
-
-
- wrench_system = arm->wrenchBattery(battery_pos, drone_orientation) + wrench_armAndRover; //
-
- float battery_vel = (battery_pos - battery_pos_prec)/dtBoard;
- float max_motor_vel = 0.03;
- float max_battery_pos = 0.14;
-
- if(battery_vel > max_motor_vel) battery_vel = max_motor_vel;
- if(battery_vel < -max_motor_vel) battery_vel = -max_motor_vel;
-
- battery_pos_saturated = battery_pos_prec + battery_vel*dtBoard;
- if(battery_pos_saturated < 0.0) battery_pos_saturated = 0.0;
- if(battery_pos_saturated > max_battery_pos) battery_pos_saturated = max_battery_pos;
- battery_pos_prec = battery_pos_saturated;
-
- }
- return battery_pos_saturated;
-
-
-}
-
-void SystemControl::toolControlFunction(){
-
- float _toolWheelsPos_dx_mis, _toolWheelsPos_sx_mis, _toolWheelsSpeed_dx_mis, _toolWheelsSpeed_sx_mis;
-
- toolWheelsVel_d = dq_cmd(7); //(3.14/8.128)*sin(0.02*2*3.14*t_temp);//q_cmd(7); //dq_cmd(7);
-
- if(ematClosed){
- toolClampPos_d = 9.0;
- }else{
- toolClampPos_d = -13.0;
- }
-
-
-
- if(i2c_tool->updateI2CCommunication(Cmd1, toolClampPos_d, toolWheelsVel_d, _toolWheelsPos_dx_mis, _toolWheelsPos_sx_mis, _toolWheelsSpeed_dx_mis, _toolWheelsSpeed_sx_mis)){
- toolWheelsPos_dx_mis = _toolWheelsPos_dx_mis;
- toolWheelsPos_sx_mis = _toolWheelsPos_sx_mis;
- toolWheelsSpeed_dx_mis = _toolWheelsSpeed_dx_mis;
- toolWheelsSpeed_sx_mis = _toolWheelsSpeed_sx_mis;
- }
-
- //t_temp = t_temp + actualDtControlCycle;
-
-}
-
-/*void SystemControl::schedulerThreadFunction() {
-
- double timeStabPrec = 0.0;
- double timeNavigPrec = 0.0;
- double timeEthPrec = 0.0;
- double timeArmPrec = 0.0;
-
- float time = 0.0;
-
- Timer schedulerTimer;
-
- schedulerTimer.start();
-
- timeStabPrec = schedulerTimer.read();
- timeEthPrec = schedulerTimer.read();
- timeNavigPrec = schedulerTimer.read();
- timeArmPrec = schedulerTimer.read();
-
- while (1){
-
- time = schedulerTimer.read();
- //if(ethDone){
- if(time - timeStabPrec > dtStab){
- //dtPassedStab = time - timeStabPrec;
- timeStabPrec = time;
-
- stabilizationThread->flags_set(STAB_ENABLE_FLAG);
-
- }
-
- if(NAVIG && time - timeNavigPrec > dtNavig){
- //dtPassedNavig = time - timeNavigPrec;
- timeNavigPrec = time;
-
- navigationThread->flags_set(NAVIG_ENABLE_FLAG);
-
- }
-
- if(time - timeArmPrec > dtArm){
-
- //dtPassedArm = time - timeArmPrec;
- timeArmPrec = time;
-
- armThread->flags_set(ARM_ENABLE_FLAG);
-
- }
-
- ethDone = false;
- //}
-
-
- ThisThread::sleep_for(1);
-
- }
-
-}*/
-
-
-void SystemControl::printThreadFunction() {
-
- //while (1){
- if(printSchedulerCounter == 100){
- std::cout << std::fixed;
- std::cout << std::setprecision(4);
-
- std::cout
- << "Cmd: " << robotCmd.Cmd
- //<< " State: " << robotRsp.StatusData
- << " q_cmd: " << q_cmd.transpose()
- << " dq_cmd: " << dq_cmd.transpose()
- << " toolWheelsVel_d: " << toolWheelsVel_d
-
- //<< " wrench: " << wrench_armAndRover.transpose()
- << " velf_d: " << velf_d
- << " vels_d: " << vels_d
- << " battery_pos: " << battery_pos
- << " pitch_m: " << utilities::deg2rad(pitch_m)
- << " forw_pos: " << roverLongitudinalIntegratedPosition
- << " Ts: " << actualDtControlCycle*1000
- << std::endl;
- printSchedulerCounter = 0;
- }
- printSchedulerCounter++;
- //ThisThread::sleep_for(100);
- //}
-
-}
--- a/systemControl.h Wed Sep 29 11:20:56 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,236 +0,0 @@
-#ifndef SYSTEM_CONTROL_H
-#define SYSTEM_CONTROL_H
-
-#include "mbed.h"
-#include "Servo.h"
-#include "Rover.h"
-#include "ARAP180_with_rover.h"
-
-#include "math.h"
-#include "TOFs.h"
-#include "Eth_tcp.h"
-#include "mbed_mem_trace.h"
-
-#include "controlDefineVariables.h"
-
-#include "FollowFilter.h"
-
-#include "InverseKinematicsControl.h"
-
-#include "SafeCheck.h"
-
-#include "I2C_master.h"
-
-#define PRINT_DEBUG true
-#define NAVIG true
-
-#define STAB_ENABLE_FLAG 1
-#define NAVIG_ENABLE_FLAG 1
-#define ARM_ENABLE_FLAG 1
-
-
-class SystemControl
-{
-public:
- SystemControl();
- void initArm();
- void initRover();
-
- void initEthernet();
-
- void startControlThread();
- void updateEthCommunication();
-
- void stabilizationFunction();
- void navigationFunction();
- void armRoverKinematicControlFunction();
- //void schedulerThreadFunction();
- void printThreadFunction();
-
- float calcDroneDynamics();
- void toolControlFunction();
-
-
- Vector8f calcNullSpaceVelocity();
-
-
-
-
- double actualDtControlCycle;
-
- double t;
-
- bool ethDone;
- bool stabDone;
- bool navigDone;
- bool armDone;
- float dtPassedStab;
- float dtPassedArm;
- float dtPassedNavig;
- float dtPassedEth;
-
- float dtBoard;
- float t_temp = 0.0;
- bool ematClosed = false;
- float toolWheelsVel_d = 0.0;
-
-
-
-private:
-
- /*ead *stabilizationThread;
- Thread *navigationThread;
- Thread *armThread;
- Thread *schedulerThread;*/
- Thread *printThread;
-
- Timer wdTimer;
-
- Eth_tcp *eth_tcp;
-
- Rover *rover;
- ARAP180_WITH_ROVER *arm;
-
- I2C_master *i2c_tool;
-
- Servo *batteryServo;
-
-
- DigitalOut *ledEth;
- DigitalOut *ledError;
-
- Status robotStatus;
- ControlMode robotControlMode;
-
- Mutex data_mutex;
- Mutex arm_mutex;
-
- ConfigMsg robotCnf;
- ComandMsg robotCmd;
- ResponseMsg robotRsp;
-
- FollowFilter *followFilter;
-
- InverseKinematicsControl *inverseKinematicsControl;
-
- SafeCheck *safeCheck;
-
- float velAccSafetyFactor;
- float forceTorqueSafetyFactor;
- int precCmd;
-
- int invKin_n_max_iter;
- float invKin_max_pError;
- float invKin_max_oError;
-
- bool parametersConfigurated;
- bool motor_enabled;
-
- float wdTime;
- bool debug;
-
- float r_frontWheel;
- float r_retroWheel;
- float pipeCurve_I;
- float pipeCurve_E;
- float pipeCurve_M;
- float retroFrontCentralDistance;
- float wheelsCntactPointDistanceFromPipeCenter;
- float pipeRadious;
- float Kp_stabilization;
- float Kd_stabilization;
- float wheelsAcceleration;
-
- float dq_stab_int;
- float dq_stab;
- float q_stab;
-
- float Kp_zero_torque;
- float Ki_zero_torque;
-
- float pitch_m;
- float pitch_vel_m;
- float roll_m;
- float pitch_d;
-
- float vels_d;
- float velf_d;
- float vela_d;
- float velf_m;
- float vels_m;
- float vela_m;
-
- float roverLongitudinalIntegratedPosition;
-
- float front_vel_dx;
- float front_vel_sx;
- float retro_vel_dx;
- float retro_vel_sx;
- float forward_vel;
-
- float jointFront_d;
- float jointRetro_d;
- float roverLaserFrontDx;
- float roverLaserFrontSx;
- float roverLaserRetroDx;
- float roverLaserRetroSx;
- float roverFrontDistance;
- float roverRetroDistance;
-
-
-
- VectorXf q_cmd;
- VectorXf dq_cmd;
- VectorXf ddq_cmd;
- VectorXf first_q;
- Vector8f q_mis;
-
- Matrix4f T_e_b_m;
-
- VectorXf omega;
- VectorXf zita;
- VectorXf maxJerk;
- VectorXf maxAcc;
- VectorXf maxVel;
- VectorXf maxPos;
- VectorXf minPos;
-
- Vector3f maxCartPos;
- Vector3f minCartPos;
-
- float battery_pos;
- float battery_pos_saturated;
- float battery_pos_prec;
-
- Vector6f K_firstOrder;
- Vector6f K_secondOrder;
-
- Vector6f D_secondOrder;
-
- Vector8f WeightVector;
-
- Vector6f wrench_armAndRover;
- Vector6f wrench_system;
-
-
- Matrix<float, 24, 1> debugVector;
-
- Vector3f drone_orientation;
-
- void armEnableMotors();
- void armDisableMotors();
- void setResponse();
-
- void setConfigurationParameters();
-
- int printSchedulerCounter;
-
- float toolClampPos_d = -13.0;
-
-
- float toolWheelsPos_dx_mis, toolWheelsPos_sx_mis, toolWheelsSpeed_dx_mis, toolWheelsSpeed_sx_mis;
-
-
-};
-
-#endif
\ No newline at end of file