PRISMA Lab
prima prova bracctio
Dependencies: Eigen MX64_senzaCorrente
Revision 0:82a5f03b7eb4, committed 2019-11-18
- Comitter:
- anfontanelli
- Date:
- Mon Nov 18 17:50:32 2019 +0000
- Commit message:
- arm created
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/.gitignore Mon Nov 18 17:50:32 2019 +0000 @@ -0,0 +1,4 @@ +.build +.mbed +projectfiles +*.py*
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ARM/ARAP180.cpp Mon Nov 18 17:50:32 2019 +0000
@@ -0,0 +1,1393 @@
+#include "ARAP180.h"
+#include "ARM_parameters.h"
+
+//*****************************************************************************
+
+ARAP180::ARAP180(){
+
+ q_sat_max(0) = M_PI/2;
+ q_sat_max(1) = M_PI;
+ q_sat_max(2) = M_PI;
+ q_sat_max(3) = M_PI/4;
+ q_sat_max(4) = M_PI;
+ q_sat_max(5) = M_PI/4;
+
+ q_sat_min(0) = -M_PI/2;
+ q_sat_min(1) = -M_PI/6;
+ q_sat_min(2) = -0.1;
+ q_sat_min(3) = -M_PI;
+ q_sat_min(4) = -M_PI;
+ q_sat_min(5) = -M_PI/4;
+
+ ID[0] = 1;
+ ID[1] = 2;
+ ID[2] = 3;
+ ID[3] = 4;
+ ID[4] = 5;
+
+ operatingMode[0] = 3;
+ operatingMode[1] = 3;
+ operatingMode[2] = 3;
+ operatingMode[3] = 3;
+ operatingMode[4] = 3;
+
+ enableVal[0] = 1;
+ enableVal[1] = 1;
+ enableVal[2] = 1;
+ enableVal[3] = 1;
+ enableVal[4] = 1;
+
+ motorSign(0) = 1;
+ motorSign(1) = -1;
+ motorSign(2) = 1;
+ motorSign(3) = 1;
+ motorSign(4) = -1;
+ motorSign(5) = 1;
+
+ offsets(0) = 0.0;
+ offsets(1) = 0.0;
+ offsets(2) = 0.0;
+ offsets(3) = 0.0;
+ offsets(4) = 0.0;
+ offsets(5) = 0.0;
+
+ dxl_port = new UARTSerial_half(PB_9,PB_8,PF_13, 1000000);
+ mx_MotorChain = new MX(ID, sizeof(ID)/sizeof(int), 1000000, dxl_port);
+}
+
+
+void ARAP180::initArmMotors(){
+ mx_MotorChain->SyncOperatingMode(operatingMode, -1);
+
+ //Accelleration init
+ printf("\n Inizializzo i profili di Accelerazione ");
+ setMaxAcc(100);
+
+
+
+ printf("\n Inizializzo i profili di Velocita' ");
+ setMaxVel(20);
+
+ mx_MotorChain->SyncTorqueEnable(enableVal, -1);
+ printf("Motori inizializzati!!\r\n");
+
+
+}
+
+
+void ARAP180::setMaxVel(float maxVel){
+
+ float Vel_profile[5] = {maxVel,maxVel,maxVel,maxVel,maxVel};
+ mx_MotorChain->SyncProfileVel(Vel_profile);
+
+}
+
+void ARAP180::setMaxAcc(float maxAcc){
+
+ float Acc_profile[5] = {maxAcc,maxAcc,maxAcc,maxAcc,maxAcc};
+ mx_MotorChain->SyncProfileAccel(Acc_profile);
+
+
+}
+
+
+void ARAP180::setJointPos(Vector6f q){
+
+
+ float Goal_position[5];
+
+ Goal_position[0] = utilities::rad2deg(q(0)) * motorSign(0) + offsets(0);
+ Goal_position[1] = utilities::rad2deg(q(1)) * motorSign(1) + offsets(1);
+ Goal_position[2] = utilities::rad2deg(q(2)) * motorSign(2) + offsets(2);
+ Goal_position[3] = utilities::rad2deg(q(3)) * motorSign(3) + offsets(3);
+ Goal_position[4] = utilities::rad2deg(q(4)) * motorSign(4) + offsets(4);
+
+
+ mx_MotorChain->SyncSetGoal(Goal_position);
+
+
+}
+
+
+Vector6f ARAP180::getJointPos(){
+
+ Vector6f getPosConverted = Vector6f::Zero();
+ float getPos[5];
+
+ mx_MotorChain->SyncGetPosition(getPos);
+
+
+ getPosConverted(0) = utilities::deg2rad(getPos[0] - offsets(0)) * motorSign(0);
+ getPosConverted(1) = utilities::deg2rad(getPos[1] - offsets(1)) * motorSign(1);
+ getPosConverted(2) = utilities::deg2rad(getPos[2] - offsets(2)) * motorSign(2);
+ getPosConverted(3) = utilities::deg2rad(getPos[3] - offsets(3)) * motorSign(3);
+ getPosConverted(4) = utilities::deg2rad(getPos[4] - offsets(4)) * motorSign(4);
+ getPosConverted(5) = (0.0 - offsets(4)) * motorSign(5);
+
+ return getPosConverted;
+
+}
+
+
+
+
+
+
+//Dyrect Kinematics Matrix Te
+Matrix4f ARAP180::forwardKinematics(Vector6f q){
+
+ Matrix4f A0 = Matrix4f::Zero();
+ float q1 = q[0];
+ float q2 = q[1];
+ float q3 = q[2];
+ float q4 = q[3];
+ float q5 = q[4];
+ float q6 = q[5];
+ float t2 = cos(q1);
+ float t3 = cos(q5);
+ float t4 = cos(q6);
+ float t5 = sin(q1);
+ float t6 = sin(q2);
+ float t7 = sin(q5);
+ float t8 = sin(q6);
+ float t9 = q2*-1.0;
+ float t10 = q2*1.0;
+ float t12 = -q2;
+ float t13 = -q3;
+ float t11 = a2*t6;
+ float t14 = t3*t5;
+ float t15 = t2*t3*1.0;
+ float t16 = q3+q4+t9;
+ float t17 = q2+t13;
+ float t19 = q3+q4+t12;
+ float t18 = cos(t17);
+ float t20 = sin(t17);
+ float t21 = cos(t16);
+ float t22 = sin(t16);
+ float t23 = cos(t19);
+ float t24 = sin(t19);
+ float t25 = a3*t18;
+ float t26 = a3i*t20*-1.0;
+ float t27 = a3i*t20*1.0;
+ float t28 = a4*t23*1.0;
+ float t29 = d5*t24*-1.0;
+ float t30 = d5*t24*1.0;
+ float t31 = t2*t7*t21;
+ float t32 = t5*t7*t21*-1.0;
+ float t33 = t5*t7*t21*1.0;
+ float t34 = t14+t31;
+ float t35 = t15+t32;
+ float t36 = a1+t11+t25+t26+t28+t29;
+ A0(0,0) = t4*t34+t2*t8*t22;
+ A0(0,1) = t8*t34*-1.0+t2*t4*t22;
+ A0(0,2) = t5*t7*-1.0+t2*t3*t23;
+ A0(0,3) = t2*t36;
+ A0(1,0) = t4*t35*-1.0+t5*t8*t22;
+ A0(1,1) = t8*t35+t4*t5*t22;
+ A0(1,2) = t2*t7+t14*t23;
+ A0(1,3) = t5*t36;
+ A0(2,0) = t8*t23*-1.0+t4*t7*t24;
+ A0(2,1) = t4*t23*-1.0-t7*t8*t24*1.0;
+ A0(2,2) = t3*t24;
+ A0(2,3) = a3*t20*-1.0+a4*t24*1.0-a3i*t18*1.0+d5*t23+a2*cos(q2);
+ A0(3,3) = 1.0;
+
+return A0;
+}
+
+//Jacobian matrix J
+Matrix6f ARAP180::jacobianMatrix(Vector6f q){
+
+ Matrix6f A0 = Matrix6f::Zero();
+ float q1 = q[0];
+ float q2 = q[1];
+ float q3 = q[2];
+ float q4 = q[3];
+ float q5 = q[4];
+ float q6 = q[5];
+ float t2 = cos(q1);
+ float t3 = cos(q2);
+ float t4 = cos(q5);
+ float t5 = sin(q1);
+ float t6 = sin(q2);
+ float t7 = sin(q5);
+ float t12 = -q2;
+ float t13 = -q3;
+ float t8 = t2*-1.0;
+ float t9 = t2*1.0;
+ float t10 = a2*t3;
+ float t11 = a2*t6;
+ float t15 = q2+t13;
+ float t17 = q3+q4+t12;
+ float t14 = a1+t11;
+ float t16 = cos(t15);
+ float t18 = sin(t15);
+ float t19 = cos(t17);
+ float t20 = sin(t17);
+ float t21 = a3*t16;
+ float t22 = a3i*t16*-1.0;
+ float t23 = a3i*t16*1.0;
+ float t24 = a3*t18*-1.0;
+ float t25 = a3*t18*1.0;
+ float t26 = a3i*t18*-1.0;
+ float t27 = a3i*t18*1.0;
+ float t28 = d5*t19;
+ float t29 = a4*t19*1.0;
+ float t30 = a4*t20*1.0;
+ float t31 = d5*t20*-1.0;
+ float t32 = d5*t20*1.0;
+ float t33 = t28+t30;
+ float t34 = t14+t21+t26;
+ float t35 = t29+t34;
+ float t40 = t22+t24+t33;
+ float t36 = t2*t35;
+ float t37 = t5*t35;
+ float t41 = t10+t40;
+ float t42 = t31+t35;
+ float t38 = -t36;
+ float t39 = -t37;
+ float t43 = t2*t42;
+ float t44 = t5*t42;
+ float t45 = t38+t43;
+ float t46 = t39+t44;
+ A0(0,0) = -t44;
+ A0(0,1) = t2*t41;
+ A0(0,2) = -t8*(t23+t25-t33);
+ A0(0,3) = t8*t33;
+ A0(0,4) = t19*(t37-t44)-t5*t20*t28*1.0;
+ A0(1,0) = t43;
+ A0(1,1) = t5*t41*1.0;
+ A0(1,2) = t5*(t23+t25-t33);
+ A0(1,3) = -t5*t33;
+ A0(1,4) = -t19*(t36-t43)+t9*t20*t28;
+ A0(2,1) = -t2*(t43-a1*t2)-t5*(t44-a1*t5)*1.0;
+ A0(2,2) = t5*(t44-t5*t14)+t9*(t43-t2*t14);
+ A0(2,3) = t5*(t44-t5*t34)+t9*(t43-t2*t34);
+ A0(2,4) = t5*t20*(t36-t43)*-1.0+t9*t20*(t37-t44);
+ A0(3,1) = t5*-1.0;
+ A0(3,2) = t5;
+ A0(3,3) = t5;
+ A0(3,4) = t8*t20;
+ A0(3,5) = t5*t7*-1.0+t2*t4*t19;
+ A0(4,1) = t2;
+ A0(4,2) = t8;
+ A0(4,3) = t8;
+ A0(4,4) = t5*t20*-1.0;
+ A0(4,5) = t2*t7+t4*t5*t19;
+ A0(5,0) = 1.0;
+ A0(5,4) = t19;
+ A0(5,5) = t4*t20;
+return A0;
+}
+
+Vector6f ARAP180::backwardKinematics(Matrix4f Td, Vector6f q_init, float Tsam, int n_max_iter, float max_pError, float max_oError ,float _Kp, float _Ko){
+
+
+ Matrix3f Kp = _Kp*Matrix3f::Identity();
+ Matrix3f Ko = _Ko*Matrix3f::Identity();
+
+ Vector6f q = Vector6f::Zero();
+
+ Vector6f dq = Vector6f::Zero();
+ Vector6f dp = Vector6f::Zero();
+ Matrix6f W_inv = Matrix6f::Identity();
+
+ //Set destination frame
+
+ Matrix4f Te = Matrix4f::Zero();
+ Matrix6f J = Matrix6f::Zero();
+ Matrix6f pinvJ = Matrix6f::Zero();
+ Matrix6f J_T = Matrix6f::Zero();
+ Matrix3f L = Matrix3f::Zero();
+ Matrix3f pinvL = Matrix3f::Zero();
+ Vector3f vp = Vector3f::Zero();
+ Vector3f vo = Vector3f::Zero();
+ Vector6f v = Vector6f::Zero();
+
+
+ int n_iter = 0;
+
+ float pError = 0.0;
+ float oError = 0.0;
+ q = q_init;
+ Te = forwardKinematics(q);
+ J = jacobianMatrix(q);
+
+
+ Vector3f ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);
+ Vector3f 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));
+ //FullPivLU<Matrix3d> Core_L(L);
+ pinvL = L.inverse();
+
+
+ //vp = dp.block(0,0,3,1) + Kp*ep;
+ vp = Kp*ep;
+
+ vo = pinvL*(L.transpose()*dp.block(3,0,3,1) + Ko*eo);
+ //vo = (Ko*eo);
+ v << vp[0], vp[1], vp[2], vo[0], vo[1], vo[2];
+
+ //J_T = J.transpose();
+
+
+ // If we need a weighted pinv
+ //FullPivLU<Matrix6d> Core_J(J*W_inv*J_T);
+ //pinvJ = W_inv*J_T*Core_J.inverse();
+
+ //FullPivLU<Matrix6d> Core_J(J);
+
+ pinvJ = J.inverse();
+
+ dq = pinvJ*v;
+
+
+ q += dq*Tsam;
+
+
+ Te = forwardKinematics(q);
+
+
+ J = 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();
+ n_iter++;
+ }
+ //cout << "posError: " << pError << " orError: " << oError << endl;
+
+
+return q;
+
+
+}
+
+Vector6f ARAP180::jointSaturation(Vector6f q){
+
+
+ Vector6f q_sat = q;
+ for(int i = 0; i<6; i++){
+
+ if(q_sat(i) > q_sat_max(i)){ q_sat(i) = q_sat_max(i);}
+ if(q_sat(i) < q_sat_min(i)){ q_sat(i) = q_sat_min(i);}
+
+ }
+
+
+
+ return q_sat;
+
+
+}
+
+
+// Inertia matrix B
+
+Matrix6f ARAP180::inertiaMatrix(Vector6f q){
+
+ Matrix6f A0 = Matrix6f::Zero();
+ float q1 = q[0];
+ float q2 = q[1];
+ float q3 = q[2];
+ float q4 = q[3];
+ float q5 = q[4];
+ float q6 = q[5];
+
+ float t2 = cos(q2);
+ float t3 = cos(q3);
+ float t4 = cos(q4);
+ float t5 = cos(q5);
+ float t6 = cos(q6);
+ float t7 = sin(q2);
+ float t8 = sin(q3);
+ float t9 = sin(q4);
+ float t10 = sin(q5);
+ float t11 = sin(q6);
+ float t12 = q3+q4;
+ float t13 = q4+q6;
+ float t14 = a1*a1;
+ float t15 = a2*a2;
+ float t16 = a3*a3;
+ float t17 = a4*a4;
+ float t18 = a3i*a3i;
+ float t19 = d5*d5;
+ float t20 = q2*2.0;
+ float t21 = q3*2.0;
+ float t22 = q4*2.0;
+ float t32 = -d5;
+ float t33 = -q2;
+ float t35 = -q3;
+ float t37 = -q4;
+ float t39 = -q5;
+ float t40 = -q6;
+ float t23 = cos(t20);
+ float t24 = a2*t2;
+ float t25 = a3i*t4;
+ float t26 = a2*t7;
+ float t27 = a3*t9;
+ float t28 = a4*t11;
+ float t29 = cos(t12);
+ float t30 = q5+t13;
+ float t31 = sin(t12);
+ float t34 = -t20;
+ float t36 = -t21;
+ float t38 = -t22;
+ float t41 = a3*mpx4*t4;
+ float t42 = a2*mpy3*t3;
+ float t43 = a2*mpx3*t8;
+ float t45 = a3i*mpx4*t9;
+ float t46 = mass4*t16;
+ float t47 = mass5*t16;
+ float t48 = mass6*t16;
+ float t49 = mass5*t17;
+ float t50 = mass6*t17;
+ float t51 = mass4*t18;
+ float t52 = mass5*t18;
+ float t53 = mass6*t18;
+ float t54 = mass6*t19;
+ float t56 = d5*mpy6*t6*2.0;
+ float t58 = a3*a4*mass5*t4;
+ float t59 = a3*a4*mass6*t4;
+ float t60 = a2*a3i*mass4*t3;
+ float t61 = a2*a3i*mass5*t3;
+ float t62 = a2*a3i*mass6*t3;
+ float t65 = a2*a3*mass4*t8;
+ float t66 = a2*a3*mass5*t8;
+ float t67 = a2*a3*mass6*t8;
+ float t68 = a4*a3i*mass5*t9;
+ float t69 = a4*a3i*mass6*t9;
+ float t71 = a3*t4*t11;
+ float t72 = d5*t6*t10;
+ float t73 = a3i*t9*t11;
+ float t75 = q2+t35;
+ float t76 = q4+t40;
+ float t83 = t12+t33;
+ float t86 = t13+t39;
+ float t87 = q5+q6+t37;
+ float t108 = a2*mpx4*t3*t9;
+ float t109 = a2*mpx4*t4*t8;
+ float t111 = d5*mpy6*t5*t11;
+ float t113 = t20+t35;
+ float t115 = a2*d5*mass6*t3*t4;
+ float t116 = a2*a4*mass5*t3*t9;
+ float t117 = a2*a4*mass5*t4*t8;
+ float t118 = a2*a4*mass6*t3*t9;
+ float t119 = a2*a4*mass6*t4*t8;
+ float t121 = a2*d5*mass6*t8*t9;
+ float t150 = a2*mpy6*t3*t4*t6;
+ float t153 = a2*mpy6*t6*t8*t9;
+ float t167 = a2*mass6*t3*t4*t32;
+ float t182 = a2*mpy6*t3*t9*t10*t11;
+ float t183 = a2*mpy6*t4*t8*t10*t11;
+ float t197 = q6-t12*2.0+t20;
+ float t44 = mpy6*t28;
+ float t55 = t41*2.0;
+ float t57 = t45*2.0;
+ float t63 = a2*t29;
+ float t64 = d5*mass6*t25;
+ float t70 = d5*mass6*t27;
+ float t74 = -t25;
+ float t77 = -t27;
+ float t78 = -t41;
+ float t80 = -t42;
+ float t81 = cos(t75);
+ float t82 = -t56;
+ float t84 = t12+t34;
+ float t85 = q5+t76;
+ float t88 = -t43;
+ float t89 = -t45;
+ float t91 = sin(t75);
+ float t92 = t58*2.0;
+ float t93 = t59*2.0;
+ float t95 = t68*2.0;
+ float t96 = t69*2.0;
+ float t98 = -t46;
+ float t99 = -t47;
+ float t100 = -t48;
+ float t101 = -t49;
+ float t102 = -t50;
+ float t103 = -t51;
+ float t104 = -t52;
+ float t105 = -t53;
+ float t106 = -t54;
+ float t107 = mpy6*t6*t25;
+ float t110 = mpy6*t6*t27;
+ float t112 = mpy6*t72;
+ float t114 = t20+t36;
+ float t120 = t6*t10*t25;
+ float t122 = t6*t10*t27;
+ float t123 = -t58;
+ float t125 = -t59;
+ float t127 = mass6*t25*t32;
+ float t129 = -t65;
+ float t130 = -t66;
+ float t131 = -t67;
+ float t132 = -t68;
+ float t134 = -t69;
+ float t136 = cos(t83);
+ float t137 = mass6*t27*t32;
+ float t139 = sin(t83);
+ float t141 = -t73;
+ float t145 = cos(t113);
+ float t148 = sin(t113);
+ float t154 = mpy6*t10*t71;
+ float t159 = mpy6*t10*t73;
+ float t160 = -t108;
+ float t161 = -t109;
+ float t169 = -t116;
+ float t170 = -t117;
+ float t171 = -t118;
+ float t172 = -t119;
+ float t185 = -t153;
+ float t196 = q6-t12+t113;
+ float t198 = t25+t27+t32;
+ float t203 = q5+t197;
+ float t79 = -t55;
+ float t90 = -t57;
+ float t94 = t64*2.0;
+ float t97 = t70*2.0;
+ float t124 = -t92;
+ float t126 = -t93;
+ float t133 = -t95;
+ float t135 = -t96;
+ float t140 = sin(t84);
+ float t142 = t107*2.0;
+ float t143 = t110*2.0;
+ float t144 = t10*t44*2.0;
+ float t146 = cos(t114);
+ float t147 = q3+t84;
+ float t149 = sin(t114);
+ float t151 = a3*t81;
+ float t152 = a3i*t81;
+ float t155 = a3*t91;
+ float t156 = a3i*t91;
+ float t157 = mpy6*t6*t74;
+ float t162 = mpy6*t6*t77;
+ float t165 = a4*t136;
+ float t166 = d5*t136;
+ float t168 = a4*t139;
+ float t173 = d5*t139;
+ float t174 = t6*t10*t74;
+ float t175 = t6*t10*t77;
+ float t176 = t154*2.0;
+ float t178 = t159*2.0;
+ float t186 = -t154;
+ float t188 = t12+t84;
+ float t191 = mpy6*t10*t141;
+ float t195 = t32*t139;
+ float t202 = q5+t196;
+ float t206 = mpy6*t5*t11*t198;
+ float t207 = d5+t63+t74+t77;
+ float t128 = -t94;
+ float t138 = -t97;
+ float t158 = -t142;
+ float t163 = -t143;
+ float t164 = -t144;
+ float t177 = cos(t147);
+ float t179 = q6+t147;
+ float t180 = sin(t147);
+ float t184 = -t152;
+ float t187 = -t176;
+ float t189 = -t155;
+ float t190 = -t156;
+ float t192 = -t178;
+ float t193 = cos(t188);
+ float t194 = q6+t188;
+ float t200 = q5+t40+t147;
+ float t204 = q5+t40+t188;
+ float t208 = t166+t168;
+ float t209 = -t206;
+ float t210 = mpy6*t5*t11*t207;
+ float t214 = t28+t71+t72+t141+t174+t175;
+ float t218 = -mpy6*t5*t11*(t152+t155-t168+t32*t136);
+ float t225 = t45+t56+t64+t68+t69+t70+t78+t101+t102+t106+t121+t123+t125+t144+t150+t154+t157+t160+t161+t162+t167+t169+t170+t171+t172+t182+t183+t185+t191;
+ float t181 = q5+t179;
+ float t199 = q5+t194;
+ float t201 = t39+t179;
+ float t205 = t39+t194;
+ float t211 = mpy6*t5*t11*t208;
+ float t212 = -t210;
+ float t213 = t184+t189+t208;
+ float t216 = mpy6*t214;
+ float t217 = a1+t26+t151+t165+t190+t195;
+ float t224 = t41+t49+t50+t54+t58+t59+t82+t89+t107+t110+t127+t132+t134+t137+t159+t164+t186;
+ float t226 = t56+t57+t60+t61+t62+t79+t80+t88+t94+t95+t96+t97+t98+t99+t100+t101+t102+t103+t104+t105+t106+t121+t124+t126+t129+t130+t131+t144+t150+t158+t160+t161+t163+t167+t169+t170+t171+t172+t176+t182+t183+t185+t192;
+ float t215 = t24+t213;
+ float t220 = mpy6*t10*t11*t217;
+ float t221 = mpy6*t5*t6*t217;
+ float t219 = mpy6*t5*t11*t215;
+ float t223 = -t220;
+ float t222 = -t219;
+ A0(0,0) = t41+t42+t43+t46/2.0+t47/2.0+t48/2.0+t49/2.0+t50/2.0+t51/2.0+t52/2.0+t53/2.0+t54/2.0+t58+t59-t60-t61-t62+t65+t66+t67+t89+t127+t132+t134+t137+mass2*t14+mass3*t14+(mass3*t15)/2.0+mass4*t14+(mass4*t15)/2.0+mass5*t14+(mass5*t15)/2.0+mass6*t14+(mass6*t15)/2.0+(t46*t146)/2.0+(t47*t146)/2.0+(t48*t146)/2.0-(t51*t146)/2.0-(t52*t146)/2.0-(t53*t146)/2.0+(t49*t193)/2.0+(t50*t193)/2.0-(t54*t193)/2.0+(a3*mpy6*cos(t30))/4.0-(a3*mpy6*cos(t85))/4.0-(a3*mpy6*cos(t86))/4.0+(a3*mpy6*cos(t87))/4.0+(a3*mpy6*cos(t181))/4.0-(a3*mpy6*cos(t200))/4.0+(a4*mpy6*cos(t199))/4.0-(a3*mpy6*cos(t201))/4.0+(a3*mpy6*cos(t202))/4.0+(a4*mpy6*cos(t203))/4.0-(a4*mpy6*cos(t204))/4.0-(a4*mpy6*cos(t205))/4.0+(a3i*mpy6*cos(t13))/2.0+(a3i*mpy6*cos(t76))/2.0-(a3i*mpy6*cos(t179))/2.0-(a3i*mpy6*cos(t196))/2.0+(d5*mpy6*cos(t194))/2.0+(d5*mpy6*cos(t197))/2.0+(a3*mpy6*sin(t13))/2.0+(a3*mpy6*sin(t76))/2.0+(a3*mpy6*sin(t179))/2.0+(a4*mpy6*sin(t194))/2.0-(a3*mpy6*sin(t196))/2.0-(a4*mpy6*sin(t197))/2.0-(a3i*mpy6*sin(t30))/4.0+(a3i*mpy6*sin(t85))/4.0+(a3i*mpy6*sin(t86))/4.0+(a3i*mpy6*sin(t87))/4.0+(a3i*mpy6*sin(t181))/4.0-(a3i*mpy6*sin(t200))/4.0-(a3i*mpy6*sin(t201))/4.0-(a3i*mpy6*sin(t202))/4.0+(a1*mpy6*cos(q2+q5+q6-t12))/2.0-(d5*mpy6*sin(t199))/4.0+(d5*mpy6*sin(t203))/4.0+(d5*mpy6*sin(t204))/4.0+(d5*mpy6*sin(t205))/4.0+(a2*mpy6*sin(q5+q6-t12+t20))/4.0+(a1*mpy6*cos(q5+q6+t83))/2.0-(a1*mpy6*cos(q5+t40+t83))/2.0-(a1*mpy6*cos(q6+t39+t83))/2.0+(a2*mpy6*sin(q5+q6+t12))/4.0-(a2*mpy6*sin(q5+q6+t84))/4.0-(a2*mpy6*sin(q5+t12+t40))/4.0-(a2*mpy6*sin(q6+t12+t39))/4.0+(a2*mpy6*sin(q5+t40+t84))/4.0+(a2*mpy6*sin(q6+t39+t84))/4.0+(a2*mpy6*sin(t12+t39+t40))/4.0+a1*mass3*t26*2.0+a1*mass4*t26*2.0+a1*mass5*t26*2.0+a1*mass6*t26*2.0+a1*mass4*t151*2.0+a1*mass5*t151*2.0+a1*mass6*t151*2.0-a1*mass4*t156*2.0-a1*mass5*t156*2.0-a1*mass6*t156*2.0+a1*mass5*t165*2.0+a1*mass6*t165*2.0-a1*mass6*t173*2.0+a1*mpx2*t7*2.0+a2*mpx4*t31+a1*mpx3*t81*2.0+a1*mpx4*t136*2.0-a2*mpx4*t140+a2*mpx3*t148+a3*mpx4*t177-a1*mpy2*t2*2.0+a1*mpy3*t91*2.0-a2*mpy3*t145+a3i*mpx4*t180+d5*mass6*t63+(a2*mpy6*cos(q6-t12+t20))/2.0-(mass3*t15*t23)/2.0-(mass4*t15*t23)/2.0-(mass5*t15*t23)/2.0-(mass6*t15*t23)/2.0+mpy6*t6*t32-a1*mpy6*sin(q2+q6-t12)+(a4*mpy6*cos(q5+q6))/2.0-(a2*mpy6*cos(q6+t12))/2.0-(a4*mpy6*cos(q5+t40))/2.0+(a2*mpy6*cos(q6+t84))/2.0-(a2*mpy6*cos(t12+t40))/2.0+a1*mpy6*sin(q6+t83)+a2*a4*mass5*t31+a2*a4*mass6*t31-a2*a4*mass5*t140-a2*a4*mass6*t140+a2*a3*mass4*t148+a2*a3*mass5*t148+a2*a3*mass6*t148+a3*a4*mass5*t177+a3*a4*mass6*t177+a2*a3i*mass4*t145+a2*a3i*mass5*t145+a2*a3i*mass6*t145-a3*a3i*mass4*t149-a3*a3i*mass5*t149-a3*a3i*mass6*t149+a4*a3i*mass5*t180+a4*a3i*mass6*t180+a3i*d5*mass6*t177+a3*mass6*t32*t180+a2*mass6*t32*cos(t84)+a4*mass6*t32*sin(t188);
+ A0(0,1) = t222;
+ A0(0,2) = t218;
+ A0(0,3) = t211;
+ A0(0,4) = t223;
+ A0(0,5) = t221;
+ A0(1,0) = t222;
+ A0(1,1) = t42*2.0+t43*2.0+t46+t47+t48+t49+t50+t51+t52+t53+t54+t55-t60*2.0-t61*2.0-t62*2.0+t65*2.0+t66*2.0+t67*2.0+t82+t90+t92+t93+t108*2.0+t109*2.0+t115*2.0+t116*2.0+t117*2.0+t118*2.0+t119*2.0-t121*2.0+t128+t133+t135+t138+t142+t143-t150*2.0+t153*2.0+t164+t178-t182*2.0-t183*2.0+t187+mass3*t15+mass4*t15+mass5*t15+mass6*t15;
+ A0(1,2) = t226;
+ A0(1,3) = t225;
+ A0(1,4) = t212;
+ A0(1,5) = -t44-mpy6*t71+mpy6*t73+t10*t107+t10*t110-t10*t150+t10*t153+mpy6*t6*t10*t32-a2*mpy6*t3*t9*t11-a2*mpy6*t4*t8*t11;
+ A0(2,0) = t218;
+ A0(2,1) = t226;
+ A0(2,2) = t46+t47+t48+t49+t50+t51+t52+t53+t54+t55+t82+t90+t92+t93+t128+t133+t135+t138+t142+t143+t164+t178+t187;
+ A0(2,3) = t224;
+ A0(2,4) = t209;
+ A0(2,5) = t216;
+ A0(3,0) = t211;
+ A0(3,1) = t225;
+ A0(3,2) = t224;
+ A0(3,3) = t49+t50+t54+t82+t164;
+ A0(3,4) = t111;
+ A0(3,5) = t44+t112;
+ A0(4,0) = t223;
+ A0(4,1) = t212;
+ A0(4,2) = t209;
+ A0(4,3) = t111;
+ A0(5,0) = t221;
+ A0(5,1) = -mpy6*(t214+a2*t3*t9*t11+a2*t4*t8*t11+a2*t3*t4*t6*t10-a2*t6*t8*t9*t10);
+ A0(5,2) = t216;
+ A0(5,3) = mpy6*(t28+t72);
+return A0;
+}
+
+
+
+// Inertia matrix B
+Vector6f ARAP180::gravityVector(Vector6f q){
+
+ Vector6f A0 = Vector6f::Zero();
+ float q1 = q[0];
+ float q2 = q[1];
+ float q3 = q[2];
+ float q4 = q[3];
+ float q5 = q[4];
+ float q6 = q[5];
+
+ float t2 = sin(q2);
+ float t3 = sin(q6);
+ float t4 = -q2;
+ float t5 = -q3;
+ float t6 = -q4;
+ float t7 = -q5;
+ float t8 = -q6;
+ float t9 = q2+t5;
+ float t11 = q3+q4+t4;
+ float t10 = cos(t9);
+ float t12 = sin(t9);
+ float t13 = cos(t11);
+ float t14 = q6+t11;
+ float t15 = sin(t11);
+ float t19 = q6+t6+t9;
+ float t21 = q5+t8+t11;
+ float t16 = q5+t14;
+ float t17 = sin(t14);
+ float t20 = q5+t19;
+ float t22 = t7+t14;
+ float t23 = sin(t19);
+ float t25 = cos(t21);
+ float t27 = mpx3*t10*9.814;
+ float t28 = mpy3*t12*9.814;
+ float t29 = a3*mass4*t10*9.814;
+ float t30 = a3*mass5*t10*9.814;
+ float t31 = a3*mass6*t10*9.814;
+ float t32 = a3i*mass4*t12*9.814;
+ float t33 = a3i*mass5*t12*9.814;
+ float t34 = a3i*mass6*t12*9.814;
+ float t35 = mpx4*t13*9.814;
+ float t36 = a4*mass5*t13*9.814;
+ float t37 = a4*mass6*t13*9.814;
+ float t38 = d5*mass6*t15*9.814;
+ float t18 = cos(t16);
+ float t24 = cos(t20);
+ float t26 = cos(t22);
+ float t39 = -t38;
+ float t40 = mpy6*t17*4.907;
+ float t42 = mpy6*t23*4.907;
+ float t45 = mpy6*t25*2.4535;
+ float t41 = mpy6*t18*2.4535;
+ float t43 = -t42;
+ float t44 = mpy6*t24*2.4535;
+ float t46 = mpy6*t26*2.4535;
+ float t47 = -t45;
+ float t48 = -t46;
+ A0(1) = -t27-t28-t29-t30-t31+t32+t33+t34-t35-t36-t37+t38-t40-t41+t42-t44+t45+t46-mpx2*t2*9.814+mpy2*cos(q2)*9.814-a2*mass3*t2*9.814-a2*mass4*t2*9.814-a2*mass5*t2*9.814-a2*mass6*t2*9.814;
+ A0(2) = t27+t28+t29+t30+t31-t32-t33-t34+t35+t36+t37+t39+t40+t41+t43+t44+t47+t48;
+ A0(3) = t35+t36+t37+t39+t40+t41+t43+t44+t47+t48;
+ A0(4) = mpy6*t3*(sin(q5+t6+t9)-sin(q5+t11))*4.907;
+ A0(5) = mpy6*(t3*t13*9.814-t15*cos(q6)*sin(q5)*9.814);
+return A0;
+}
+
+
+Vector6f ARAP180::frictionVector(Vector6f dq){
+
+ Vector6f A0 = Vector6f::Zero();
+ float dq1 = dq[0];
+ float dq2 = dq[1];
+ float dq3 = dq[2];
+ float dq4 = dq[3];
+ float dq5 = dq[4];
+ float dq6 = dq[5];
+
+ A0(0) = Fv1*dq1+Fs1*((dq1/fabs(dq1)));
+ A0(1) = Fv2*dq2+Fs2*((dq2/fabs(dq2)));
+ A0(2) = Fv3*dq3+Fs3*((dq3/fabs(dq3)));
+ A0(3) = Fv4*dq4+Fs4*((dq4/fabs(dq4)));
+ A0(4) = Fv5*dq5+Fs5*((dq5/fabs(dq5)));
+ A0(5) = Fv6*dq6+Fs6*((dq6/fabs(dq6)));
+return A0;
+
+}
+
+Vector6f ARAP180::stiffnessVector(Vector6f q){
+
+ Vector6f A0 = Vector6f::Zero();
+ float q1 = q[0];
+ float q2 = q[1];
+ float q3 = q[2];
+ float q4 = q[3];
+ float q5 = q[4];
+ float q6 = q[5];
+
+ A0(1) = O2+K2*q2;
+ A0(2) = O3+K3*q3;
+ A0(3) = O4+K4*q4;
+return A0;
+
+}
+
+/*
+Matrix6f ARAP180::coriolisCentrifugalMatrix(Vector6f q, Vector6f dq){
+
+ Matrix6f A0 = Matrix6f::Zero();
+ float q1 = q[0];
+ float q2 = q[1];
+ float q3 = q[2];
+ float q4 = q[3];
+ float q5 = q[4];
+ float q6 = q[5];
+ float dq1 = dq[0];
+ float dq2 = dq[1];
+ float dq3 = dq[2];
+ float dq4 = dq[3];
+ float dq5 = dq[4];
+ float dq6 = dq[5];
+
+ float t2 = cos(q2);
+ float t3 = cos(q3);
+ float t4 = cos(q4);
+ float t5 = cos(q5);
+ float t6 = cos(q6);
+ float t7 = sin(q2);
+ float t8 = sin(q3);
+ float t9 = sin(q4);
+ float t10 = sin(q5);
+ float t11 = sin(q6);
+ float t12 = q3+q4;
+ float t13 = q4+q6;
+ float t14 = q5+q6;
+ float t15 = a2*a2;
+ float t16 = a3*a3;
+ float t17 = a4*a4;
+ float t18 = a3i*a3i;
+ float t19 = d5*d5;
+ float t20 = q2*2.0;
+ float t21 = q3*2.0;
+ float t22 = q4*2.0;
+ float t36 = -q2;
+ float t38 = -q3;
+ float t40 = -q4;
+ float t42 = -q5;
+ float t43 = -q6;
+ float t23 = cos(t20);
+ float t24 = cos(t21);
+ float t25 = cos(t22);
+ float t26 = sin(t20);
+ float t27 = sin(t21);
+ float t28 = sin(t22);
+ float t29 = cos(t12);
+ float t30 = cos(t13);
+ float t31 = q6+t12;
+ float t32 = q5+t13;
+ float t33 = sin(t12);
+ float t34 = sin(t13);
+ float t35 = sin(t14);
+ float t37 = -t20;
+ float t39 = -t21;
+ float t41 = -t22;
+ float t45 = t12+t14;
+ float t48 = a4*dq4*t5;
+ float t49 = d5*dq2*t11*2.0;
+ float t50 = d5*dq3*t11*2.0;
+ float t51 = d5*dq4*t11*2.0;
+ float t52 = dq6*t5*t6;
+ float t54 = dq5*t10*t11;
+ float t55 = q2+t38;
+ float t56 = q4+t43;
+ float t57 = q5+t43;
+ float t61 = t12+t36;
+ float t63 = q3+q5+t36;
+ float t65 = t12+t43;
+ float t67 = t13+t42;
+ float t68 = t14+t40;
+ float t72 = a3i*dq2*mpx4*t4*2.0;
+ float t73 = a3i*dq3*mpx4*t4*2.0;
+ float t74 = a3i*dq4*mpx4*t4*2.0;
+ float t75 = a3*dq2*mpx4*t9*2.0;
+ float t76 = a3*dq3*mpx4*t9*2.0;
+ float t77 = a3*dq4*mpx4*t9*2.0;
+ float t81 = d5*dq6*mpy6*t11*2.0;
+ float t82 = t20+t38;
+ float t98 = d5*dq2*mpy6*t11*-2.0;
+ float t103 = q3+t14+t36;
+ float t116 = a4*a3i*dq2*mass5*t4*2.0;
+ float t117 = a4*a3i*dq2*mass6*t4*2.0;
+ float t118 = a4*a3i*dq3*mass5*t4*2.0;
+ float t119 = a4*a3i*dq3*mass6*t4*2.0;
+ float t120 = a4*a3i*dq4*mass5*t4*2.0;
+ float t121 = a4*a3i*dq4*mass6*t4*2.0;
+ float t122 = a3*d5*dq2*mass6*t4*2.0;
+ float t123 = a3*d5*dq3*mass6*t4*2.0;
+ float t124 = a3*d5*dq4*mass6*t4*2.0;
+ float t125 = a3*a4*dq2*mass5*t9*2.0;
+ float t126 = a3*a4*dq2*mass6*t9*2.0;
+ float t127 = a3*a4*dq3*mass5*t9*2.0;
+ float t128 = a3*a4*dq3*mass6*t9*2.0;
+ float t129 = a3*a4*dq4*mass5*t9*2.0;
+ float t130 = a3*a4*dq4*mass6*t9*2.0;
+ float t131 = a3i*d5*dq2*mass6*t9*2.0;
+ float t132 = a3i*d5*dq3*mass6*t9*2.0;
+ float t133 = a3i*d5*dq4*mass6*t9*2.0;
+ float t134 = a4*dq2*t6*t10*2.0;
+ float t135 = a4*dq3*t6*t10*2.0;
+ float t136 = a4*dq4*t6*t10*2.0;
+ float t137 = a2*dq2*mpx4*t3*t4*2.0;
+ float t138 = a2*dq3*mpx4*t3*t4*2.0;
+ float t139 = a2*dq4*mpx4*t3*t4*2.0;
+ float t140 = a3*dq2*mpy6*t4*t6*2.0;
+ float t141 = a3*dq3*mpy6*t4*t6*2.0;
+ float t142 = a3*dq4*mpy6*t4*t6*2.0;
+ float t143 = d5*dq5*mpy6*t5*t6*2.0;
+ float t145 = a4*dq2*mpy6*t5*t11*2.0;
+ float t147 = a4*dq3*mpy6*t5*t11*2.0;
+ float t151 = a4*dq5*mpy6*t5*t11*2.0;
+ float t152 = a4*dq6*mpy6*t6*t10*2.0;
+ float t153 = a3i*dq2*mpy6*t4*t11*2.0;
+ float t154 = a3i*dq2*mpy6*t6*t9*2.0;
+ float t155 = a3i*dq3*mpy6*t4*t11*2.0;
+ float t156 = a3i*dq3*mpy6*t6*t9*2.0;
+ float t157 = a3i*dq4*mpy6*t4*t11*2.0;
+ float t158 = a3i*dq4*mpy6*t6*t9*2.0;
+ float t159 = a3i*dq6*mpy6*t4*t11*2.0;
+ float t160 = a2*dq2*mpx4*t8*t9*2.0;
+ float t161 = a2*dq3*mpx4*t8*t9*2.0;
+ float t162 = a2*dq4*mpx4*t8*t9*2.0;
+ float t163 = a3*dq2*mpy6*t9*t11*2.0;
+ float t164 = a3*dq3*mpy6*t9*t11*2.0;
+ float t165 = a3*dq4*mpy6*t9*t11*2.0;
+ float t166 = a3*dq6*mpy6*t9*t11*2.0;
+ float t175 = a3i*dq1*t2*t3*t10;
+ float t176 = a3*dq1*t2*t8*t10;
+ float t177 = a3*dq1*t3*t7*t10;
+ float t188 = a3i*dq1*t7*t8*t10;
+ float t215 = a4*dq3*mpy6*t6*t10*-2.0;
+ float t217 = a4*dq4*mpy6*t6*t10*-2.0;
+ float t229 = a2*a4*dq2*mass5*t3*t4*2.0;
+ float t230 = a2*a4*dq2*mass6*t3*t4*2.0;
+ float t231 = a2*a4*dq3*mass5*t3*t4*2.0;
+ float t232 = a2*a4*dq3*mass6*t3*t4*2.0;
+ float t233 = a2*a4*dq4*mass5*t3*t4*2.0;
+ float t234 = a2*a4*dq4*mass6*t3*t4*2.0;
+ float t235 = a2*d5*dq2*mass6*t3*t9*2.0;
+ float t236 = a2*d5*dq2*mass6*t4*t8*2.0;
+ float t237 = a2*d5*dq3*mass6*t3*t9*2.0;
+ float t238 = a2*d5*dq3*mass6*t4*t8*2.0;
+ float t239 = a2*d5*dq4*mass6*t3*t9*2.0;
+ float t240 = a2*d5*dq4*mass6*t4*t8*2.0;
+ float t241 = a2*a4*dq2*mass5*t8*t9*2.0;
+ float t242 = a2*a4*dq2*mass6*t8*t9*2.0;
+ float t243 = a2*a4*dq3*mass5*t8*t9*2.0;
+ float t244 = a2*a4*dq3*mass6*t8*t9*2.0;
+ float t245 = a2*a4*dq4*mass5*t8*t9*2.0;
+ float t246 = a2*a4*dq4*mass6*t8*t9*2.0;
+ float t250 = a3i*dq5*mpy6*t4*t5*t6*2.0;
+ float t252 = a2*dq2*mpy6*t3*t6*t9*2.0;
+ float t253 = a2*dq2*mpy6*t4*t6*t8*2.0;
+ float t254 = a2*dq3*mpy6*t3*t6*t9*2.0;
+ float t255 = a2*dq3*mpy6*t4*t6*t8*2.0;
+ float t256 = a2*dq4*mpy6*t3*t6*t9*2.0;
+ float t257 = a2*dq4*mpy6*t4*t6*t8*2.0;
+ float t258 = a3*dq2*mpy6*t4*t5*t11*2.0;
+ float t260 = a2*dq6*mpy6*t3*t4*t11*2.0;
+ float t261 = a3*dq3*mpy6*t4*t5*t11*2.0;
+ float t263 = a3*dq4*mpy6*t4*t5*t11*2.0;
+ float t265 = a3*dq5*mpy6*t4*t5*t11*2.0;
+ float t266 = a3*dq5*mpy6*t5*t6*t9*2.0;
+ float t267 = a3*dq6*mpy6*t4*t6*t10*2.0;
+ float t270 = a3i*dq2*mpy6*t5*t9*t11*2.0;
+ float t273 = a3i*dq3*mpy6*t5*t9*t11*2.0;
+ float t276 = a3i*dq4*mpy6*t5*t9*t11*2.0;
+ float t278 = a3i*dq5*mpy6*t5*t9*t11*2.0;
+ float t279 = a3i*dq6*mpy6*t6*t9*t10*2.0;
+ float t281 = a2*dq6*mpy6*t8*t9*t11*2.0;
+ float t287 = d5*dq1*t2*t3*t4*t10;
+ float t288 = a4*dq1*t2*t3*t9*t10;
+ float t289 = a4*dq1*t2*t4*t8*t10;
+ float t290 = a4*dq1*t3*t4*t7*t10;
+ float t291 = d5*dq1*t2*t8*t9*t10;
+ float t292 = d5*dq1*t3*t7*t9*t10;
+ float t293 = d5*dq1*t4*t7*t8*t10;
+ float t298 = a4*dq1*t7*t8*t9*t10;
+ float t306 = q2+q5-t12;
+ float t307 = q2+q6-t12;
+ float t310 = t12-t14;
+ float t315 = a3i*dq4*mpy6*t4*t10*t11*-2.0;
+ float t319 = a3*dq4*mpy6*t9*t10*t11*-2.0;
+ float t335 = a3i*dq1*t2*t3*t5*t6*2.0;
+ float t336 = a3*dq1*t2*t5*t6*t8*2.0;
+ float t337 = a3*dq1*t3*t5*t6*t7*2.0;
+ float t338 = a3i*dq1*t5*t6*t7*t8*2.0;
+ float t344 = a2*dq2*mpy6*t3*t4*t10*t11*2.0;
+ float t345 = a2*dq3*mpy6*t3*t4*t10*t11*2.0;
+ float t346 = a2*dq4*mpy6*t3*t4*t10*t11*2.0;
+ float t347 = a2*dq5*mpy6*t3*t5*t9*t11*2.0;
+ float t348 = a2*dq5*mpy6*t4*t5*t8*t11*2.0;
+ float t349 = a2*dq6*mpy6*t3*t6*t9*t10*2.0;
+ float t350 = a2*dq6*mpy6*t4*t6*t8*t10*2.0;
+ float t360 = a2*dq2*mpy6*t8*t9*t10*t11*2.0;
+ float t361 = a2*dq3*mpy6*t8*t9*t10*t11*2.0;
+ float t362 = a2*dq4*mpy6*t8*t9*t10*t11*2.0;
+ float t364 = q6-t12+t20;
+ float t398 = d5*dq1*t2*t3*t4*t5*t6*2.0;
+ float t399 = a4*dq1*t2*t3*t5*t6*t9*2.0;
+ float t400 = a4*dq1*t2*t4*t5*t6*t8*2.0;
+ float t401 = a4*dq1*t3*t4*t5*t6*t7*2.0;
+ float t403 = d5*dq1*t2*t5*t6*t8*t9*2.0;
+ float t404 = d5*dq1*t3*t5*t6*t7*t9*2.0;
+ float t405 = d5*dq1*t4*t5*t6*t7*t8*2.0;
+ float t408 = a4*dq1*t5*t6*t7*t8*t9*2.0;
+ float t474 = a4*dq1*mpy6*t3*t4*t5*t6*t7*-2.0;
+ float t481 = d5*dq1*mpy6*t2*t5*t6*t8*t9*-2.0;
+ float t44 = cos(t32);
+ float t46 = sin(t31);
+ float t47 = sin(t32);
+ float t53 = cos(t45);
+ float t58 = cos(t55);
+ float t59 = cos(t56);
+ float t60 = -t49;
+ float t62 = q5+t55;
+ float t64 = t12+t37;
+ float t66 = q5+t56;
+ float t69 = sin(t55);
+ float t70 = sin(t56);
+ float t71 = sin(t57);
+ float t78 = mpy6*t49;
+ float t79 = mpy6*t50;
+ float t80 = mpy6*t51;
+ float t83 = t20+t39;
+ float t84 = a2*dq1*mpx4*t29;
+ float t85 = -t72;
+ float t86 = -t73;
+ float t87 = -t74;
+ float t88 = cos(t61);
+ float t90 = cos(t63);
+ float t93 = cos(t67);
+ float t94 = cos(t68);
+ float t95 = -t75;
+ float t96 = -t76;
+ float t97 = -t77;
+ float t99 = -t81;
+ float t100 = q5+t61;
+ float t101 = t31+t36;
+ float t102 = t14+t55;
+ float t104 = t31+t37;
+ float t105 = t12+t57;
+ float t106 = t31+t42;
+ float t107 = sin(t61);
+ float t109 = sin(t63);
+ float t111 = sin(t65);
+ float t113 = sin(t67);
+ float t114 = sin(t68);
+ float t115 = -t54;
+ float t144 = d5*mpy6*t52*2.0;
+ float t146 = mpy6*t134;
+ float t148 = mpy6*t135;
+ float t149 = mpy6*t11*t48*2.0;
+ float t150 = mpy6*t136;
+ float t167 = cos(t82);
+ float t170 = sin(t82);
+ float t172 = a2*a4*dq1*mass5*t29;
+ float t173 = a2*a4*dq1*mass6*t29;
+ float t174 = a2*d5*dq1*mass6*t33;
+ float t179 = -t116;
+ float t180 = -t117;
+ float t181 = -t118;
+ float t182 = -t119;
+ float t183 = -t120;
+ float t184 = -t121;
+ float t185 = -t122;
+ float t186 = -t123;
+ float t187 = -t124;
+ float t190 = -t125;
+ float t191 = -t126;
+ float t192 = -t127;
+ float t193 = -t128;
+ float t194 = -t129;
+ float t195 = -t130;
+ float t196 = -t133;
+ float t197 = -t135;
+ float t198 = -t136;
+ float t202 = cos(t103);
+ float t205 = t36+t45;
+ float t206 = t37+t45;
+ float t209 = sin(t103);
+ float t212 = -t142;
+ float t214 = -t147;
+ float t218 = -t151;
+ float t219 = -t152;
+ float t220 = -t154;
+ float t221 = -t156;
+ float t222 = -t158;
+ float t223 = -t159;
+ float t226 = -t160;
+ float t227 = -t166;
+ float t251 = a3i*mpy6*t4*t52*2.0;
+ float t259 = t10*t140;
+ float t262 = t10*t141;
+ float t264 = t10*t142;
+ float t268 = a3*mpy6*t9*t52*2.0;
+ float t269 = t10*t153;
+ float t271 = t10*t154;
+ float t272 = t10*t155;
+ float t274 = t10*t156;
+ float t275 = t10*t157;
+ float t277 = t10*t158;
+ float t280 = t10*t163;
+ float t282 = t10*t164;
+ float t283 = t10*t165;
+ float t295 = -t235;
+ float t296 = -t236;
+ float t297 = -t175;
+ float t299 = -t241;
+ float t300 = -t242;
+ float t301 = -t177;
+ float t304 = -t188;
+ float t308 = t55+t57;
+ float t309 = q6+t42+t55;
+ float t312 = -t260;
+ float t313 = -t265;
+ float t314 = -t267;
+ float t316 = -t278;
+ float t317 = -t279;
+ float t322 = cos(t306);
+ float t323 = cos(t307);
+ float t326 = cos(t310);
+ float t327 = q2-t310;
+ float t328 = t57+t61;
+ float t332 = sin(t306);
+ float t351 = mpy6*t11*t175*2.0;
+ float t352 = mpy6*t338;
+ float t356 = mpy6*t11*t176*2.0;
+ float t357 = mpy6*t11*t177*2.0;
+ float t363 = mpy6*t11*t188*2.0;
+ float t365 = q6-t12+t82;
+ float t366 = -t12+t364;
+ float t369 = -t335;
+ float t372 = -t337;
+ float t373 = -t290;
+ float t374 = -t338;
+ float t376 = -t291;
+ float t387 = mpy6*t335;
+ float t388 = mpy6*t336;
+ float t389 = mpy6*t337;
+ float t393 = -t344;
+ float t411 = t20-t310;
+ float t414 = t82-t310;
+ float t416 = sin(t364);
+ float t419 = mpy6*t11*t298*2.0;
+ float t434 = -t401;
+ float t445 = -t403;
+ float t449 = mpy6*t398;
+ float t450 = mpy6*t399;
+ float t451 = mpy6*t400;
+ float t452 = mpy6*t401;
+ float t454 = mpy6*t11*t287*2.0;
+ float t455 = mpy6*t403;
+ float t456 = mpy6*t404;
+ float t457 = mpy6*t405;
+ float t459 = mpy6*t11*t288*2.0;
+ float t460 = mpy6*t11*t289*2.0;
+ float t461 = mpy6*t11*t290*2.0;
+ float t462 = mpy6*t408;
+ float t463 = mpy6*t11*t291*2.0;
+ float t464 = mpy6*t11*t292*2.0;
+ float t465 = mpy6*t11*t293*2.0;
+ float t89 = cos(t62);
+ float t91 = cos(t64);
+ float t92 = cos(t66);
+ float t108 = sin(t62);
+ float t110 = sin(t64);
+ float t112 = sin(t66);
+ float t168 = cos(t83);
+ float t169 = q3+t64;
+ float t171 = sin(t83);
+ float t178 = a3i*t58;
+ float t189 = a3*t69;
+ float t199 = cos(t100);
+ float t200 = cos(t101);
+ float t201 = cos(t102);
+ float t203 = cos(t105);
+ float t204 = cos(t106);
+ float t207 = sin(t100);
+ float t208 = sin(t102);
+ float t210 = sin(t104);
+ float t211 = d5*t88;
+ float t213 = a4*t107;
+ float t216 = -t149;
+ float t224 = cos(t205);
+ float t225 = cos(t206);
+ float t228 = sin(t205);
+ float t248 = q3+t104;
+ float t285 = q3+t206;
+ float t294 = -t174;
+ float t303 = t12+t64;
+ float t320 = t31+t64;
+ float t324 = cos(t308);
+ float t325 = cos(t309);
+ float t329 = t42+t101;
+ float t330 = t57+t64;
+ float t331 = t42+t104;
+ float t333 = sin(t308);
+ float t334 = sin(t309);
+ float t339 = a1*dq1*mpy3*t58*2.0;
+ float t340 = a1*dq1*mpx3*t69*2.0;
+ float t342 = (a2*dq1*mpy6*t46)/2.0;
+ float t358 = a1*dq1*mpx4*t107*2.0;
+ float t359 = (a2*dq1*mpy6*t53)/4.0;
+ float t377 = t45+t64;
+ float t379 = cos(t327);
+ float t380 = cos(t328);
+ float t384 = sin(t327);
+ float t385 = sin(t328);
+ float t409 = cos(t365);
+ float t410 = cos(t366);
+ float t415 = -t12+t411;
+ float t417 = sin(t365);
+ float t418 = sin(t366);
+ float t420 = cos(t411);
+ float t423 = cos(t414);
+ float t425 = (a2*dq1*mpy6*t111)/2.0;
+ float t428 = sin(t414);
+ float t433 = t52+t115;
+ float t466 = -t419;
+ float t467 = t64+t105;
+ float t468 = t64+t106;
+ float t478 = a1*dq1*mpy6*t323;
+ float t480 = -t454;
+ float t482 = -t459;
+ float t483 = -t460;
+ float t484 = -t464;
+ float t485 = -t465;
+ float t519 = (a2*dq1*mpy6*t326)/4.0;
+ float t545 = (a2*dq1*mpy6*t416)/2.0;
+ float t247 = cos(t169);
+ float t249 = sin(t169);
+ float t284 = cos(t248);
+ float t286 = sin(t248);
+ float t302 = -t178;
+ float t305 = -t189;
+ float t311 = a2*dq1*mpx4*t91;
+ float t318 = cos(t303);
+ float t321 = sin(t303);
+ float t341 = cos(t285);
+ float t343 = sin(t285);
+ float t353 = a1*dq1*mass4*t189*2.0;
+ float t354 = a1*dq1*mass5*t189*2.0;
+ float t355 = a1*dq1*mass6*t189*2.0;
+ float t367 = a2*a4*dq1*mass5*t91;
+ float t368 = a2*a4*dq1*mass6*t91;
+ float t370 = a1*dq1*mpy6*t200;
+ float t371 = a2*d5*dq1*mass6*t110;
+ float t375 = cos(t320);
+ float t378 = sin(t320);
+ float t381 = cos(t329);
+ float t382 = cos(t330);
+ float t383 = cos(t331);
+ float t386 = sin(t329);
+ float t390 = a1*dq1*mass4*t178*2.0;
+ float t391 = a1*dq1*mass5*t178*2.0;
+ float t392 = a1*dq1*mass6*t178*2.0;
+ float t395 = cos(t377);
+ float t396 = -t358;
+ float t397 = sin(t377);
+ float t402 = a1*dq1*mass6*t211*2.0;
+ float t406 = a1*dq1*mass5*t213*2.0;
+ float t407 = a1*dq1*mass6*t213*2.0;
+ float t412 = t57+t169;
+ float t413 = t42+t248;
+ float t424 = cos(t415);
+ float t429 = sin(t415);
+ float t430 = a3*a3i*dq1*mass4*t168*2.0;
+ float t431 = a3*a3i*dq1*mass5*t168*2.0;
+ float t432 = a3*a3i*dq1*mass6*t168*2.0;
+ float t435 = dq1*mass4*t16*t171;
+ float t436 = dq1*mass5*t16*t171;
+ float t437 = dq1*mass6*t16*t171;
+ float t438 = dq1*mass4*t18*t171;
+ float t439 = dq1*mass5*t18*t171;
+ float t440 = dq1*mass6*t18*t171;
+ float t469 = (a2*dq1*mpy6*t203)/4.0;
+ float t470 = (a2*dq1*mpy6*t204)/4.0;
+ float t471 = (a2*dq1*mpy6*t210)/2.0;
+ float t488 = cos(t467);
+ float t489 = cos(t468);
+ float t492 = sin(t467);
+ float t493 = sin(t468);
+ float t495 = (a2*dq1*mpy6*t225)/4.0;
+ float t496 = (d5*dq2*mpy6*t224)/2.0;
+ float t497 = (d5*dq3*mpy6*t224)/2.0;
+ float t498 = (d5*dq4*mpy6*t224)/2.0;
+ float t499 = (a1*dq1*mpy6*t228)/2.0;
+ float t500 = (a4*dq2*mpy6*t228)/2.0;
+ float t501 = (a4*dq3*mpy6*t228)/2.0;
+ float t502 = (a4*dq4*mpy6*t228)/2.0;
+ float t506 = a3*dq1*mpy6*t409;
+ float t507 = a4*dq1*mpy6*t410;
+ float t509 = a3i*dq1*mpy6*t417;
+ float t510 = d5*dq1*mpy6*t418;
+ float t524 = (d5*dq2*mpy6*t379)/2.0;
+ float t525 = (d5*dq2*mpy6*t380)/2.0;
+ float t527 = (d5*dq3*mpy6*t379)/2.0;
+ float t528 = (d5*dq3*mpy6*t380)/2.0;
+ float t530 = (d5*dq4*mpy6*t379)/2.0;
+ float t531 = (d5*dq4*mpy6*t380)/2.0;
+ float t533 = (a1*dq1*mpy6*t384)/2.0;
+ float t534 = (a1*dq1*mpy6*t385)/2.0;
+ float t536 = (a4*dq2*mpy6*t384)/2.0;
+ float t537 = (a4*dq2*mpy6*t385)/2.0;
+ float t539 = (a4*dq3*mpy6*t384)/2.0;
+ float t540 = (a4*dq3*mpy6*t385)/2.0;
+ float t542 = (a4*dq4*mpy6*t384)/2.0;
+ float t543 = (a4*dq4*mpy6*t385)/2.0;
+ float t551 = (a2*dq1*mpy6*t420)/4.0;
+ float t554 = (a3i*dq1*mpy6*t423)/2.0;
+ float t559 = (a3*dq1*mpy6*t428)/2.0;
+ float t394 = -t311;
+ float t421 = cos(t412);
+ float t422 = cos(t413);
+ float t426 = sin(t412);
+ float t427 = sin(t413);
+ float t441 = a3i*dq1*mpx4*t247*2.0;
+ float t442 = -t367;
+ float t443 = -t368;
+ float t444 = -t402;
+ float t446 = a3*dq1*mpx4*t249*2.0;
+ float t447 = -t406;
+ float t448 = -t407;
+ float t453 = a3*dq1*mpy6*t284;
+ float t458 = a3i*dq1*mpy6*t286;
+ float t472 = a4*a3i*dq1*mass5*t247*2.0;
+ float t473 = a4*a3i*dq1*mass6*t247*2.0;
+ float t475 = a3*d5*dq1*mass6*t247*2.0;
+ float t476 = a3*a4*dq1*mass5*t249*2.0;
+ float t477 = a3*a4*dq1*mass6*t249*2.0;
+ float t479 = a3i*d5*dq1*mass6*t249*2.0;
+ float t486 = a4*dq1*mpy6*t375;
+ float t487 = d5*dq1*mpy6*t378;
+ float t490 = -t469;
+ float t491 = -t470;
+ float t494 = -t471;
+ float t503 = a4*d5*dq1*mass6*t318*2.0;
+ float t504 = dq1*mass5*t17*t321;
+ float t505 = dq1*mass6*t17*t321;
+ float t508 = dq1*mass6*t19*t321;
+ float t511 = -t495;
+ float t512 = -t496;
+ float t513 = -t499;
+ float t514 = -t500;
+ float t516 = (a3i*dq1*mpy6*t341)/2.0;
+ float t518 = (a3*dq1*mpy6*t343)/2.0;
+ float t522 = (a2*dq1*mpy6*t382)/4.0;
+ float t523 = (a2*dq1*mpy6*t383)/4.0;
+ float t526 = (d5*dq2*mpy6*t381)/2.0;
+ float t529 = (d5*dq3*mpy6*t381)/2.0;
+ float t532 = (d5*dq4*mpy6*t381)/2.0;
+ float t535 = (a1*dq1*mpy6*t386)/2.0;
+ float t538 = (a4*dq2*mpy6*t386)/2.0;
+ float t541 = (a4*dq3*mpy6*t386)/2.0;
+ float t544 = (a4*dq4*mpy6*t386)/2.0;
+ float t547 = -t536;
+ float t549 = (d5*dq1*mpy6*t395)/2.0;
+ float t550 = (a4*dq1*mpy6*t397)/2.0;
+ float t556 = (d5*dq1*mpy6*t424)/2.0;
+ float t561 = (a4*dq1*mpy6*t429)/2.0;
+ float t562 = -t551;
+ float t564 = (d5*dq1*mpy6*t488)/2.0;
+ float t565 = (d5*dq1*mpy6*t489)/2.0;
+ float t566 = (a4*dq1*mpy6*t492)/2.0;
+ float t567 = (a4*dq1*mpy6*t493)/2.0;
+ float t515 = -t503;
+ float t517 = -t487;
+ float t520 = -t504;
+ float t521 = -t505;
+ float t546 = -t526;
+ float t548 = -t538;
+ float t552 = (a3i*dq1*mpy6*t421)/2.0;
+ float t553 = (a3i*dq1*mpy6*t422)/2.0;
+ float t555 = -t549;
+ float t557 = (a3*dq1*mpy6*t426)/2.0;
+ float t558 = (a3*dq1*mpy6*t427)/2.0;
+ float t560 = -t550;
+ float t563 = -t556;
+ A0(0,0) = a1*dq2*mass4*t178*-2.0-a1*dq2*mass5*t178*2.0+a1*dq3*mass4*t178*2.0-a1*dq2*mass6*t178*2.0+a1*dq3*mass5*t178*2.0+a1*dq3*mass6*t178*2.0-a1*dq2*mass4*t189*2.0-a1*dq2*mass5*t189*2.0+a1*dq3*mass4*t189*2.0-a1*dq2*mass6*t189*2.0+a1*dq3*mass5*t189*2.0+a1*dq3*mass6*t189*2.0+a1*dq2*mass6*t211*2.0+a1*dq2*mass5*t213*2.0-a1*dq3*mass6*t211*2.0+a1*dq2*mass6*t213*2.0-a1*dq3*mass5*t213*2.0-a1*dq4*mass6*t211*2.0-a1*dq3*mass6*t213*2.0-a1*dq4*mass5*t213*2.0-a1*dq4*mass6*t213*2.0+a1*dq2*mpx2*t2*2.0+a2*dq3*mpx3*t3-a3*dq4*mpx4*t9+a2*dq3*mpx4*t29+a2*dq4*mpx4*t29-a1*dq2*mpx3*t69*2.0+a1*dq3*mpx3*t69*2.0+a2*dq2*mpx4*t91*2.0-a2*dq3*mpx4*t91-a2*dq4*mpx4*t91+a1*dq2*mpx4*t107*2.0-a1*dq3*mpx4*t107*2.0-a1*dq4*mpx4*t107*2.0+a2*dq2*mpx3*t167*2.0-a2*dq3*mpx3*t167+a3*dq2*mpx4*t249*2.0-a3*dq3*mpx4*t249*2.0-a3*dq4*mpx4*t249+a1*dq2*mpy2*t7*2.0-a2*dq3*mpy3*t8+(a3*dq4*mpy6*t30)/2.0+(a3*dq6*mpy6*t30)/2.0-(a4*dq5*mpy6*t35)/2.0-(a4*dq6*mpy6*t35)/2.0+(a2*dq3*mpy6*t46)/2.0+(a2*dq4*mpy6*t46)/2.0+(a2*dq6*mpy6*t46)/2.0-(a3*dq4*mpy6*t47)/4.0-(a3*dq5*mpy6*t47)/4.0-(a3*dq6*mpy6*t47)/4.0+a1*dq2*mpy3*t58*2.0+(a2*dq3*mpy6*t53)/4.0-a1*dq3*mpy3*t58*2.0+(a2*dq4*mpy6*t53)/4.0+(a2*dq5*mpy6*t53)/4.0+(a2*dq6*mpy6*t53)/4.0+(a3*dq4*mpy6*t59)/2.0-(a3*dq6*mpy6*t59)/2.0+(a4*dq5*mpy6*t71)/2.0-(a4*dq6*mpy6*t71)/2.0+(a2*dq3*mpy6*t111)/2.0+(a2*dq4*mpy6*t111)/2.0-(a2*dq6*mpy6*t111)/2.0+(a3*dq4*mpy6*t112)/4.0+(a3*dq4*mpy6*t113)/4.0+(a3*dq5*mpy6*t112)/4.0+(a3*dq4*mpy6*t114)/4.0-(a3*dq5*mpy6*t113)/4.0-(a3*dq6*mpy6*t112)/4.0-(a3*dq5*mpy6*t114)/4.0+(a3*dq6*mpy6*t113)/4.0-(a3*dq6*mpy6*t114)/4.0+a2*dq2*mpy3*t170*2.0-a2*dq3*mpy3*t170-a1*dq2*mpy6*t200+a1*dq3*mpy6*t200+a1*dq4*mpy6*t200+a1*dq6*mpy6*t200-(a2*dq3*mpy6*t203)/4.0-(a2*dq3*mpy6*t204)/4.0-(a2*dq4*mpy6*t203)/4.0-(a2*dq4*mpy6*t204)/4.0-(a2*dq5*mpy6*t203)/4.0+(a2*dq5*mpy6*t204)/4.0+(a2*dq6*mpy6*t203)/4.0-(a2*dq6*mpy6*t204)/4.0+a2*dq2*mpy6*t210-(a2*dq3*mpy6*t210)/2.0-(a2*dq4*mpy6*t210)/2.0-(a2*dq6*mpy6*t210)/2.0+(a2*dq2*mpy6*t225)/2.0-(a2*dq3*mpy6*t225)/4.0+(a1*dq2*mpy6*t228)/2.0-(a2*dq4*mpy6*t225)/4.0-(a1*dq3*mpy6*t228)/2.0-(a2*dq5*mpy6*t225)/4.0-(a1*dq4*mpy6*t228)/2.0-(a2*dq6*mpy6*t225)/4.0-(a1*dq5*mpy6*t228)/2.0-(a1*dq6*mpy6*t228)/2.0-a3*dq2*mpy6*t284+a3*dq3*mpy6*t284+(a3*dq4*mpy6*t284)/2.0+(a3*dq6*mpy6*t284)/2.0-a1*dq2*mpy6*t323+a1*dq3*mpy6*t323+a1*dq4*mpy6*t323-a1*dq6*mpy6*t323+(a2*dq3*mpy6*t326)/4.0+(a2*dq4*mpy6*t326)/4.0-(a2*dq5*mpy6*t326)/4.0-(a2*dq6*mpy6*t326)/4.0+(a3*dq2*mpy6*t343)/2.0-(a3*dq3*mpy6*t343)/2.0-(a3*dq4*mpy6*t343)/4.0-(a3*dq5*mpy6*t343)/4.0-(a3*dq6*mpy6*t343)/4.0-a4*dq2*mpy6*t375+a4*dq3*mpy6*t375+a4*dq4*mpy6*t375+(a4*dq6*mpy6*t375)/2.0-(a2*dq2*mpy6*t382)/2.0-(a1*dq2*mpy6*t384)/2.0-(a2*dq2*mpy6*t383)/2.0+(a2*dq3*mpy6*t382)/4.0-(a1*dq2*mpy6*t385)/2.0+(a1*dq3*mpy6*t384)/2.0+(a2*dq3*mpy6*t383)/4.0+(a2*dq4*mpy6*t382)/4.0-(a1*dq2*mpy6*t386)/2.0+(a1*dq3*mpy6*t385)/2.0+(a1*dq4*mpy6*t384)/2.0+(a2*dq4*mpy6*t383)/4.0+(a2*dq5*mpy6*t382)/4.0+(a1*dq3*mpy6*t386)/2.0+(a1*dq4*mpy6*t385)/2.0-(a1*dq5*mpy6*t384)/2.0-(a2*dq5*mpy6*t383)/4.0-(a2*dq6*mpy6*t382)/4.0+(a1*dq4*mpy6*t386)/2.0+(a1*dq5*mpy6*t385)/2.0-(a1*dq6*mpy6*t384)/2.0+(a2*dq6*mpy6*t383)/4.0-(a1*dq5*mpy6*t386)/2.0-(a1*dq6*mpy6*t385)/2.0+(a1*dq6*mpy6*t386)/2.0+(a4*dq2*mpy6*t397)/2.0-(a4*dq3*mpy6*t397)/2.0-(a4*dq4*mpy6*t397)/2.0-(a4*dq5*mpy6*t397)/4.0-(a4*dq6*mpy6*t397)/4.0-a3*dq2*mpy6*t409+a3*dq3*mpy6*t409+(a3*dq4*mpy6*t409)/2.0-a4*dq2*mpy6*t410+a4*dq3*mpy6*t410-(a3*dq6*mpy6*t409)/2.0+a4*dq4*mpy6*t410-a2*dq2*mpy6*t416-(a4*dq6*mpy6*t410)/2.0+(a2*dq3*mpy6*t416)/2.0+(a2*dq4*mpy6*t416)/2.0+(a2*dq2*mpy6*t420)/2.0-(a2*dq6*mpy6*t416)/2.0-(a2*dq3*mpy6*t420)/4.0-(a2*dq4*mpy6*t420)/4.0+(a2*dq5*mpy6*t420)/4.0+(a2*dq6*mpy6*t420)/4.0-(a3*dq2*mpy6*t426)/2.0-(a3*dq2*mpy6*t427)/2.0+(a3*dq3*mpy6*t426)/2.0-(a3*dq2*mpy6*t428)/2.0+(a3*dq3*mpy6*t427)/2.0+(a3*dq4*mpy6*t426)/4.0+(a3*dq3*mpy6*t428)/2.0+(a3*dq4*mpy6*t427)/4.0+(a3*dq5*mpy6*t426)/4.0+(a3*dq4*mpy6*t428)/4.0-(a3*dq5*mpy6*t427)/4.0-(a3*dq6*mpy6*t426)/4.0-(a4*dq2*mpy6*t429)/2.0-(a3*dq5*mpy6*t428)/4.0+(a3*dq6*mpy6*t427)/4.0+(a4*dq3*mpy6*t429)/2.0-(a3*dq6*mpy6*t428)/4.0+(a4*dq4*mpy6*t429)/2.0-(a4*dq5*mpy6*t429)/4.0-(a4*dq6*mpy6*t429)/4.0-(a4*dq2*mpy6*t492)/2.0-(a4*dq2*mpy6*t493)/2.0+(a4*dq3*mpy6*t492)/2.0+(a4*dq3*mpy6*t493)/2.0+(a4*dq4*mpy6*t492)/2.0+(a4*dq4*mpy6*t493)/2.0+(a4*dq5*mpy6*t492)/4.0-(a4*dq5*mpy6*t493)/4.0-(a4*dq6*mpy6*t492)/4.0+(a4*dq6*mpy6*t493)/4.0-a3i*dq4*mpx4*t4-a3i*dq2*mpx4*t247*2.0+a3i*dq3*mpx4*t247*2.0+a3i*dq4*mpx4*t247-(a3i*dq4*mpy6*t34)/2.0-(a3i*dq6*mpy6*t34)/2.0-(a3i*dq4*mpy6*t44)/4.0-(a3i*dq5*mpy6*t44)/4.0-(a3i*dq6*mpy6*t44)/4.0-(a3i*dq4*mpy6*t70)/2.0+(a3i*dq6*mpy6*t70)/2.0+(a3i*dq4*mpy6*t92)/4.0+(a3i*dq4*mpy6*t93)/4.0+(a3i*dq5*mpy6*t92)/4.0-(a3i*dq4*mpy6*t94)/4.0-(a3i*dq5*mpy6*t93)/4.0-(a3i*dq6*mpy6*t92)/4.0+(a3i*dq5*mpy6*t94)/4.0+(a3i*dq6*mpy6*t93)/4.0+(a3i*dq6*mpy6*t94)/4.0-a3i*dq2*mpy6*t286+a3i*dq3*mpy6*t286+(a3i*dq4*mpy6*t286)/2.0+(a3i*dq6*mpy6*t286)/2.0-(a3i*dq2*mpy6*t341)/2.0+(a3i*dq3*mpy6*t341)/2.0+(a3i*dq4*mpy6*t341)/4.0+(a3i*dq5*mpy6*t341)/4.0+(a3i*dq6*mpy6*t341)/4.0+a3i*dq2*mpy6*t417-a3i*dq3*mpy6*t417-(a3i*dq4*mpy6*t417)/2.0+(a3i*dq2*mpy6*t421)/2.0+(a3i*dq6*mpy6*t417)/2.0+(a3i*dq2*mpy6*t422)/2.0-(a3i*dq3*mpy6*t421)/2.0-(a3i*dq2*mpy6*t423)/2.0-(a3i*dq3*mpy6*t422)/2.0-(a3i*dq4*mpy6*t421)/4.0+(a3i*dq3*mpy6*t423)/2.0-(a3i*dq4*mpy6*t422)/4.0-(a3i*dq5*mpy6*t421)/4.0+(a3i*dq4*mpy6*t423)/4.0+(a3i*dq5*mpy6*t422)/4.0+(a3i*dq6*mpy6*t421)/4.0-(a3i*dq5*mpy6*t423)/4.0-(a3i*dq6*mpy6*t422)/4.0-(a3i*dq6*mpy6*t423)/4.0+d5*dq6*mpy6*t11+d5*dq2*mpy6*t378-d5*dq3*mpy6*t378-d5*dq4*mpy6*t378-(d5*dq6*mpy6*t378)/2.0+(d5*dq2*mpy6*t395)/2.0-(d5*dq3*mpy6*t395)/2.0-(d5*dq4*mpy6*t395)/2.0-(d5*dq5*mpy6*t395)/4.0-(d5*dq6*mpy6*t395)/4.0-d5*dq2*mpy6*t418+d5*dq3*mpy6*t418+d5*dq4*mpy6*t418-(d5*dq6*mpy6*t418)/2.0+(d5*dq2*mpy6*t424)/2.0-(d5*dq3*mpy6*t424)/2.0-(d5*dq4*mpy6*t424)/2.0+(d5*dq5*mpy6*t424)/4.0+(d5*dq6*mpy6*t424)/4.0-(d5*dq2*mpy6*t488)/2.0-(d5*dq2*mpy6*t489)/2.0+(d5*dq3*mpy6*t488)/2.0+(d5*dq3*mpy6*t489)/2.0+(d5*dq4*mpy6*t488)/2.0+(d5*dq4*mpy6*t489)/2.0+(d5*dq5*mpy6*t488)/4.0-(d5*dq5*mpy6*t489)/4.0-(d5*dq6*mpy6*t488)/4.0+(d5*dq6*mpy6*t489)/4.0+dq2*mass3*t15*t26+dq2*mass4*t15*t26+dq2*mass5*t15*t26+dq2*mass6*t15*t26-dq2*mass4*t16*t171-dq2*mass5*t16*t171+dq3*mass4*t16*t171+dq2*mass4*t18*t171-dq2*mass6*t16*t171+dq3*mass5*t16*t171+dq2*mass5*t18*t171-dq3*mass4*t18*t171+dq3*mass6*t16*t171+dq2*mass6*t18*t171-dq3*mass5*t18*t171-dq3*mass6*t18*t171+dq2*mass5*t17*t321+dq2*mass6*t17*t321-dq3*mass5*t17*t321-dq3*mass6*t17*t321-dq4*mass5*t17*t321-dq2*mass6*t19*t321-dq4*mass6*t17*t321+dq3*mass6*t19*t321+dq4*mass6*t19*t321+a1*a2*dq2*mass3*t2*2.0+a1*a2*dq2*mass4*t2*2.0+a1*a2*dq2*mass5*t2*2.0+a1*a2*dq2*mass6*t2*2.0+a2*a3*dq3*mass4*t3+a2*a3*dq3*mass5*t3+a2*a3*dq3*mass6*t3-a3*a4*dq4*mass5*t9-a3*a4*dq4*mass6*t9+a2*a4*dq3*mass5*t29+a2*a4*dq3*mass6*t29+a2*a4*dq4*mass5*t29+a2*a4*dq4*mass6*t29+a2*a4*dq2*mass5*t91*2.0+a2*a4*dq2*mass6*t91*2.0-a2*a4*dq3*mass5*t91-a2*a4*dq3*mass6*t91-a2*a4*dq4*mass5*t91-a2*a4*dq4*mass6*t91+a2*a3*dq2*mass4*t167*2.0+a2*a3*dq2*mass5*t167*2.0-a2*a3*dq3*mass4*t167+a2*a3*dq2*mass6*t167*2.0-a2*a3*dq3*mass5*t167-a2*a3*dq3*mass6*t167+a3*a4*dq2*mass5*t249*2.0+a3*a4*dq2*mass6*t249*2.0-a3*a4*dq3*mass5*t249*2.0-a3*a4*dq3*mass6*t249*2.0-a3*a4*dq4*mass5*t249-a3*a4*dq4*mass6*t249+a2*a3i*dq3*mass4*t8-a4*a3i*dq4*mass5*t4+a2*a3i*dq3*mass5*t8-a4*a3i*dq4*mass6*t4+a2*a3i*dq3*mass6*t8-a3*a3i*dq2*mass4*t168*2.0-a2*a3i*dq2*mass4*t170*2.0-a3*a3i*dq2*mass5*t168*2.0+a3*a3i*dq3*mass4*t168*2.0-a2*a3i*dq2*mass5*t170*2.0+a2*a3i*dq3*mass4*t170-a3*a3i*dq2*mass6*t168*2.0+a3*a3i*dq3*mass5*t168*2.0-a2*a3i*dq2*mass6*t170*2.0+a2*a3i*dq3*mass5*t170+a3*a3i*dq3*mass6*t168*2.0+a2*a3i*dq3*mass6*t170-a4*a3i*dq2*mass5*t247*2.0-a4*a3i*dq2*mass6*t247*2.0+a4*a3i*dq3*mass5*t247*2.0+a4*a3i*dq3*mass6*t247*2.0+a4*a3i*dq4*mass5*t247+a4*a3i*dq4*mass6*t247-a3*d5*dq4*mass6*t4-a2*d5*dq3*mass6*t33-a2*d5*dq4*mass6*t33-a2*d5*dq2*mass6*t110*2.0+a2*d5*dq3*mass6*t110+a2*d5*dq4*mass6*t110+a3*d5*dq2*mass6*t247*2.0-a3*d5*dq3*mass6*t247*2.0-a3*d5*dq4*mass6*t247+a4*d5*dq2*mass6*t318*2.0-a4*d5*dq3*mass6*t318*2.0-a4*d5*dq4*mass6*t318*2.0+a3i*d5*dq4*mass6*t9+a3i*d5*dq2*mass6*t249*2.0-a3i*d5*dq3*mass6*t249*2.0-a3i*d5*dq4*mass6*t249;
+ A0(0,1) = t311*2.0+t339-t340-t353-t354-t355+t358+t367*2.0+t368*2.0-t370-t371*2.0-t390-t391-t392+t402+t406+t407-t430-t431-t432-t435-t436-t437+t438+t439+t440-t441+t446-t453-t458-t472-t473+t475+t476+t477-t478+t479-t486+t487-t497-t498+t499-t501-t502+t503+t504+t505-t506-t507-t508+t509-t510-t516+t518+t527+t528-t529+t530+t531-t532-t533-t534-t535-t539+t540-t541-t542+t543-t544+t549+t550+t552+t553-t554+t556-t557-t558-t559-t561-t564-t565-t566-t567+a1*dq1*mpx2*t2*2.0+a2*dq1*mpx3*t167*2.0+a1*dq1*mpy2*t7*2.0+a2*dq1*mpy3*t170*2.0+a2*dq1*mpy6*t210-(a3*dq3*mpy6*t208)/2.0-(a3*dq3*mpy6*t209)/2.0+(a2*dq1*mpy6*t225)/2.0+(a3*dq3*mpy6*t333)/2.0-(a3*dq3*mpy6*t334)/2.0-(a2*dq1*mpy6*t382)/2.0-(a2*dq1*mpy6*t383)/2.0-a2*dq1*mpy6*t416+(a2*dq1*mpy6*t420)/2.0-(a3i*dq3*mpy6*t201)/2.0+(a3i*dq3*mpy6*t202)/2.0+(a3i*dq3*mpy6*t324)/2.0-(a3i*dq3*mpy6*t325)/2.0+dq1*mass3*t15*t26+dq1*mass4*t15*t26+dq1*mass5*t15*t26+dq1*mass6*t15*t26+a1*a2*dq1*mass3*t2*2.0+a1*a2*dq1*mass4*t2*2.0+a1*a2*dq1*mass5*t2*2.0+a1*a2*dq1*mass6*t2*2.0+a2*a3*dq1*mass4*t167*2.0+a2*a3*dq1*mass5*t167*2.0+a2*a3*dq1*mass6*t167*2.0-a2*a3i*dq1*mass4*t170*2.0-a2*a3i*dq1*mass5*t170*2.0-a2*a3i*dq1*mass6*t170*2.0;
+ A0(0,2) = t84+t172+t173+t294-t339+t340+t342+t353+t354+t355+t359+t370+t371+t390+t391+t392+t394+t396+t425+t430+t431+t432+t435+t436+t437-t438-t439-t440+t441+t442+t443+t444-t446+t447+t448+t453+t458+t472+t473-t475-t476-t477+t478-t479+t486+t490+t491+t494+t498+t502+t506+t507+t508-t509+t510+t511+t512+t513+t514+t515+t516+t517-t518+t519+t520+t521+t522+t523+t524+t525-t530-t531+t532+t533+t534+t535+t537+t542-t543+t544+t545+t546+t547+t548-t552-t553+t554+t555+t557+t558+t559+t560+t561+t562+t563+t564+t565+t566+t567+a2*dq1*mpx3*t3-a2*dq1*mpx3*t167-a2*dq1*mpy3*t8-a2*dq1*mpy3*t170-(a3*dq2*mpy6*t208)/2.0-(a3*dq2*mpy6*t209)/2.0+(a3*dq2*mpy6*t333)/2.0-(a3*dq2*mpy6*t334)/2.0-(a3i*dq2*mpy6*t201)/2.0+(a3i*dq2*mpy6*t202)/2.0+(a3i*dq2*mpy6*t324)/2.0-(a3i*dq2*mpy6*t325)/2.0+a2*a3*dq1*mass4*t3+a2*a3*dq1*mass5*t3+a2*a3*dq1*mass6*t3-a2*a3*dq1*mass4*t167-a2*a3*dq1*mass5*t167-a2*a3*dq1*mass6*t167+a2*a3i*dq1*mass4*t8+a2*a3i*dq1*mass5*t8+a2*a3i*dq1*mass6*t8+a2*a3i*dq1*mass4*t170+a2*a3i*dq1*mass5*t170+a2*a3i*dq1*mass6*t170;
+ A0(0,3) = t84+t172+t173+t294+t342+t359+t370+t371+t394+t396+t425+t442+t443+t444+t447+t448+t453/2.0+t458/2.0+t478+t486+t490+t491+t494+t497+t501+t506/2.0+t507+t508-t509/2.0+t510+t511+t512+t513+t514+t515+t517+t519+t520+t521+t522+t523+t524+t525-t527-t528+t529+t533+t534+t535+t537+t539-t540+t541+t545+t546+t547+t548+t555+t560+t561+t562+t563+t564+t565+t566+t567-a3*dq1*mpx4*t9-a3*dq1*mpx4*t249+(a3*dq1*mpy6*t30)/2.0-(a3*dq1*mpy6*t47)/4.0+(a3*dq1*mpy6*t59)/2.0+(a3*dq1*mpy6*t112)/4.0+(a3*dq1*mpy6*t113)/4.0+(a3*dq1*mpy6*t114)/4.0-(a3*dq1*mpy6*t343)/4.0+(a3*dq1*mpy6*t426)/4.0+(a3*dq1*mpy6*t427)/4.0+(a3*dq1*mpy6*t428)/4.0-a3i*dq1*mpx4*t4+a3i*dq1*mpx4*t247-(a3i*dq1*mpy6*t34)/2.0-(a3i*dq1*mpy6*t44)/4.0-(a3i*dq1*mpy6*t70)/2.0+(a3i*dq1*mpy6*t92)/4.0+(a3i*dq1*mpy6*t93)/4.0-(a3i*dq1*mpy6*t94)/4.0+(a3i*dq1*mpy6*t341)/4.0-(a3i*dq1*mpy6*t421)/4.0-(a3i*dq1*mpy6*t422)/4.0+(a3i*dq1*mpy6*t423)/4.0-a3*a4*dq1*mass5*t9-a3*a4*dq1*mass6*t9-a3*a4*dq1*mass5*t249-a3*a4*dq1*mass6*t249-a4*a3i*dq1*mass5*t4-a4*a3i*dq1*mass6*t4+a4*a3i*dq1*mass5*t247+a4*a3i*dq1*mass6*t247-a3*d5*dq1*mass6*t4-a3*d5*dq1*mass6*t247+a3i*d5*dq1*mass6*t9-a3i*d5*dq1*mass6*t249;
+ A0(0,4) = t2*t8*t9*t152-a4*dq1*mpy6*t5*t11-a1*dq6*mpy6*t6*t10*2.0-a3*dq1*mpy6*t4*t5*t11-a2*dq6*mpy6*t6*t7*t10*2.0+a3i*dq1*mpy6*t5*t9*t11-a3*dq6*mpy6*t2*t3*t6*t10*2.0-a2*dq1*mpy6*t3*t5*t9*t11-a2*dq1*mpy6*t4*t5*t8*t11-a3*dq6*mpy6*t6*t7*t8*t10*2.0-a3i*dq6*mpy6*t2*t6*t8*t10*2.0+a3i*dq6*mpy6*t3*t6*t7*t10*2.0-a1*dq1*mpy6*t2*t3*t4*t5*t11*2.0-a4*dq6*mpy6*t2*t3*t4*t6*t10*2.0+a1*dq1*mpy6*t2*t5*t8*t9*t11*2.0-a1*dq1*mpy6*t3*t5*t7*t9*t11*2.0-a1*dq1*mpy6*t4*t5*t7*t8*t11*2.0-a4*dq6*mpy6*t3*t6*t7*t9*t10*2.0-a4*dq6*mpy6*t4*t6*t7*t8*t10*2.0-a2*dq1*mpy6*t3*t4*t5*t11*t26+a2*dq1*mpy6*t3*t5*t9*t11*t23+a2*dq1*mpy6*t4*t5*t8*t11*t23+a2*dq1*mpy6*t5*t8*t9*t11*t26-a3*dq1*mpy6*t4*t5*t11*t23*t24-a3*dq1*mpy6*t4*t5*t11*t26*t27+a3*dq1*mpy6*t5*t9*t11*t23*t27-a3*dq1*mpy6*t5*t9*t11*t24*t26-a4*dq1*mpy6*t5*t11*t23*t24*t25+a4*dq1*mpy6*t5*t11*t23*t27*t28-a4*dq1*mpy6*t5*t11*t24*t26*t28-a4*dq1*mpy6*t5*t11*t25*t26*t27-a3i*dq1*mpy6*t4*t5*t11*t23*t27+a3i*dq1*mpy6*t4*t5*t11*t24*t26-a3i*dq1*mpy6*t5*t9*t11*t23*t24-a3i*dq1*mpy6*t5*t9*t11*t26*t27+d5*dq6*mpy6*t2*t3*t6*t9*t10*2.0+d5*dq6*mpy6*t2*t4*t6*t8*t10*2.0-d5*dq6*mpy6*t3*t4*t6*t7*t10*2.0+d5*dq6*mpy6*t6*t7*t8*t9*t10*2.0+d5*dq1*mpy6*t5*t11*t23*t24*t28+d5*dq1*mpy6*t5*t11*t23*t25*t27-d5*dq1*mpy6*t5*t11*t24*t25*t26+d5*dq1*mpy6*t5*t11*t26*t27*t28;
+ A0(0,5) = d5*dq1*mpy6*t11-a4*dq1*mpy6*t6*t10-a1*dq5*mpy6*t6*t10*2.0-a3*dq1*mpy6*t9*t11-a3i*dq1*mpy6*t4*t11+a2*dq1*mpy6*t3*t4*t11-a3*dq1*mpy6*t4*t6*t10-a2*dq5*mpy6*t6*t7*t10*2.0-a2*dq1*mpy6*t8*t9*t11+a3i*dq1*mpy6*t6*t9*t10-a1*dq1*mpy6*t2*t3*t9*t11*2.0-a1*dq1*mpy6*t2*t4*t8*t11*2.0+a1*dq1*mpy6*t3*t4*t7*t11*2.0-a3*dq5*mpy6*t2*t3*t6*t10*2.0-a2*dq1*mpy6*t3*t6*t9*t10-a2*dq1*mpy6*t4*t6*t8*t10-a1*dq1*mpy6*t7*t8*t9*t11*2.0-a3*dq5*mpy6*t6*t7*t8*t10*2.0-a2*dq1*mpy6*t3*t4*t11*t23-a2*dq1*mpy6*t3*t9*t11*t26-a2*dq1*mpy6*t4*t8*t11*t26+a2*dq1*mpy6*t8*t9*t11*t23-a3*dq1*mpy6*t4*t11*t23*t27+a3*dq1*mpy6*t4*t11*t24*t26-a3*dq1*mpy6*t9*t11*t23*t24-a3*dq1*mpy6*t9*t11*t26*t27-a4*dq1*mpy6*t11*t23*t24*t28-a4*dq1*mpy6*t11*t23*t25*t27+a4*dq1*mpy6*t11*t24*t25*t26-a4*dq1*mpy6*t11*t26*t27*t28-a3i*dq5*mpy6*t2*t6*t8*t10*2.0+a3i*dq5*mpy6*t3*t6*t7*t10*2.0+a3i*dq1*mpy6*t4*t11*t23*t24+a3i*dq1*mpy6*t4*t11*t26*t27-a3i*dq1*mpy6*t9*t11*t23*t27+a3i*dq1*mpy6*t9*t11*t24*t26-d5*dq1*mpy6*t11*t23*t24*t25+d5*dq1*mpy6*t11*t23*t27*t28-d5*dq1*mpy6*t11*t24*t26*t28-d5*dq1*mpy6*t11*t25*t26*t27-a1*dq1*mpy6*t2*t3*t4*t6*t10*2.0-a4*dq5*mpy6*t2*t3*t4*t6*t10*2.0+a1*dq1*mpy6*t2*t6*t8*t9*t10*2.0-a1*dq1*mpy6*t3*t6*t7*t9*t10*2.0-a1*dq1*mpy6*t4*t6*t7*t8*t10*2.0+a4*dq5*mpy6*t2*t6*t8*t9*t10*2.0-a4*dq5*mpy6*t3*t6*t7*t9*t10*2.0-a4*dq5*mpy6*t4*t6*t7*t8*t10*2.0-a2*dq1*mpy6*t3*t4*t6*t10*t26+a2*dq1*mpy6*t3*t6*t9*t10*t23+a2*dq1*mpy6*t4*t6*t8*t10*t23+a2*dq1*mpy6*t6*t8*t9*t10*t26-a3*dq1*mpy6*t4*t6*t10*t23*t24-a3*dq1*mpy6*t4*t6*t10*t26*t27+a3*dq1*mpy6*t6*t9*t10*t23*t27-a3*dq1*mpy6*t6*t9*t10*t24*t26-a4*dq1*mpy6*t6*t10*t23*t24*t25+a4*dq1*mpy6*t6*t10*t23*t27*t28-a4*dq1*mpy6*t6*t10*t24*t26*t28-a4*dq1*mpy6*t6*t10*t25*t26*t27-a3i*dq1*mpy6*t4*t6*t10*t23*t27+a3i*dq1*mpy6*t4*t6*t10*t24*t26-a3i*dq1*mpy6*t6*t9*t10*t23*t24-a3i*dq1*mpy6*t6*t9*t10*t26*t27+d5*dq5*mpy6*t2*t3*t6*t9*t10*2.0+d5*dq5*mpy6*t2*t4*t6*t8*t10*2.0-d5*dq5*mpy6*t3*t4*t6*t7*t10*2.0+d5*dq5*mpy6*t6*t7*t8*t9*t10*2.0+d5*dq1*mpy6*t6*t10*t23*t24*t28+d5*dq1*mpy6*t6*t10*t23*t25*t27-d5*dq1*mpy6*t6*t10*t24*t25*t26+d5*dq1*mpy6*t6*t10*t26*t27*t28;
+ A0(1,0) = mpy6*t433*(t211+t213+t302+t305+a2*t2)*-2.0;
+ A0(1,1) = t81+t87+t97+t133+t138+t139+t142-t161-t162+t183+t184+t187+t194+t195+t218+t219+t222+t223+t227+t231+t232+t233+t234-t237-t238-t239-t240-t243-t244-t245-t246+t254+t255+t256+t257+t260+t275+t278+t279-t281+t283+t313+t314-t345-t346-t347-t348-t349-t350+t361+t362+a2*dq3*mpx3*t3*2.0-a2*dq3*mpy3*t8*2.0+a2*a3*dq3*mass4*t3*2.0+a2*a3*dq3*mass5*t3*2.0+a2*a3*dq3*mass6*t3*2.0+a2*a3i*dq3*mass4*t8*2.0+a2*a3i*dq3*mass5*t8*2.0+a2*a3i*dq3*mass6*t8*2.0;
+ A0(1,2) = t74+t77+t99+t120+t121+t124+t129+t130+t137-t139+t151+t152+t158+t159+t162+t166+t196+t212+t226+t229+t230-t233-t234+t239+t240+t245+t246+t252+t253-t256-t257+t265+t267+t281+t295+t296+t299+t300+t312+t315+t316+t317+t319+t346+t347+t348+t349+t350+t360-t362+t393+a2*dq2*mpx3*t3*2.0-a2*dq2*mpy3*t8*2.0+a2*a3*dq2*mass4*t3*2.0+a2*a3*dq2*mass5*t3*2.0+a2*a3*dq2*mass6*t3*2.0+a2*a3i*dq2*mass4*t8*2.0+a2*a3i*dq2*mass5*t8*2.0+a2*a3i*dq2*mass6*t8*2.0;
+ A0(1,3) = t73+t76+t85+t95+t99+t118+t119+t123+t127+t128+t131-t132+t137-t138+t140-t141+t151+t152+t156+t159+t161+t166+t179+t180+t185+t190+t191+t220+t226+t229+t230-t231-t232+t237+t238+t243+t244+t252+t253-t254-t255+t265+t267+t269+t280+t281+t295+t296+t299+t300+t312+t316+t317+t345+t347+t348+t349+t350+t360-t361+t393-a3*dq3*mpy6*t9*t10*t11*2.0-a3i*dq3*mpy6*t4*t10*t11*2.0;
+ A0(1,4) = -t144-t145+t147+t149+t251-t258+t261+t263+t268+t270-t273-t276-t351+t356-t357-t363+t419+t454+t459+t460-t461-t463+t464+t465-a2*mpy6*t3*t4*t52*2.0+a2*mpy6*t8*t9*t52*2.0+a2*dq1*mpy6*t2*t10*t11*2.0-a2*dq2*mpy6*t3*t5*t9*t11*2.0-a2*dq2*mpy6*t4*t5*t8*t11*2.0+a2*dq3*mpy6*t3*t5*t9*t11*2.0+a2*dq3*mpy6*t4*t5*t8*t11*2.0+a2*dq4*mpy6*t3*t5*t9*t11*2.0+a2*dq4*mpy6*t4*t5*t8*t11*2.0;
+ A0(1,5) = t78-t143+t148+t150-t153+t155+t157-t163+t164+t165+t250+t262+t264+t266+t271+t352+t387+t389+t452+t455+t10*t254+t10*t255+t10*t256+t10*t257-d5*dq3*mpy6*t11*2.0-d5*dq4*mpy6*t11*2.0-a4*dq2*mpy6*t6*t10*2.0-a2*dq1*mpy6*t2*t5*t6*2.0+a2*dq2*mpy6*t3*t4*t11*2.0-a2*dq3*mpy6*t3*t4*t11*2.0-a2*dq4*mpy6*t3*t4*t11*2.0-a3*dq2*mpy6*t4*t6*t10*2.0-a2*dq2*mpy6*t8*t9*t11*2.0+a2*dq3*mpy6*t8*t9*t11*2.0+a2*dq4*mpy6*t8*t9*t11*2.0-a3i*dq3*mpy6*t6*t9*t10*2.0-a3i*dq4*mpy6*t6*t9*t10*2.0-a2*dq5*mpy6*t3*t4*t5*t6*2.0-a3*dq1*mpy6*t2*t5*t6*t8*2.0-a2*dq2*mpy6*t3*t6*t9*t10*2.0-a2*dq2*mpy6*t4*t6*t8*t10*2.0+a2*dq5*mpy6*t5*t6*t8*t9*2.0-a4*dq1*mpy6*t2*t3*t5*t6*t9*2.0-a4*dq1*mpy6*t2*t4*t5*t6*t8*2.0-a4*dq1*mpy6*t5*t6*t7*t8*t9*2.0-d5*dq1*mpy6*t2*t3*t4*t5*t6*2.0-d5*dq1*mpy6*t3*t5*t6*t7*t9*2.0-d5*dq1*mpy6*t4*t5*t6*t7*t8*2.0;
+ A0(2,0) = mpy6*t433*(t178+t189-t211-t213)*-2.0;
+ A0(2,1) = t74+t77+t99+t120+t121+t124+t129+t130+t151+t152+t158+t159+t166+t196+t212+t265+t267+t315+t316+t317+t319;
+ A0(2,2) = t81+t87+t97+t133+t142+t183+t184+t187+t194+t195+t218+t219+t222+t223+t227+t275+t278+t279+t283+t313+t314;
+ A0(2,3) = t72+t75+t81+t86+t96+t116+t117+t122+t125+t126-t131+t132-t140+t141+t154+t181+t182+t186+t192+t193+t218+t219+t221+t223+t227+t272+t278+t279+t282+t313+t314-a3*dq2*mpy6*t9*t10*t11*2.0-a3i*dq2*mpy6*t4*t10*t11*2.0;
+ A0(2,4) = t144+t145+t214+t216-t251+t258-t261-t263-t268-t270+t273+t276+t351-t356+t357+t363+t461+t463+t466+t480+t482+t483+t484+t485;
+ A0(2,5) = t79+t80+t98+t143+t146+t153-t155-t157+t163-t164-t165+t215+t217-t250+t259-t266+t274+t277+t388+t449+t450+t451+t456+t457+t462+t474+t481-a3*dq3*mpy6*t4*t6*t10*2.0-a3*dq4*mpy6*t4*t6*t10*2.0-a3i*dq2*mpy6*t6*t9*t10*2.0-a3*dq1*mpy6*t3*t5*t6*t7*2.0-a3i*dq1*mpy6*t2*t3*t5*t6*2.0-a3i*dq1*mpy6*t5*t6*t7*t8*2.0;
+ A0(3,0) = mpy6*t433*(t211+t213)*2.0;
+ A0(3,1) = t86+t96+t99+t132+t141+t151+t152+t181+t182+t186+t192+t193+t221+t272+t282;
+ A0(3,2) = t81+t85+t95+t131+t140+t179+t180+t185+t190+t191+t218+t219+t220+t269+t280;
+ A0(3,3) = mpy6*(-d5*dq6*t11+a4*dq5*t5*t11+a4*dq6*t6*t10)*-2.0;
+ A0(3,4) = t144+t145+t214+t216+t461+t463+t466+t480+t482+t483+t484+t485;
+ A0(3,5) = t79+t80+t98+t143+t146+t215+t217+t449+t450+t451+t456+t457+t462+t474+t481;
+ A0(4,0) = mpy6*t11*(-a3*dq2*t89+a3*dq2*t90+a3*dq3*t89-a3*dq3*t90+a4*dq2*t199-a4*dq3*t199-a4*dq4*t199-a4*dq2*t322+a4*dq3*t322+a4*dq4*t322+a3i*dq2*t108+a3i*dq2*t109-a3i*dq3*t108-a3i*dq3*t109-d5*dq2*t207+d5*dq3*t207+d5*dq4*t207-d5*dq2*t332+d5*dq3*t332+d5*dq4*t332-a2*dq2*sin(q2+q5)+a2*dq2*sin(q2+t42));
+ A0(4,1) = mpy6*t11*(t48+t176+t287+t288+t289+t292+t293+t297+t298+t301+t304+t373+t376+a4*dq3*t5+a2*dq1*t2*t10+a3*dq3*t4*t5-a3i*dq3*t5*t9)*-2.0;
+ A0(4,2) = mpy6*t11*(t48+t176+t287+t288+t289+t292+t293+t297+t298+t301+t304+t373+t376-a4*dq2*t5-a3*dq2*t4*t5+a3i*dq2*t5*t9)*2.0;
+ A0(4,3) = mpy6*t11*(a4*dq2*t5*-2.0+a4*dq3*t5*2.0-a4*dq1*t199+a4*dq1*t322+d5*dq1*t207+d5*dq1*t332);
+ A0(5,0) = mpy6*t5*t6*(dq2*t178+dq2*t189-dq2*t211+dq3*t211-dq2*t213+dq4*t211+dq3*t213+dq4*t213+dq3*t302+dq3*t305-a2*dq2*t2)*-2.0;
+ A0(5,1) = mpy6*(t50+t51+t197+t198+t336+t369+t372+t374+t398+t399+t400+t404+t405+t408+t434+t445-a3*dq3*t9*t11*2.0-a3i*dq3*t4*t11*2.0+a2*dq1*t2*t5*t6*2.0-a3*dq3*t4*t6*t10*2.0+a3i*dq3*t6*t9*t10*2.0);
+ A0(5,2) = -mpy6*(t51+t60+t134+t198+t336+t369+t372+t374+t398+t399+t400+t404+t405+t408+t434+t445+a3*dq2*t9*t11*2.0+a3i*dq2*t4*t11*2.0+a3*dq2*t4*t6*t10*2.0-a3i*dq2*t6*t9*t10*2.0);
+ A0(5,3) = -mpy6*(t50+t60+t134+t197+t398+t399+t400+t404+t405+t408+t434+t445);
+return A0;
+}
+*/
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ARM/ARAP180.h Mon Nov 18 17:50:32 2019 +0000
@@ -0,0 +1,110 @@
+/*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_H
+#define _ARAP180_H
+#include "mbed.h"
+#include "Core.h"
+#include <Eigen/Dense.h>
+#include "utilities.hpp"
+#include "UARTSerial_half.h"
+#include "MX.h"
+
+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, 6, 1> Vector6f;
+typedef Matrix<float, 7, 1> Vector7f;
+typedef Matrix<float, 6, 6> Matrix6f;
+
+class ARAP180
+{
+ private:
+
+ string robName;
+ string paramFile;
+
+
+ bool read_parameters_from_file(string paramFile);
+ Vector6f q_sat_max;
+ Vector6f q_sat_min;
+
+
+ UARTSerial_half *dxl_port;
+
+ int ID[5];
+ int operatingMode[5];
+ bool enableVal[5];
+ MX *mx_MotorChain;
+
+ Vector6f offsets;
+
+ Vector6f motorSign;
+
+
+
+
+ public:
+
+ /* brief Constructor. */
+ ARAP180();
+ VectorXf get_parameters();
+
+ Matrix6f jacobianMatrix(Vector6f q); //Jacobian matrix
+ Matrix4f forwardKinematics(Vector6f q); //Dyrect kinematics
+ Vector6f backwardKinematics(Matrix4f T_d, Vector6f q, float _Tsam, int _n_max_iter, float _max_pError, float _max_oError ,float _Kp, float _Ko); //Dyrect kinematics
+
+ Vector6f jointSaturation(Vector6f q);
+
+ Matrix6f inertiaMatrix(Vector6f q); //Inertia matrix
+ Matrix6f coriolisCentrifugalMatrix(Vector6f q, Vector6f dq); //Coriolis and centrifugal matrix
+
+ Vector6f gravityVector(Vector6f q); //Gravity vector
+ Vector6f frictionVector(Vector6f dq); //Friction vector
+ Vector6f stiffnessVector(Vector6f q); //Friction vector
+
+ void initArmMotors();
+ Vector6f getJointPos();
+ void setJointPos(Vector6f q);
+
+
+ void setMaxVel(float maxVel);
+ void setMaxAcc(float maxAcc);
+
+
+
+
+
+};
+
+#endif // _ARMHyfliers_H
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ARM/ARM_parameters.h Mon Nov 18 17:50:32 2019 +0000 @@ -0,0 +1,68 @@ +//Kinematic param +#define a1 0.029 +#define a2 0.340 +#define a3 0.029 +#define a3i 0.335 +#define a4 0.084 +#define d5 -0.218 +//Dynamic param +#define mass1 0 +#define mpx1 0 +#define mpy1 0 +#define mpz1 0 +#define Ixx1 0 +#define Iyy1 0 +#define Izz1 0 +#define Fv1 0 +#define Fs1 0 +#define mass2 0 +#define mpx2 0 +#define mpy2 0 +#define mpz2 0 +#define Ixx2 0 +#define Iyy2 0 +#define Izz2 0 +#define Fv2 0 +#define Fs2 0 +#define K2 0 +#define O2 0 +#define mass3 0 +#define mpx3 0 +#define mpy3 0 +#define mpz3 0 +#define Ixx3 0 +#define Iyy3 0 +#define Izz3 0 +#define Fv3 0 +#define Fs3 0 +#define K3 0 +#define O3 0 +#define mass4 0 +#define mpx4 0 +#define mpy4 0 +#define mpz4 0 +#define Ixx4 0 +#define Iyy4 0 +#define Izz4 0 +#define Fv4 0 +#define Fs4 0 +#define K4 0 +#define O4 0 +#define mass5 0 +#define mpx5 0 +#define mpy5 0 +#define mpz5 0 +#define Ixx5 0 +#define Iyy5 0 +#define Izz5 0 +#define Fv5 0 +#define Fs5 0 +#define mass6 0 +#define mpx6 0 +#define mpy6 0 +#define mpz6 0 +#define Ixx6 0 +#define Iyy6 0 +#define Izz6 0 +#define Fv6 0 +#define Fs6 0 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ARM/MX64.lib Mon Nov 18 17:50:32 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/PRISMA-Lab/code/MX64_senzaCorrente/#5d3f37356915
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ARM/utilities.hpp Mon Nov 18 17:50:32 2019 +0000
@@ -0,0 +1,379 @@
+#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) {
+ float 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
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CONTRIBUTING.md Mon Nov 18 17:50:32 2019 +0000 @@ -0,0 +1,5 @@ +# Contributing to Mbed OS + +Mbed OS is an open-source, device software platform for the Internet of Things. Contributions are an important part of the platform, and our goal is to make it as simple as possible to become a contributor. + +To encourage productive collaboration, as well as robust, consistent and maintainable code, we have a set of guidelines for [contributing to Mbed OS](https://os.mbed.com/docs/mbed-os/latest/contributing/index.html).
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eigen.lib Mon Nov 18 17:50:32 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/ykuroda/code/Eigen/#13a5d365ba16
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/README.md Mon Nov 18 17:50:32 2019 +0000 @@ -0,0 +1,152 @@ +# Getting started example for Mbed OS + +This guide reviews the steps required to get Blinky with the addition of dynamic OS statistics working on an Mbed OS platform. (Note: To see a rendered example you can import into the Arm Online Compiler, please see our [quick start](https://os.mbed.com/docs/mbed-os/latest/quick-start/online-with-the-online-compiler.html#importing-the-code).) + +Please install [Mbed CLI](https://github.com/ARMmbed/mbed-cli#installing-mbed-cli). + +## Import the example application + +From the command-line, import the example: + +``` +mbed import mbed-os-example-blinky +cd mbed-os-example-blinky +``` + +### Now compile + +Invoke `mbed compile`, and specify the name of your platform and your favorite toolchain (`GCC_ARM`, `ARM`, `IAR`). For example, for the Arm Compiler: + +``` +mbed compile -m K64F -t ARM +``` + +Your PC may take a few minutes to compile your code. At the end, you see the following result: + +``` +[snip] + +Image: ./BUILD/K64F/GCC_ARM/mbed-os-example-blinky.bin +``` + +### Program your board + +1. Connect your Mbed device to the computer over USB. +1. Copy the binary file to the Mbed device. +1. Press the reset button to start the program. + +The LED on your platform turns on and off. The main thread will additionally take a snapshot of the device's runtime statistics and display it over serial to your PC. The snapshot includes: + +* System Information: + * Mbed OS Version: Will currently default to 999999 + * Compiler ID + * ARM = 1 + * GCC_ARM = 2 + * IAR = 3 + * [CPUID Register Information](#cpuid-register-information) + * [Compiler Version](#compiler-version) +* CPU Statistics + * Percentage of runtime that the device has spent awake versus in sleep +* Heap Statistics + * Current heap size + * Max heap size which refers to the largest the heap has grown to +* Thread Statistics + * Provides information on all running threads in the OS including + * Thread ID + * Thread Name + * Thread State + * Thread Priority + * Thread Stack Size + * Thread Stack Space + +#### Compiler Version + +| Compiler | Version Layout | +| -------- | -------------- | +| ARM | PVVbbbb (P = Major; VV = Minor; bbbb = build number) | +| GCC | VVRRPP (VV = Version; RR = Revision; PP = Patch) | +| IAR | VRRRPPP (V = Version; RRR = Revision; PPP = Patch) | + +#### CPUID Register Information + +| Bit Field | Field Description | Values | +| --------- | ----------------- | ------ | +|[31:24] | Implementer | 0x41 = ARM | +|[23:20] | Variant | Major revision 0x0 = Revision 0 | +|[19:16] | Architecture | 0xC = Baseline Architecture | +| | | 0xF = Constant (Mainline Architecture) | +|[15:4] | Part Number | 0xC20 = Cortex-M0 | +| | | 0xC60 = Cortex-M0+ | +| | | 0xC23 = Cortex-M3 | +| | | 0xC24 = Cortex-M4 | +| | | 0xC27 = Cortex-M7 | +| | | 0xD20 = Cortex-M23 | +| | | 0xD21 = Cortex-M33 | +|[3:0] | Revision | Minor revision: 0x1 = Patch 1 | + + + +You can view individual examples and additional API information of the statistics collection tools at the bottom of the page in the [related links section](#related-links). + + +### Output + +To view the serial output you can use any terminal client of your choosing such as [PuTTY](http://www.putty.org/) or [CoolTerm](http://freeware.the-meiers.org/). Unless otherwise specified, printf defaults to a baud rate of 9600 on Mbed OS. + +You can find more information on the Mbed OS configuration tools and serial communication in Mbed OS in the related [related links section](#related-links). + +The output should contain the following block transmitted at the blinking LED frequency (actual values may vary depending on your target, build profile, and toolchain): + +``` +=============================== SYSTEM INFO ================================ +Mbed OS Version: 999999 +CPU ID: 0x410fc241 +Compiler ID: 2 +Compiler Version: 60300 +RAM0: Start 0x20000000 Size: 0x30000 +RAM1: Start 0x1fff0000 Size: 0x10000 +ROM0: Start 0x0 Size: 0x100000 +================= CPU STATS ================= +Idle: 98% Usage: 2% +================ HEAP STATS ================= +Current heap: 1096 +Max heap size: 1096 +================ THREAD STATS =============== +ID: 0x20001eac +Name: main_thread +State: 2 +Priority: 24 +Stack Size: 4096 +Stack Space: 3296 + +ID: 0x20000f5c +Name: idle_thread +State: 1 +Priority: 1 +Stack Size: 512 +Stack Space: 352 + +ID: 0x20000f18 +Name: timer_thread +State: 3 +Priority: 40 +Stack Size: 768 +Stack Space: 664 + +``` + +## Troubleshooting + +If you have problems, you can review the [documentation](https://os.mbed.com/docs/latest/tutorials/debugging.html) for suggestions on what could be wrong and how to fix it. + +## Related Links + +* [Mbed OS Stats API](https://os.mbed.com/docs/latest/apis/mbed-statistics.html) +* [Mbed OS Configuration](https://os.mbed.com/docs/latest/reference/configuration.html) +* [Mbed OS Serial Communication](https://os.mbed.com/docs/latest/tutorials/serial-communication.html) + +### License and contributions + +The software is provided under Apache-2.0 license. Contributions to this project are accepted under the same license. Please see contributing.md for more info. + +This project contains code from other projects. The original license text is included in those source files. They must comply with our license guide.
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Nov 18 17:50:32 2019 +0000
@@ -0,0 +1,142 @@
+#include "mbed.h"
+#include "ARAP180.h"
+#include "platform/mbed_thread.h"
+ARAP180 arm;
+
+
+
+int main() {
+arm.initArmMotors();
+
+Vector6f q = Vector6f::Zero();
+Vector6f q_out = Vector6f::Zero();
+Vector6f q_out_sat = Vector6f::Zero();
+
+Matrix4f T = Matrix4f::Zero();
+Matrix6f J = Matrix6f::Zero();
+Matrix4f Td = Matrix4f::Identity();
+
+
+Matrix4f T_e_b_0 = Matrix4f::Zero();
+/*T_e_b_0(0,2) = 1.0;
+T_e_b_0(1,0) = -1.0;
+T_e_b_0(2,1) = -1.0;
+T_e_b_0(0,3) = 0.142;
+T_e_b_0(2,3) = -0.213;
+T_e_b_0(3,3) = 1.0;*/
+
+
+
+Matrix4f T_e_b = Matrix4f::Identity();
+Matrix4f RotoTraslation = Matrix4f::Identity();
+
+Matrix4f T_r_e = Matrix4f::Identity();
+
+T_r_e(1,3) = -0.218;
+
+double t = 0.0;
+
+Vector6f q0 = Vector6f::Zero();
+Vector6f q_offsetted = Vector6f::Zero();
+Vector6f q_act = Vector6f::Zero();
+//q0 = arm.getJointPos();
+q0(0) = 0.0;
+q0(1) = M_PI/4;
+q0(2) = M_PI/3;
+q0(3) = -M_PI/3 + M_PI/4;
+q0(4) = M_PI/2;
+q0(5) = 0.0;
+T_e_b_0 = arm.forwardKinematics(q0);
+
+
+q_out = q0;
+
+float traslation_x;
+float traslation_y;
+float traslation_z;
+
+arm.setJointPos(q0);
+
+wait_us(5000000);
+
+arm.setMaxVel(100);
+
+arm.setMaxAcc(100);
+
+while (true){
+
+
+ traslation_x = 0.0;//0.3*sin(0.2*2*M_PI*t);//+0.15*sin(0.2*2*M_PI*t);
+ traslation_y = 0.0;//-0.4*sin(0.2*2*M_PI*t);
+ traslation_z = 0.25*sin(0.2*2*M_PI*t);//-0.15*sin(0.2*2*M_PI*t);//0.3+0.1*sin(0.2*2*M_PI*t);
+
+ RotoTraslation.block(0,0,3,3) = utilities::rotx(0.0);//utilities::rotx((M_PI/3)*sin(0.2*2*M_PI*t));
+
+ RotoTraslation(0,3) = traslation_x;
+ RotoTraslation(1,3) = traslation_y;
+ RotoTraslation(2,3) = traslation_z;
+
+ T_e_b=T_e_b_0*T_r_e*RotoTraslation*T_r_e.inverse();
+
+ //T_e_b=T_e_b_0*RotoTraslation;
+
+ T_e_b.block(0,0,3,3) = utilities::matrixOrthonormalization(T_e_b.block(0,0,3,3));
+
+ //T_e_b(0,3) = traslation_x;
+ //T_e_b(1,3) = traslation_y;
+ //T_e_b(2,3) = traslation_z;
+
+ /*printf("T_e_b=\n");
+ for(int i = 0; i<4; i++){
+ for(int j = 0; j<4; j++){
+ printf("%4.3f", T_e_b(i,j));
+ }
+ printf("\r\n\n");
+ } */
+
+ q_out = arm.backwardKinematics(T_e_b, q_out, 0.005, 1, 0.001, 0.001 ,150, 50);
+
+ q_out_sat = arm.jointSaturation(q_out);
+ //q_out = Vector6f::Zero();
+
+
+ /*printf("q=");
+ for(int i = 0; i<6; i++){
+ printf(" %4.1f",q_out[i]);
+ }
+ printf("\r\n"); */
+
+ /* printf("q_sat=");
+ for(int i = 0; i<6; i++){
+ printf(" %4.1f",q_out_sat[i]);
+ }
+ printf("\r\n\n"); */
+
+ //q_act = arm.getJointPos();
+
+
+
+ /* printf("q_act = ");
+ for(int i = 0; i<6; i++){
+ printf(" %4.2f ",q_act[i]);
+ }
+ printf("\r\n");*/
+
+ arm.setJointPos(q_out);
+
+ //arm.gravityVector(q);
+ //arm.inertiaMatrix(q);
+ //arm.stiffnessVector(q);
+ //arm.frictionVector(q);
+
+
+
+
+ t = t+0.001;
+ wait_us(1000);
+
+}
+
+
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Mon Nov 18 17:50:32 2019 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#679d24833acf0a0b5b0d528576bb37c70863bc4e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed_app.json Mon Nov 18 17:50:32 2019 +0000
@@ -0,0 +1,11 @@
+{
+ "target_overrides": {
+ "*": {
+ "platform.stack-stats-enabled": true,
+ "platform.heap-stats-enabled": true,
+ "platform.cpu-stats-enabled": true,
+ "platform.thread-stats-enabled": true,
+ "platform.sys-stats-enabled": true
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stats_report.h Mon Nov 18 17:50:32 2019 +0000
@@ -0,0 +1,133 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef STATS_REPORT_H
+#define STATS_REPORT_H
+
+#include <inttypes.h>
+#include "mbed.h"
+
+/**
+ * System Reporting library. Provides runtime information on device:
+ * - CPU sleep, idle, and wake times
+ * - Heap and stack usage
+ * - Thread information
+ * - Static system information
+ */
+class SystemReport {
+ mbed_stats_heap_t heap_stats;
+ mbed_stats_cpu_t cpu_stats;
+ mbed_stats_sys_t sys_stats;
+
+ mbed_stats_thread_t *thread_stats;
+ uint8_t thread_count;
+ uint8_t max_thread_count;
+ uint32_t sample_time_ms;
+
+public:
+ /**
+ * SystemReport - Sample rate in ms is required to handle the CPU percent awake logic
+ */
+ SystemReport(uint32_t sample_rate) : max_thread_count(8), sample_time_ms(sample_rate)
+ {
+ thread_stats = new mbed_stats_thread_t[max_thread_count];
+
+ // Collect the static system information
+ mbed_stats_sys_get(&sys_stats);
+
+ printf("=============================== SYSTEM INFO ================================\r\n");
+ printf("Mbed OS Version: %" PRIu32 " \r\n", sys_stats.os_version);
+ printf("CPU ID: 0x%" PRIx32 " \r\n", sys_stats.cpu_id);
+ printf("Compiler ID: %d \r\n", sys_stats.compiler_id);
+ printf("Compiler Version: %" PRIu32 " \r\n", sys_stats.compiler_version);
+
+ for (int i = 0; i < MBED_MAX_MEM_REGIONS; i++) {
+ if (sys_stats.ram_size[i] != 0) {
+ printf("RAM%d: Start 0x%" PRIx32 " Size: 0x%" PRIx32 " \r\n", i, sys_stats.ram_start[i], sys_stats.ram_size[i]);
+ }
+ }
+ for (int i = 0; i < MBED_MAX_MEM_REGIONS; i++) {
+ if (sys_stats.rom_size[i] != 0) {
+ printf("ROM%d: Start 0x%" PRIx32 " Size: 0x%" PRIx32 " \r\n", i, sys_stats.rom_start[i], sys_stats.rom_size[i]);
+ }
+ }
+ }
+
+ ~SystemReport(void)
+ {
+ free(thread_stats);
+ }
+
+ /**
+ * Report on each Mbed OS Platform stats API
+ */
+ void report_state(void)
+ {
+ report_cpu_stats();
+ report_heap_stats();
+ report_thread_stats();
+
+ // Clear next line to separate subsequent report logs
+ printf("\r\n");
+ }
+
+ /**
+ * Report CPU idle and awake time in terms of percentage
+ */
+ void report_cpu_stats(void)
+ {
+ static uint64_t prev_idle_time = 0;
+
+ printf("================= CPU STATS =================\r\n");
+
+ // Collect and print cpu stats
+ mbed_stats_cpu_get(&cpu_stats);
+
+ uint64_t diff = (cpu_stats.idle_time - prev_idle_time);
+ uint8_t idle = (diff * 100) / (sample_time_ms * 1000); // usec;
+ uint8_t usage = 100 - ((diff * 100) / (sample_time_ms * 1000)); // usec;;
+ prev_idle_time = cpu_stats.idle_time;
+
+ printf("Idle: %d%% Usage: %d%% \r\n", idle, usage);
+ }
+
+ /**
+ * Report current heap stats. Current heap refers to the current amount of
+ * allocated heap. Max heap refers to the highest amount of heap allocated
+ * since reset.
+ */
+ void report_heap_stats(void)
+ {
+ printf("================ HEAP STATS =================\r\n");
+
+ // Collect and print heap stats
+ mbed_stats_heap_get(&heap_stats);
+
+ printf("Current heap: %" PRIu32 "\r\n", heap_stats.current_size);
+ printf("Max heap size: %" PRIu32 "\r\n", heap_stats.max_size);
+ }
+
+ /**
+ * Report active thread stats
+ */
+ void report_thread_stats(void)
+ {
+ printf("================ THREAD STATS ===============\r\n");
+
+ // Collect and print running thread stats
+ int count = mbed_stats_thread_get_each(thread_stats, max_thread_count);
+
+ for (int i = 0; i < count; i++) {
+ printf("ID: 0x%" PRIx32 " \r\n", thread_stats[i].id);
+ printf("Name: %s \r\n", thread_stats[i].name);
+ printf("State: %" PRIu32 " \r\n", thread_stats[i].state);
+ printf("Priority: %" PRIu32 " \r\n", thread_stats[i].priority);
+ printf("Stack Size: %" PRIu32 " \r\n", thread_stats[i].stack_size);
+ printf("Stack Space: %" PRIu32 " \r\n", thread_stats[i].stack_space);
+ }
+ }
+};
+
+#endif // STATS_REPORT_H
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tests/README.md Mon Nov 18 17:50:32 2019 +0000 @@ -0,0 +1,12 @@ +# Testing examples + +Examples are tested using tool [htrun](https://github.com/ARMmbed/mbed-os-tools/tree/master/packages/mbed-host-tests) and templated print log. + +To run the test, use following command after you build the example: +``` +mbedhtrun -d D: -p COM4 -m K64F -f .\BUILD\K64F\GCC_ARM\blinky.bin --compare-log tests\blinky.log +``` + + +More details about `htrun` are [here](https://github.com/ARMmbed/mbed-os-tools/tree/master/packages/mbed-host-tests#testing-mbed-os-examples). +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tests/blinky.log Mon Nov 18 17:50:32 2019 +0000 @@ -0,0 +1,29 @@ +=============================== SYSTEM INFO ================================ +Mbed OS Version: +CPU ID: 0x[0-9a-fA-F]+ +Compiler ID: \d+ +Compiler Version: +================= CPU STATS ================= +Idle: \d+% Usage: \d+% +================ HEAP STATS ================= +Current heap: \d+ +Max heap size: \d+ +================ THREAD STATS =============== +ID: 0x[0-9a-fA-F]+ +Name: main +State: \d+ +Priority: \d+ +Stack Size: \d+ +Stack Space: \d+ +ID: 0x[0-9a-fA-F]+ +Name: rtx_idle +State: \d+ +Priority: \d+ +Stack Size: \d+ +Stack Space: \d+ +ID: 0x[0-9a-fA-F]+ +Name: rtx_timer +State: \d+ +Priority: \d+ +Stack Size: \d+ +Stack Space: \d+