refactored controller code. now going to test it

config/default.yaml

@@ -7,4 +7,8 @@
     "lr": 0.6060229593671598,
     "cr": [-1.9731173515319824, -0.04246790334582329, -0.050492866697747864],
     "arm": 0.66392470071039278
+    "should": {
+        "L": [0, 0.08, 0.15],
+        "R": [0, -0.08, 0.15]
+    }
 }

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():
+def jacobian(side):
-    # get current positions
+    end_xyz = xyz('{}Arm'.format(side))
-    # according to control figure these values should actually come
+    xyzs = np.array([xyz(side + j) for j in JOINTS])
-    # 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
 
     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)
+    ax_order = (y_axis, z_axis, x_axis, z_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])
 
     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):
+def to_nao(my_xyz, side):
-    return np.linalg.pinv(jacobian)
+    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):
+def our_cartesian(my_xyz, side):
-    delta = K * (xyz - xyz('LArm'))
+    nao_xyz = to_nao(my_xyz, side)
-    return np.linalg.pinv(jacobian()).dot(delta).flatten()
+    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):
+def nao_cartesian(my_xyz, side):
-    ref = reference_generator(np.array(xyz)).tolist()
+    pass
-    mp.changeAngles(['LShoulderPitch', 'LShoulderRoll',
+
-                     'LElbowYaw', 'LElbowRoll'], ref, 0.2)
+
+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)

script/imitator.py

@@ -1,44 +1,30 @@
 #! /usr/bin/env python
 import os
-import json
-from math import atan, asin, radians, sqrt
 
 import rospy
 import tf
-import numpy as np
 from naoqi import ALProxy
 
 from masterloop import inform_masterloop_factory
+from controller import movement, dumb, mp
 
 
 _inform_masterloop = inform_masterloop_factory('imitator')
 
 
-_FRAME_TORSO = 0
+TORSO = False
-arm = None
-
-
-def _elbow(arm_):
-    quot = min(1.0, arm_ / arm)
-    return radians(180 * (1 - quot))
 
 
 if __name__ == '__main__':
     rospy.init_node('imitator')
     rospy.wait_for_service('inform_masterloop')
+
     ll = tf.TransformListener()
+
     am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
     am.setExpressiveListeningEnabled(False)
-    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
+
     mp.wakeUp()
-    mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'],
-                 [radians(-70), radians(70), -radians(40), radians(40)], 0.5)
-
-    here = os.path.dirname(os.path.realpath(__file__))
-    with open('{}/../config/default.yaml'.format(here)) as f:
-        x = json.load(f)
-
-    arm = x['arm']
 
     while not rospy.is_shutdown():
         rospy.Rate(10).sleep()
@@ -46,24 +32,20 @@ if __name__ == '__main__':
             continue
         rospy.logdebug('IMITATOR: ACTIVE')
 
-        # torso movement
+        if TORSO:
-
-        # try:
-            # _, q = ll.lookupTransform('odom',
-                                      # 'Aruco_0_frame',
-                                      # rospy.Time(0))
-            # rot = tf.transformations.euler_from_quaternion(q)
-            # mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
-                         # [-rot[1], -rot[1]], 0.3)
-        # except Exception as e:
-            # rospy.logwarn(e)
-
-        # continue
-
-        for i, eff in enumerate(['L',
-                                 'R'], 1):
             try:
-                trans, _ = ll.lookupTransform(
+                _, q = ll.lookupTransform('odom',
+                                          'Aruco_0_frame',
+                                          rospy.Time(0))
+                rot = tf.transformations.euler_from_quaternion(q)
+                mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
+                             [-rot[1], -rot[1]], 0.3)
+            except Exception as e:
+                rospy.logwarn(e)
+
+        for i, side in enumerate(['L', 'R'], 1):
+            try:
+                my_arm_xyz, _ = ll.lookupTransform(
                     'Aruco_0_frame',
                     'Aruco_{}_frame'.format(i),
                     rospy.Time(0)
@@ -72,28 +54,6 @@ if __name__ == '__main__':
                 rospy.logwarn(e)
                 continue
 
-            sign = 1 if eff == 'L' else -1
+            movement(my_arm_xyz, side, dumb)
-            roll = asin(trans[1] /
-                        sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2))
-            roll -= sign * radians(25)
-            pitch = atan(-trans[2] / abs(trans[0]))
-
-            eroll = -sign * _elbow(np.linalg.norm(np.array(trans)))
-
-            mp.setAngles([
-                '{}ShoulderRoll'.format(eff),
-                '{}ShoulderPitch'.format(eff),
-                '{}ElbowRoll'.format(eff)
-            ], [
-                roll,
-                pitch,
-                eroll
-            ], 0.3)
-
-            # targ = np.array(trans)
-            # targ = targ / np.linalg.norm(targ) * 0.3
-            # mp.setPositions(eff, _FRAME_TORSO,
-                            # targ.tolist() + [0, 0, 0], 0.5, 7)
-            # print eff, targ
 
     mp.rest()