did some good day's work

config/default.json

@@ -1,10 +0,0 @@
-{
-    "cr": [-1.95, 0.00, 0.00],
-    "fw": [-1.70, -1.40],
-    "bk": [-2.20, -2.40],
-    "lt": [ 0.35,  0.53],
-    "rt": [-0.35, -0.53],
-    "lr": [ 0.45,  0.85],
-    "rr": [-0.45, -0.85],
-    "arm": 70
-}

config/default.yaml

@@ -1,14 +1,24 @@
 {
-    "rt": -0.7155492901802063,
+    "rt": -0.6568748354911804, 
-    "rr": -0.6855806884976677,
+    "lt": 0.5433389544487, 
-    "fw": -1.3561756610870361,
+    "rsh": [
-    "bk": -2.5843331813812256,
+        0.0, 
-    "lt": 0.5666157603263855,
+        -0.20736836269497871, 
-    "lr": 0.6060229593671598,
+        0.08598418533802032
-    "cr": [-1.9731173515319824, -0.04246790334582329, -0.050492866697747864],
+    ], 
-    "arm": 0.66392470071039278,
+    "rr": -0.560604497907194, 
-    "should": {
+    "fw": -1.1799260377883911, 
-        "L": [0, 0.08, 0.15],
+    "cr": [
-        "R": [0, -0.08, 0.15]
+        -1.9407565593719482, 
-    }
+        -0.03346257656812668, 
+        -0.08454087852040597
+    ], 
+    "bk": -2.479292869567871, 
+    "arm": 0.46978936571017016, 
+    "lsh": [
+        0.0, 
+        0.1996915601193905, 
+        0.09728588163852692
+    ], 
+    "lr": 0.9522803428862967
 }

launch/calibration.launch

@@ -8,5 +8,7 @@
         output="screen"/>
     <node name="speech_server" pkg="teleoperation" type="speech_server.py"
         output="screen"/>
-    <node name="calibrator" pkg="teleoperation" type="calibrator.py"/>
+    <node name="calibrator" pkg="teleoperation" type="calibrator.py"
+        output="screen"/>
+
 </launch>

launch/fulltest.launch

@@ -15,14 +15,15 @@
         output="screen"/>
     <node name="walker" pkg="teleoperation" type="walker.py"/>
     <node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/>
+
+    <!--
     <node name="human" pkg="teleoperation" type="rviz_human"/>
 
-    <!-- open rviz window showing nao -->
     <node name="rviz_nao" pkg="rviz" type="rviz" args =
         " -d $(find teleoperation)/launch/rviz_config/nao.rviz"/>
 
-    <!-- open rviz window showing human -->
     <node name="rviz_human" pkg="rviz" type="rviz"
         args=" -d $(find teleoperation)/launch/rviz_config/human.rviz"/>
+    -->
 
 </launch>

script/calibrator.py

@@ -1,7 +1,6 @@
 #! /usr/bin/env python
 import os
 import json
-from sys import argv
 
 import rospy
 import tf
@@ -9,167 +8,127 @@ import actionlib
 import numpy as np
 from naoqi import ALProxy
 
-from masterloop import inform_masterloop_factory
 from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
 
 
-_inform_masterloop = inform_masterloop_factory('calibrator')
+def calib_field(sentences, src_frames, targ_frames):
+    ok = False
+    t_and_q = []
+
+    while not ok:
+        for s in sentences:
+            tts.say(s)
+        tts.say('Then say start')
+        client.send_goal(RequestSpeechGoal(['start']))
+        client.wait_for_result()
+        for src, targ in zip(src_frames, targ_frames):
+            try:
+                t_and_q.append(ll.lookupTransform(src, targ, rospy.Time(0)))
+                ok = True
+            except:
+                ok = False
+                tts.say('Step failed. Try again')
+                break
+
+    return t_and_q
+
+
+def calib_center():
+    trans, q = calib_field([
+        'Stand in front of camera',
+        'Far enough to see your arms'
+    ], ['odom'], ['Aruco_0_frame'])[0]
+    rot = tf.transformations.euler_from_quaternion(q)
+    return trans, rot
+
+
+def calib_arms():
+    ts_and_qs = calib_field(
+        ['Now stretch your arms', 'Make sure the markers are detected'],
+        ['odom'] * 3,
+        ['Aruco_{}_frame'.format(i) for i in range(3)]
+    )
+    a0 = ts_and_qs[0][0]
+    a1 = ts_and_qs[1][0]
+    a2 = ts_and_qs[2][0]
+    return (
+        tuple(c1 - c0 for c0, c1 in zip(a0, a1)),
+        tuple(c2 - c0 for c0, c2 in zip(a0, a2))
+    )
+
+
+def calib_shoulders():
+    ts_and_qs = calib_field(
+        ['Now place the markers on the corresponding shoulders',
+         'Make sure the markers are detected'],
+        ['odom'] * 3,
+        ['Aruco_{}_frame'.format(i) for i in range(3)]
+    )
+    a0 = ts_and_qs[0][0]
+    a1 = ts_and_qs[1][0]
+    a2 = ts_and_qs[2][0]
+    return (
+        tuple(c1 - c0 for c0, c1 in zip(a0, a1)),
+        tuple(c2 - c0 for c0, c2 in zip(a0, a2))
+    )
+
+
+def calib_rotation():
+    rots = []
+    for side in ('left', 'right'):
+        _, q = calib_field(
+            ['Now rotate to your {}'.format(side),
+             'So that marker on the chest is still detected'],
+            ['odom'], ['Aruco_0_frame']
+        )[0]
+        rots.append(tf.transformations.euler_from_quaternion(q))
+    return tuple(rots)
+
+
+def calib_extremes():
+    transs = []
+    for direction in ('forward', 'backward', 'to the left', 'to the right'):
+         trans, _ = calib_field(
+            ['Now return to the initial position',
+             'Take a big step {}'.format(direction)],
+            ['odom'],
+            ['Aruco_0_frame']
+         )[0]
+         transs.append(trans)
+
+    return tuple(transs)
 
 
 def calibration():
