implemented Jacobian calculation

src/NAO_Jacobian.cpp

@@ -0,0 +1,60 @@
+#include <Vector3.h>
+using namespace std;
+// Calculate the Jacobian Matrix for the NAO
+// inputs: original angles tau1 - tau4 and new angles tau1' - tau4'
+// original endeffector position e1 - e4 and new endeffector position e1' - e4'
+ 
+typedef struct position{
+    float x;
+    float y;
+    float z;
+}position;
+
+typedef struct angles{
+    float tau_1;
+    float tau_2;
+    float tau_3;
+    float tau_4;
+}angles;
+
+angles a_end, a, a_diff;
+position e_end, e, e_diff;
+
+e_diff = diff(e_end, e);
+a_diff = diff(a_end, a);
+
+vector<float> postion_vec;
+postion_vec.push_back(e_diff.x);
+postion_vec.push_back(e_diff.y);
+postion_vec.push_back(e_diff.z);
+vector<float> angles_vec;
+angles_vec.push_back(a_diff.tau_1);
+angles_vec.push_back(a_diff.tau_2);
+angles_vec.push_back(a_diff.tau_3);
+angles_vec.push_back(a_diff.tau_4);
+vector<vector<float>> Jacobian;
+
+
+for (int i = 0; i<3; i++) {
+    for(int j = 0; j<4; j++ ) {
+        Jacobian[i][j] = postion_vec[i]/angles_vec[j];
+    }
+}
+
+position diff(position end, position actual){
+    position temp;
+    temp.x = end.x - actual.x;
+    temp.y = end.y - actual.y;
+    temp.z = end.z - actual.z;
+    return temp;
+}
+
+angles diff(angles end, angles actual){
+    angles temp;
+    temp.tau_1 = end.tau_1 - actual.tau_1;
+    temp.tau_2 = end.tau_2 - actual.tau_2;
+    temp.tau_3 = end.tau_3 - actual.tau_3;
+    temp.tau_4 = end.tau_4 - actual.tau_4;
+
+    return temp;
+}