small update

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