-    tts.say('Stand in front of camera')
+    center = calib_center()
-    tts.say('Far enough to see your arms')
+    larm, rarm = calib_arms()
-    tts.say('Then say start')
+    lsh, rsh = calib_shoulders()
-    client.send_goal(RequestSpeechGoal(['start']))
+    larm, rarm, lsh, rsh = larm[1:], rarm[1:], lsh[1:], rsh[1:]
-    client.wait_for_result()
+    lr, rr = calib_rotation()
+    fw, bk, lt, rt = calib_extremes()
 
-    try:
+    tts.say('Well done')
-        trans, q = ll.lookupTransform('odom',
-                                      'Aruco_0_frame',
-                                      rospy.Time(0))
-        rot = tf.transformations.euler_from_quaternion(q)
-        center = trans, rot
 
-    except Exception:
+    calib = {
-        pass
+        'cr': [center[0][0], center[0][1], center[1][2]],
+        'arm': (np.linalg.norm(np.array(larm) - np.array(lsh)) +
+                np.linalg.norm(np.array(rarm) - np.array(rsh))) / 2,
+        'lsh': [0.0] + list(lsh),
+        'rsh': [0.0] + list(rsh),
+        'rr': rr[2],
+        'lr': lr[2],
+        'fw': fw[0],
+        'bk': bk[0],
+        'lt': lt[1],
+        'rt': rt[1],
+    }
 
