Merge remote-tracking branch 'origin/master'

config/default.yaml

@@ -1,24 +1,24 @@
 {
-    "rt": -0.6568748354911804, 
-    "lt": 0.5433389544487, 
+    "rt": -0.6346718668937683, 
+    "lt": 0.4780789315700531, 
     "rsh": [
         0.0, 
-        -0.20736836269497871, 
-        0.08598418533802032
+        -0.20951810479164124, 
+        0.11152809858322144
     ], 
-    "rr": -0.560604497907194, 
-    "fw": -1.1799260377883911, 
+    "rr": -0.9144995069967937, 
+    "fw": -1.158365249633789, 
     "cr": [
-        -1.9407565593719482, 
-        -0.03346257656812668, 
-        -0.08454087852040597
+        -1.8029232025146484, 
+        8.554260421078652e-05, 
+        -0.014778438399650092
     ], 
-    "bk": -2.479292869567871, 
-    "arm": 0.46978936571017016, 
+    "bk": -2.5150508880615234, 
+    "arm": 0.48753388244912577, 
     "lsh": [
         0.0, 
-        0.1996915601193905, 
-        0.09728588163852692
+        0.18295197188854218, 
+        0.11881899833679199
     ], 
-    "lr": 0.9522803428862967
+    "lr": 0.9070798867837204
 }

launch/with_gui.launch

@@ -4,6 +4,10 @@
 
 <launch>
 
+    <!--
+    <include file="$(find nao_bringup)/launch/nao_full.launch"/>
+    -->
+
     <include file="$(find teleoperation)/launch/fulltest.launch"
         pass_all_args="true"/>
 

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
 

src/rviz_human.cpp

@@ -65,8 +65,11 @@ int main( int argc, char** argv )
     base_position_out.y = center[1];
     base_position_out.z = torso.z-0.75*size;
 
-    double base_in_r = (config["fw"].as<double>() - center[0])*0.33;
-    double base_out_r = config["fw"].as<double>() - center[0];
+    double base_in_r_x = (config["fw"].as<double>() - center[0])*0.33;
+    double base_out_r_x = config["fw"].as<double>() - center[0];
+
+    double base_in_r_y = (config["rt"].as<double>() - center[1])*0.33;
+    double base_out_r_y = (config["rt"].as<double>() - center[1]);
 
     
 
@@ -377,12 +380,12 @@ int main( int argc, char** argv )
         camera.scale.y = 0.2;
         camera.scale.z = 0.2;
 
-        base_in.scale.x = 2*base_in_r;
-        base_in.scale.y = 2*base_in_r;
+        base_in.scale.x = 2*base_in_r_x;
+        base_in.scale.y = 2*base_in_r_y;
         base_in.scale.z = 0.1;
 
-	base_out.scale.x = 2*base_out_r;
-        base_out.scale.y = 2*base_out_r;
+	base_out.scale.x = 2*base_out_r_x;
+        base_out.scale.y = 2*base_out_r_y;
         base_out.scale.z = 0.01;
 
         // Set the color