did some good day's work

script/controller.py

@@ -1,5 +1,6 @@
 #! /usr/bin/env python
 """Controller should execute control for a given effector"""
+from __future__ import division
 import os
 import json
 from math import asin, atan, radians
@@ -10,6 +11,7 @@ from masterloop import mp
 
 
 FRAME_TORSO = 0
+AXIS_XYZ = 7
 K = 0.1
 
 _here = os.path.dirname(os.path.realpath(__file__))
@@ -17,7 +19,10 @@ with open('{}/../config/default.yaml'.format(_here)) as f:
     _xxx = json.load(f)
 
 ARM = _xxx['arm']
-MY_SHOULDERS = _xxx['should']
+MY_SHOULDERS = {
+    'L': _xxx['lsh'],
+    'R': _xxx['rsh']
+}
 JOINTS = ('ShoulderPitch', 'ShoulderRoll', 'ElbowYaw', 'ElbowRoll')
 NAO_ARM = 0.22
 
@@ -59,20 +64,24 @@ def to_nao(my_xyz, side):
     return nao_xyz
 
 
+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)
+    s_inv[np.abs(s_inv) > 30] = 0
+    return vt.T.dot(s_inv).dot(u.T)
+
+
 def our_cartesian(my_xyz, side):
     nao_xyz = to_nao(my_xyz, side)
-    delta_r = K * (nao_xyz - xyz('{}Arm'.format(side)))
+    delta_r = 0.1 * (nao_xyz - xyz('{}Arm'.format(side)))
     crt_q = mp.getAngles([side + j for j in JOINTS], False)
-    delta_q =  np.linalg.pinv(jacobian(side)).dot(delta_r).flatten()
+    delta_q =  inv_jacobian(jacobian(side)).dot(delta_r).flatten()
     return crt_q + delta_q
 
 
-def nao_cartesian(my_xyz, side):
-    pass
-
-
 def _elbow(arm_):
-    quot = min(1.0, arm_ / ARM)
+    quot = min(1.0, arm_ / 0.70)
     return radians(180 * (1 - quot))
 
 
@@ -83,10 +92,17 @@ def dumb(my_xyz, side):
     pitch = atan(-my_xyz[2] / np.abs(my_xyz[0]))
 
     eroll = -sign * _elbow(np.linalg.norm(my_xyz))
-    eyaw = mp.getAngles(['{}ElbowYaw'.format(side)], False)[0]
+    eyaw = -sign * radians(40)
     return np.array([pitch, roll, eyaw, eroll])
 
 
 def movement(my_xyz, side, control):
     ref = control(np.array(my_xyz), side).tolist()
     mp.setAngles([side + j for j in JOINTS], ref, 0.3)
+
+
+def nao_cart_movement(my_arm_xyz, side, *_):
+    nao_xyz = to_nao(my_arm_xyz, side)
+    mp.setPositions('{}Arm'.format(side), FRAME_TORSO,
+                    tuple(nao_xyz.tolist()) + (0, 0, 0),
+                    1.0, AXIS_XYZ)