-    tts.say('Now stretch your arms')
+    here = os.path.dirname(os.path.realpath(__file__))
-    tts.say('Make sure the markers are detected')
+    with open('{}/../config/default.yaml'.format(here), 'w') as f:
-    tts.say('Then say start')
+        json.dump(calib, f, indent=4)
-    client.send_goal(RequestSpeechGoal(['start']))
-    client.wait_for_result()
-
-    try:
-        larm, _ = ll.lookupTransform('Aruco_0_frame',
-                                        'Aruco_1_frame',
-                                        rospy.Time(0))
-        rarm, _ = ll.lookupTransform('Aruco_0_frame',
-                                     'Aruco_2_frame',
-                                     rospy.Time(0))
-    except Exception:
-        pass
-
-    tts.say('Now place the markers on the corresponding shoulders')
-    tts.say('Make sure the markers are detected')
-    tts.say('Then say start')
-    client.send_goal(RequestSpeechGoal(['start']))
-    client.wait_for_result()
-
-    try:
-        lsh, _ = ll.lookupTransform('Aruco_0_frame',
-                                    'Aruco_1_frame',
-                                    rospy.Time(0))
-        rsh, _ = ll.lookupTransform('Aruco_0_frame',
-                                    'Aruco_2_frame',
-                                    rospy.Time(0))
-    except Exception:
-        pass
-
-    tts.say('Now rotate to your right')
-    tts.say('So that marker is still detected')
-    tts.say('Then say start')
-    client.send_goal(RequestSpeechGoal(['start']))
-    client.wait_for_result()
-
-    try:
-        _, q = ll.lookupTransform('odom',
-                                  'Aruco_0_frame',
-                                  rospy.Time(0))
-        RR = tf.transformations.euler_from_quaternion(q)
-    except Exception:
-        pass
-
-    tts.say('Now rotate to your left')
-    tts.say('So that marker is still detected')
-    tts.say('Then say start')
-    client.send_goal(RequestSpeechGoal(['start']))
-    client.wait_for_result()
-
-    try:
-        _, q = ll.lookupTransform('odom',
-                                  'Aruco_0_frame',
-                                  rospy.Time(0))
-        LR = tf.transformations.euler_from_quaternion(q)
-    except Exception:
-        pass
-
-
-    tts.say('Now rotate to initial position')
-    tts.say('Take a big step forward')
-    tts.say('Then say start')
-    client.send_goal(RequestSpeechGoal(['start']))
-    client.wait_for_result()
-
-    try:
-        FW, _ = ll.lookupTransform('odom',
-                                   'Aruco_0_frame',
-                                   rospy.Time(0))
-    except Exception:
-        pass
-
-    tts.say('Now return to the initial position')
-    tts.say('Then take a big step backward')
-    tts.say('Then say start')
-    client.send_goal(RequestSpeechGoal(['start']))
-    client.wait_for_result()
-
-    try:
-        BK, _ = ll.lookupTransform('odom',
-                                   'Aruco_0_frame',
-                                   rospy.Time(0))
-    except Exception:
-        pass
-
-    tts.say('Now return to the initial position')
-    tts.say('Then take a big step to the right')
-    tts.say('Then say start')
-    client.send_goal(RequestSpeechGoal(['start']))
-    client.wait_for_result()
-
-    try:
-        RT, _ = ll.lookupTransform('odom',
-                                   'Aruco_0_frame',
-                                   rospy.Time(0))
-    except Exception:
-        pass
-
-    tts.say('Now return to the initial position')
-    tts.say('Then take a big step to the left')
-    tts.say('Then say start')
-    client.send_goal(RequestSpeechGoal(['start']))
-    client.wait_for_result()
-
-    try:
-        LT, _ = ll.lookupTransform('odom',
-                                   'Aruco_0_frame',
-                                   rospy.Time(0))
-    except Exception:
-        pass
-
-    calib = {}
-
-    try:
-        tts.say('Well done')
-        calib['cr'] = [center[0][0], center[0][1], center[1][2]]
-        calib['arm'] = (np.linalg.norm(np.array(larm)) +
-                        np.linalg.norm(np.array(rarm))) / 2
-        calib['rr'] = RR[2]
-        calib['lr'] = LR[2]
-        calib['fw'] = FW[0]
-        calib['bk'] = BK[0]
-        calib['lt'] = LT[1]
-        calib['rt'] = RT[1]
-        here = os.path.dirname(os.path.realpath(__file__))
-        with open('{}/../config/default.yaml'.format(here), 'w') as f:
-            json.dump(calib, f)
-
-    except:
-        tts.say('Something has gone very wrong')
 
 
 if __name__ == '__main__':
-    if len(argv) < 2:
-        raise SystemExit
 
     rospy.init_node('calibrator', disable_signals=True)
 
@@ -184,5 +143,6 @@ if __name__ == '__main__':
         rospy.on_shutdown(lambda: client.cancel_goal())
 
         calibration()
+
     except KeyboardInterrupt:
         rospy.signal_shutdown('Interrupted')

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)

script/imitator.py

@@ -1,10 +1,12 @@
 #! /usr/bin/env python
+from argparse import ArgumentParser
 
 import rospy
 import tf
+import numpy as np
 
 from masterloop import inform_masterloop_factory
-from controller import movement, dumb, our_cartesian
+from controller import nao_cart_movement, movement, dumb, our_cartesian
 
 
 _inform_masterloop = inform_masterloop_factory('imitator')
@@ -20,7 +22,7 @@ if __name__ == '__main__':
     ll = tf.TransformListener()
 
     while not rospy.is_shutdown():
-        rospy.Rate(10).sleep()
+        rospy.Rate(20).sleep()
         if not _inform_masterloop('imitate'):
             continue
         rospy.logdebug('IMITATOR: ACTIVE')
@@ -38,13 +40,20 @@ if __name__ == '__main__':
 
         for i, side in enumerate(['L', 'R'], 1):
             try:
-                my_arm_xyz, _ = ll.lookupTransform(
+                a0, _ = ll.lookupTransform(
+                    'odom',
                     'Aruco_0_frame',
+                    rospy.Time(0)
+                )
+                arm, _ = ll.lookupTransform(
+                    'odom',
                     'Aruco_{}_frame'.format(i),
                     rospy.Time(0)
                 )
             except Exception as e:
                 rospy.logwarn(e)
                 continue
-
+            my_arm_xyz = np.array(arm) - np.array(a0)
-            movement(my_arm_xyz, side, our_cartesian)
+            # rospy.loginfo('{}'.format(my_arm_xyz))
+            # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side)))
+            movement(my_arm_xyz, side, dumb)

script/masterloop.py

@@ -172,7 +172,8 @@ if __name__ == '__main__':
             if not speech_in_progress:
                 speech_in_progress = True
                 speech.send_goal(RequestSpeechGoal(
-                    VOC_STATE[state] + VOC_HAND[hand_state]
+                    VOC_STATE[state] +
+                    (VOC_HAND[hand_state] if state != 'dead' else [])
                 ), speech_done_cb)
         else:
             if speech_in_progress: