diff --git a/script/cartesian_controller.py b/script/cartesian_controller.py
index ec584d0..ea626a5 100755
--- a/script/cartesian_controller.py
+++ b/script/cartesian_controller.py
@@ -73,12 +73,29 @@ def pseudo_inverse(jacobian):
 def reference_generator(p_d)
 
     # calculate jacobian
+    jac_mat = jacobian()   
 
     # use jacobian to compute desired joint speed
-
+    
+    derivative_speed = (p_d - end_position) / 5
+    inv_jac = pseudo_inverse(jac_mat)
+    angular_velocity = derivative_speed * inv_jac
+    
+        
     # integrate over desired speed to get desired joint position
+    
+    names         = "LArm"
+    useSensors    = False
+    commandAngles = motionProxy.getAngles(names, useSensors)
 
+    names         = "RArm"
+    useSensors    = False
+    commandAngles = motionProxy.getAngles(names, useSensors)
+
+    goal_angle = mom_angle + (angular_velocity * 5)
     # return desired joint position and speed
+    
+    
 
     return
 
diff --git a/src/NAO_Jacobian.cpp b/src/NAO_Jacobian.cpp
index 764e11c..92a1453 100644
--- a/src/NAO_Jacobian.cpp
+++ b/src/NAO_Jacobian.cpp
@@ -1,4 +1,5 @@
 #include <Vector3.h>
+
 using namespace std;
 // Calculate the Jacobian Matrix for the NAO
 // inputs: original angles tau1 - tau4 and new angles tau1' - tau4'
@@ -24,14 +25,14 @@ 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);
+postion_vec.vector::push_back(e_diff.x);
+postion_vec.vector::push_back(e_diff.y);
+postion_vec.vector::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);
+angles_vec.vector::push_back(a_diff.tau_1);
+angles_vec.vector::push_back(a_diff.tau_2);
+angles_vec.vector::push_back(a_diff.tau_3);
+angles_vec.vector::push_back(a_diff.tau_4);s
 vector<vector<float>> Jacobian;