Created mapping from me to NAO for left hand

script/controller.py

@@ -10,16 +10,15 @@ from naoqi import ALProxy
 FRAME_TORSO = 0
 K = 0.1
 mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
-mp.wakeUp()
 
 
-def _get_transform(joint):
+def get_transform(joint):
     return np.array(
         mp.getTransform(joint, FRAME_TORSO, False)
     ).reshape((4, 4))
 
 
-def _xyz(joint):
+def xyz(joint):
     return np.array(mp.getPosition(joint, FRAME_TORSO, False))[:3]
 
 
@@ -28,11 +27,11 @@ def jacobian():
     # according to control figure these values should actually come
     # from the integration step in the previous first control loop
 
-    end_position = _xyz('LArm')
-    shoulder_position = _xyz('LShoulderPitch')
-    bicep_position = _xyz('LShoulderRoll')
-    elbow_position = _xyz('LElbowYaw')
-    forearm_position = _xyz('LElbowRoll')
+    end_position = xyz('LArm')
+    shoulder_position = xyz('LShoulderPitch')
+    bicep_position = xyz('LShoulderRoll')
+    elbow_position = xyz('LElbowYaw')
+    forearm_position = xyz('LElbowRoll')
 
     # get transformed rotation axes, transformation to torso frame
 
@@ -40,10 +39,10 @@ def jacobian():
     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)
+    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)
 
     xyzs = np.array([shoulder_position, bicep_position, elbow_position,
                      forearm_position])
@@ -53,25 +52,7 @@ def jacobian():
         axis=1
     )[:3].T
 
-    # get basis vectors of jacobian
-
     jacobian = np.cross(axes, end_position - xyzs).T
-
-    # 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
-
     return jacobian
 
 
@@ -80,7 +61,7 @@ def pseudo_inverse(jacobian):
 
 
 def reference_generator(xyz):
-    delta = K * (xyz - _xyz('LArm'))
+    delta = K * (xyz - xyz('LArm'))
     return np.linalg.pinv(jacobian()).dot(delta).flatten()