added some pseudo code comments

script/cartesian_controller.py

@@ -1,5 +1,4 @@
 #! /usr/bin/env python
-
 import os 
 
 import rospy
@@ -12,7 +11,6 @@ import motion
 motionProxy = 0
 
 def get_transform(joint):
-
     frame = motion.FRAME_TORSO
     useSensorValues = True
     result = motionProxy.getTransform(joint,frame,useSensorValues)
@@ -34,7 +32,9 @@ def cartesian_position(joint):
 
 def jacobian():
 
-    # get current positions
+    # get current positions/ accordint to control figure these values should actually come from the
+    # integration step in the previous first control loop
+
     end_position = cartesian_position('LArm')
 
     shoulder_position = cartesian_position('LShoulderPitch')
@@ -42,15 +42,12 @@ def jacobian():
     elbow_position = cartesian_position('LElbowYaw')
     forearm_position = cartesian_position('LElbowRoll')
 
-    # get transformed rotation axes
+    # get transformed rotation axes, transformation to torso frame
 
     x_axis = np.array([[1, 0, 0, 1]]).T
     y_axis = np.array([[0, 1, 0, 1]]).T
     z_axis = np.array([[0, 0, 1, 1]]).T
     
-    #print np.shape(x_axis)
-    #print np.shape(get_transform('LShoulderPitch'))
-    
     shoulder_axis = get_transform('LShoulderPitch').dot(y_axis)
     bicep_axis = get_transform('LShoulderRoll').dot(z_axis)
     elbow_axis = get_transform('LElbowYaw').dot(x_axis)
@@ -63,24 +60,40 @@ def jacobian():
     elbow_basis = np.cross(elbow_axis[:3].flatten(), end_position - elbow_position)
     forearm_basis = np.cross(forearm_axis[:3].flatten(), end_position - forearm_position)
 
-    print shoulder_basis.T
-
     # build jacobian matrix
     jacobian = np.concatenate([shoulder_basis, bicep_basis, elbow_basis, forearm_basis], axis=0).T
 
-    
-    print np.shape(jacobian)
-    print 'jacobian'
-    print jacobian
-
     return jacobian
 
 
 def pseudo_inverse(jacobian):
-
     return np.linalg.pinv(jacobian)
 
 
+def reference_generator(p_d)
+
+    # calculate jacobian
+
+    # use jacobian to compute desired joint speed
+
+    # integrate over desired speed to get desired joint position
+
+    # return desired joint position and speed
+
+    return
+
+
+def movement(e)
+
+    # scale joint states with matrix K
+
+    # add desired joint speed
+
+    # move robot arm    
+
+    return
+ 
+
 if __name__ == '__main__':
 
     motionProxy = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
@@ -89,6 +102,19 @@ if __name__ == '__main__':
     jacob = pseudo_inverse(jacob)
     print(jacob)
 
+    # given new desired coordinates
+
+    e = 1;
+    """ 
+    while e bigger some value
+
+        # run reference generator to get desired joint postion and speed
+
+        # subtract current joint states
+
+        # movement
+ 
+    """    
     #rospy.init_node('cartesian_controller')
     #rospy.spin()