renamed controller to masterloop

script/cartesian_controller.py

@@ -1,19 +1,20 @@
 #! /usr/bin/env python
-import os 
+import os
 
 import rospy
 import numpy as np
 
-import sys
 from naoqi import ALProxy
 import motion
 
-motionProxy = 0
+
+mp = None
+
 
 def get_transform(joint):
     frame = motion.FRAME_TORSO
     useSensorValues = True
-    result = motionProxy.getTransform(joint,frame,useSensorValues)
+    result = mp.getTransform(joint,frame,useSensorValues)
     result = np.matrix(result)
     print result
     result = np.reshape(result, (4,4))
@@ -25,15 +26,15 @@ def cartesian_position(joint):
     print 'function'
     frame = motion.FRAME_TORSO
     useSensorValues = True
-    result = motionProxy.getPosition(joint, frame, useSensorValues)
+    result = mp.getPosition(joint, frame, useSensorValues)
     #print result
     return np.array(result[:3])
 
 
 def jacobian():
-
-    # get current positions/ accordint to control figure these values should actually come from the
-    # integration step in the previous first control loop
+    # get current positions
+    # according to control figure these values should actually come
+    # from the integration step in the previous first control loop
 
     end_position = cartesian_position('LArm')
 
@@ -47,21 +48,28 @@ def jacobian():
     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
-    
+
     shoulder_axis = get_transform('LShoulderPitch').dot(y_axis)
     bicep_axis = get_transform('LShoulderRoll').dot(z_axis)
     elbow_axis = get_transform('LElbowYaw').dot(x_axis)
     forearm_axis = get_transform('LElbowRoll').dot(z_axis)
 
     # get basis vectors of jacobian
-    
-    shoulder_basis = np.cross(shoulder_axis[:3].flatten(), end_position - shoulder_position)
-    bicep_basis = np.cross(bicep_axis[:3].flatten(), end_position - bicep_position)
-    elbow_basis = np.cross(elbow_axis[:3].flatten(), end_position - elbow_position)
-    forearm_basis = np.cross(forearm_axis[:3].flatten(), end_position - forearm_position)
+
+    shoulder_basis = np.cross(shoulder_axis[:3].flatten(),
+                              end_position - shoulder_position)
+    bicep_basis = np.cross(bicep_axis[:3].flatten(),
+                           end_position - bicep_position)
+    elbow_basis = np.cross(elbow_axis[:3].flatten(),
+                           end_position - elbow_position)
+    forearm_basis = np.cross(forearm_axis[:3].flatten(),
+                             end_position - forearm_position)
 
     # build jacobian matrix
-    jacobian = np.concatenate([shoulder_basis, bicep_basis, elbow_basis, forearm_basis], axis=0).T
+    jacobian = np.concatenate(
+        [shoulder_basis, bicep_basis, elbow_basis, forearm_basis],
+        axis=0
+    ).T
 
     return jacobian
 
@@ -70,7 +78,7 @@ def pseudo_inverse(jacobian):
     return np.linalg.pinv(jacobian)
 
 
-def reference_generator(p_d)
+def reference_generator(p_d):
 
     # calculate jacobian
 
@@ -83,20 +91,19 @@ def reference_generator(p_d)
     return
 
 
-def movement(e)
+def movement(e):
 
     # scale joint states with matrix K
 
     # add desired joint speed
 
-    # move robot arm    
+    # move robot arm
 
     return
- 
+
 
 if __name__ == '__main__':
-
-    motionProxy = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
+    mp = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
     jacob = jacobian()
     print jacob
     jacob = pseudo_inverse(jacob)
@@ -105,7 +112,7 @@ if __name__ == '__main__':
     # given new desired coordinates
 
     e = 1;
-    """ 
+    """
     while e bigger some value
 
         # run reference generator to get desired joint postion and speed
@@ -113,8 +120,8 @@ if __name__ == '__main__':
         # subtract current joint states
 
         # movement
- 
-    """    
-    #rospy.init_node('cartesian_controller')
+
+    """
+    #rospy.init_node('cartesian_masterloop')
     #rospy.spin()