S.W Eijpe / Mbed 2 deprecated MatrixMult

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL mbed

MatrixMult.cpp@1:fda67b3a1100, 2018-10-29 (annotated)

Committer:
SvenEijpe
Date:
Mon Oct 29 16:14:30 2018 +0000
Revision:
1:fda67b3a1100
Parent:
0:9914ce5b3003 
2nd Jacobian with new reference configuration: link of q2 orthogonal to q1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SvenEijpe 0:9914ce5b3003 1 #include "mbed.h"
SvenEijpe 0:9914ce5b3003 2 #include "math.h"
SvenEijpe 0:9914ce5b3003 3 #include "MODSERIAL.h"
SvenEijpe 0:9914ce5b3003 4
SvenEijpe 0:9914ce5b3003 5 MODSERIAL pc(USBTX, USBRX);
SvenEijpe 0:9914ce5b3003 6
SvenEijpe 0:9914ce5b3003 7 int main()
SvenEijpe 0:9914ce5b3003 8 {
SvenEijpe 0:9914ce5b3003 9 pc.baud(115200);
SvenEijpe 0:9914ce5b3003 10 pc.printf("Hello World!\r\n");
SvenEijpe 0:9914ce5b3003 11 float L0 = 0.1;
SvenEijpe 0:9914ce5b3003 12 float L1 = 0.326;
SvenEijpe 0:9914ce5b3003 13 float L2 = 0.209;
SvenEijpe 0:9914ce5b3003 14 float q1 = 0;
SvenEijpe 0:9914ce5b3003 15 float q2 = 0;
SvenEijpe 0:9914ce5b3003 16 float Oe0_x;
SvenEijpe 0:9914ce5b3003 17 float Oe0_y;
SvenEijpe 1:fda67b3a1100 18 float J2inv11;
SvenEijpe 1:fda67b3a1100 19 float J2inv12;
SvenEijpe 1:fda67b3a1100 20 float J2inv21;
SvenEijpe 1:fda67b3a1100 21 float J2inv22;
SvenEijpe 0:9914ce5b3003 22
SvenEijpe 0:9914ce5b3003 23 //Oe0x
SvenEijpe 1:fda67b3a1100 24 Oe0_x = L0 + L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1);
SvenEijpe 0:9914ce5b3003 25
SvenEijpe 0:9914ce5b3003 26 //Oe0y
SvenEijpe 1:fda67b3a1100 27 Oe0_y = sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - L0*sin(q1) + (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) - cos(q1)*sin(q2)*(L0 + L1);
SvenEijpe 0:9914ce5b3003 28
SvenEijpe 1:fda67b3a1100 29 //2nd Jacobian for reference position with q2 at angle
SvenEijpe 1:fda67b3a1100 30 J2inv11 = (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
SvenEijpe 1:fda67b3a1100 31 J2inv12 = -(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
SvenEijpe 1:fda67b3a1100 32 J2inv21 = -(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
SvenEijpe 1:fda67b3a1100 33 J2inv22 = (L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
SvenEijpe 1:fda67b3a1100 34
SvenEijpe 1:fda67b3a1100 35 //Printing on screen
SvenEijpe 1:fda67b3a1100 36 pc.printf("Oe0_x = %f, Oe0_y = %f, J2inv11 = %f, J2inv12 = %f, J2inv21 = %f & J2inv22 = %f \r\n", Oe0_x, Oe0_y, J2inv11, J2inv12, J2inv21, J2inv22); // print the value of variable c
SvenEijpe 0:9914ce5b3003 37
SvenEijpe 0:9914ce5b3003 38
SvenEijpe 0:9914ce5b3003 39
SvenEijpe 0:9914ce5b3003 40
SvenEijpe 0:9914ce5b3003 41 }
SvenEijpe 0:9914ce5b3003 42