refactored controller code. now going to test it

script/controller.py

@@ -1,8 +1,9 @@
 #! /usr/bin/env python
 """Controller should execute control for a given effector"""
 import os
+import json
+from math import asin, atan, radians
 
-# import rospy
 import numpy as np
 from naoqi import ALProxy
 
@@ -11,6 +12,15 @@ FRAME_TORSO = 0
 K = 0.1
 mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
 
+_here = os.path.dirname(os.path.realpath(__file__))
+with open('{}/../config/default.yaml'.format(_here)) as f:
+    _xxx = json.load(f)
+
+ARM = _xxx['arm']
+MY_SHOULDERS = _xxx['should']
+JOINTS = ('ShoulderPitch', 'ShoulderRoll', 'ElbowYaw', 'ElbowRoll')
+NAO_ARM = 0.22
+
 
 def get_transform(joint):
     return np.array(
@@ -22,50 +32,61 @@ def xyz(joint):
     return np.array(mp.getPosition(joint, FRAME_TORSO, False))[:3]
 
 
-def jacobian():
-    # get current positions
-    # 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')
-
-    # get transformed rotation axes, transformation to torso frame
+def jacobian(side):
+    end_xyz = xyz('{}Arm'.format(side))
+    xyzs = np.array([xyz(side + j) for j in JOINTS])
 
     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)
-
-    xyzs = np.array([shoulder_position, bicep_position, elbow_position,
-                     forearm_position])
+    ax_order = (y_axis, z_axis, x_axis, z_axis)
 
     axes = np.concatenate(
-        [shoulder_axis, bicep_axis, elbow_axis, forearm_axis],
+        [get_transform(side + j).dot(ax) for j, ax in zip(JOINTS, ax_order)],
         axis=1
     )[:3].T
 
-    jacobian = np.cross(axes, end_position - xyzs).T
+    jacobian = np.cross(axes, end_xyz - xyzs).T
     return jacobian
 
 
-def pseudo_inverse(jacobian):
-    return np.linalg.pinv(jacobian)
+def to_nao(my_xyz, side):
+    sh_offs = np.array(MY_SHOULDERS[side])
+    my_sh_xyz = my_xyz - sh_offs
+    nao_sh_xyz = my_sh_xyz / ARM * NAO_ARM
+    nao_xyz = nao_sh_xyz + xyz(side + 'Shoulder')
+    return nao_xyz
 
 
-def reference_generator(xyz):
-    delta = K * (xyz - xyz('LArm'))
-    return np.linalg.pinv(jacobian()).dot(delta).flatten()
+def our_cartesian(my_xyz, side):
+    nao_xyz = to_nao(my_xyz, side)
+    delta_r = K * (nao_xyz - xyz('{}Arm'.format(side)))
+    crt_q = mp.getAngles([side + j for j in JOINTS], False)
+    delta_q =  np.linalg.pinv(jacobian()).dot(delta_r).flatten()
+    return crt_q + delta_q
 
 
-def movement(xyz):
-    ref = reference_generator(np.array(xyz)).tolist()
-    mp.changeAngles(['LShoulderPitch', 'LShoulderRoll',
-                     'LElbowYaw', 'LElbowRoll'], ref, 0.2)
+def nao_cartesian(my_xyz, side):
+    pass
+
+
+def _elbow(arm_):
+    quot = min(1.0, arm_ / ARM)
+    return radians(180 * (1 - quot))
+
+
+def dumb(my_xyz, side):
+    sign = 1 if side == 'L' else -1
+    roll = asin(my_xyz[1] / np.linalg.norm(my_xyz))
+    roll -= sign * radians(25)
+    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]
+    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)