From 89559d1b540682fa67e791fc2aad4288d8d41ff2 Mon Sep 17 00:00:00 2001
From: Pavel Lutskov <pavel.lutskov@tum.de>
Date: Tue, 5 Feb 2019 17:15:01 +0100
Subject: [PATCH] Created mapping from me to NAO for left hand

---
 action/SetSpeechVocabulary.action |  9 -------
 action/SpeechWithFeedback.action  | 11 --------
 script/controller.py              | 43 +++++++++----------------------
 3 files changed, 12 insertions(+), 51 deletions(-)
 delete mode 100644 action/SetSpeechVocabulary.action
 delete mode 100644 action/SpeechWithFeedback.action

diff --git a/action/SetSpeechVocabulary.action b/action/SetSpeechVocabulary.action
deleted file mode 100644
index d188731..0000000
--- a/action/SetSpeechVocabulary.action
+++ /dev/null
@@ -1,9 +0,0 @@
-# Goal: The new vocabulary to be set in the speech recognition module
-# Result: True if the vocabulary was set
-# Feedback: None
-
-string[] words
----
-bool success
----
-
diff --git a/action/SpeechWithFeedback.action b/action/SpeechWithFeedback.action
deleted file mode 100644
index e756dcc..0000000
--- a/action/SpeechWithFeedback.action
+++ /dev/null
@@ -1,11 +0,0 @@
-# Purpose : To have feedback on when the speech was started and when
-# NAO stopped talking
-# Goal: The sentence for NAO to say
-# Result: NAO has finished speaking
-# Feedback: When NAO starts speaking
-
-string say
----
-# Empty result
----
-# Empty feedback
diff --git a/script/controller.py b/script/controller.py
index 35947bd..fb29d9b 100755
--- a/script/controller.py
+++ b/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()