finalizing

script/controller.py

@@ -27,14 +27,51 @@ JOINTS = ('ShoulderPitch', 'ShoulderRoll', 'ElbowYaw', 'ElbowRoll')
 NAO_ARM = 0.22
 
 
+def xyz(joint):
+    return np.array(mp.getPosition(joint, FRAME_TORSO, False),
+                    dtype=np.float64)[:3]
+
+
+crt_xyz = {
+    'L': xyz('LArm'),
+    'R': xyz('RArm')
+}
+
+
+crt_joint = {
+    'L': np.array(mp.getAngles(['L' + j for j in JOINTS], False),
+                  dtype=np.float64),
+    'R': np.array(mp.getAngles(['R' + j for j in JOINTS], False),
+                  dtype=np.float64)
+}
+
+
+best_guess = {
+    'L': np.random.randn(3, 4),
+    'R': np.random.randn(3, 4)
+}
+
+
 def get_transform(joint):
     return np.array(
-        mp.getTransform(joint, FRAME_TORSO, False)
+        mp.getTransform(joint, FRAME_TORSO, False), dtype=np.float64
     ).reshape((4, 4))
 
 
-def xyz(joint):
-    return np.array(mp.getPosition(joint, FRAME_TORSO, False))[:3]
+def diff_jacobian(side):
+    new_end_xyz = xyz('{}Arm'.format(side))
+    delta_r = new_end_xyz - crt_xyz[side]
+    crt_xyz[side] = new_end_xyz.copy()
+
+    new_joint = np.array(mp.getAngles([side + j for j in JOINTS], False))
+    delta_j = new_joint - crt_joint[side]
+    crt_joint[side] = new_joint.copy()
+
+    J = delta_r[:,None] / delta_j
+    okval = np.isfinite(J)
+    J[~okval] = best_guess[side][~okval]
+    best_guess[side][okval] = J[okval]
+    return J
 
 
 def jacobian(side):
@@ -52,8 +89,8 @@ def jacobian(side):
         axis=1
     )[:3].T
 
-    jacobian = np.cross(axes, end_xyz - xyzs).T
-    return jacobian
+    J = np.cross(axes, end_xyz - xyzs).T
+    return J
 
 
 def to_nao(my_xyz, side):
@@ -67,19 +104,27 @@ def to_nao(my_xyz, side):
 def inv_jacobian(j):
     u, s, vt = np.linalg.svd(j)
     s_inv = np.zeros((vt.shape[0], u.shape[1]))
-    np.fill_diagonal(s_inv, 1 // s)
+    np.fill_diagonal(s_inv, 1 / s)
     s_inv[np.abs(s_inv) > 30] = 0
     return vt.T.dot(s_inv).dot(u.T)
 
 
-def our_cartesian(my_xyz, side):
+def _some_cartesian(my_xyz, side, jfunc):
     nao_xyz = to_nao(my_xyz, side)
     delta_r = 0.1 * (nao_xyz - xyz('{}Arm'.format(side)))
     crt_q = mp.getAngles([side + j for j in JOINTS], False)
-    delta_q =  inv_jacobian(jacobian(side)).dot(delta_r).flatten()
+    delta_q =  inv_jacobian(jfunc(side)).dot(delta_r).flatten()
     return crt_q + delta_q
 
 
+def our_cartesian(my_xyz, side):
+    return _some_cartesian(my_xyz, side, jacobian)
+
+
+def our_diff_cartesian(my_xyz, side):
+    return _some_cartesian(my_xyz, side, diff_jacobian)
+
+
 def _elbow(arm_):
     quot = min(1.0, arm_ / 0.70)
     return radians(180 * (1 - quot))

script/imitator.py

@@ -1,12 +1,14 @@
 #! /usr/bin/env python
 from argparse import ArgumentParser
+from math import radians
 
 import rospy
 import tf
 import numpy as np
 
-from masterloop import inform_masterloop_factory
-from controller import nao_cart_movement, movement, dumb, our_cartesian
+from masterloop import inform_masterloop_factory, mp
+from controller import nao_cart_movement, movement
+from controller import dumb, our_cartesian, our_diff_cartesian
 
 
 _inform_masterloop = inform_masterloop_factory('imitator')
@@ -23,6 +25,8 @@ def controller_factory(ctrl):
         cfunc = dumb
     elif ctrl == 'our_cartesian':
         cfunc = our_cartesian
+    elif ctrl == 'our_diff_cartesian':
+        cfunc = our_diff_cartesian
     return lambda my_arm_xyz, side: movement(my_arm_xyz, side, cfunc)
 
 
@@ -30,7 +34,8 @@ if __name__ == '__main__':
     rospy.init_node('imitator')
     ap = ArgumentParser()
     ap.add_argument('--controller', default='dumb', const='dumb', nargs='?',
-                    choices=['dumb', 'nao_cartesian', 'our_cartesian'],
+                    choices=['dumb', 'nao_cartesian', 'our_cartesian',
+                             'our_diff_cartesian'],
                     help='Choose the controller for arm motions')
     args, _ = ap.parse_known_args()
     imitation_cycle = controller_factory(args.controller)
@@ -75,3 +80,5 @@ if __name__ == '__main__':
             # rospy.loginfo('{}'.format(my_arm_xyz))
             # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side)))
             imitation_cycle(my_arm_xyz, side)
+            mp.setAngles(('LWristYaw', 'RWristYaw'),
+                         (radians(-70), radians(70)), 0.3)

script/speech_server.py

@@ -49,7 +49,7 @@ class SpeechDetectorModule(ALModule):
         self._busy = False
 
     def get_status(self):
-        print(almem.getData('ALSpeechRecognition/Status'))
+        return almem.getData('ALSpeechRecognition/Status')
 
     def start_speech(self, voc, resume=False):
         if self._busy != resume:
@@ -80,7 +80,6 @@ class SpeechDetectorModule(ALModule):
 
     def on_word_recognized(self, *_args):
         word, conf = almem.getData('WordRecognized')
-        print(word, conf)
         if conf > 0.4:
             self.stop_speech(pause=True)
             self.tts.say(word)

script/walker.py

@@ -1,4 +1,6 @@
 #! /usr/bin/env python
+from __future__ import division
+
 import os
 import json
 from time import sleep
@@ -60,6 +62,7 @@ if __name__ == '__main__':
     rospy.wait_for_service('inform_masterloop')
     ll = tf.TransformListener()
     mp.setMoveArmsEnabled(False, False)
+    mp.setAngles(('HeadYaw', 'HeadPitch'), (0.0, 0.35), 0.3)
 
     while not rospy.is_shutdown():
         sleep(0.3)
@@ -101,6 +104,7 @@ if __name__ == '__main__':
         if not permission:
             mp.stopMove()
         else:
+            mp.setAngles(('HeadYaw', 'HeadPitch'), (0.0, 0.35), 0.3)
             mp.moveToward(*movement)  # DON'T DELETE THIS ONE
             pass