DOCUMENT IT

.gitignore

@@ -18,6 +18,8 @@ literature/
 
 # Pictures stuff
 *.png
+# But not the whole scheme
+!teleoperation_overview.png
 
 # Presentation stuff
 *.pptx

README.md

@@ -0,0 +1,49 @@
+# Teleoperation NAO
+
+## Project description
+
+This ROS package allows you to remotely control a NAO robot by standing in
+front of a webcam and having two ArUco markers on the hands and one ArUco
+marker on the chest.
+
+You can move a NAO around using a "Human Joystick" approach and make NAO
+imitate your arm motions.
+
+## Requirements
+
+* ROS Indigo on Ubuntu 14.04
+* [aruco_ros](http://wiki.ros.org/aruco_ros)
+* [nao_robot](http://wiki.ros.org/nao_robot)
+* [nao_meshes](http://wiki.ros.org/nao_meshes)
+* [usb_cam](http://wiki.ros.org/usb_cam)
+
+## Usage
+
+Hang an ArUco #0 marker on the chest, take the #1 in the left hand and #2 in
+the right hand. The markers should be 15cm high/wide. Then you can run
+
+```sh
+$ roslaunch teleoperation calibration.launch
+```
+
+The NAO will tell you what to do.
+
+After that you can launch the teleoperation routine by running.
+
+```sh
+$ roslaunch teleoperation fulltest.launch
+```
+
+If you want our awesome GUI, run
+
+```sh
+$ roslaunch nao_bringup nao_full.launch
+$ roslaunch teleoperation with_gui.launch
+```
+
+This diagram depicts what you should expect from the teleoperation routine.
+
+![State Machine](docs/teleoperation_overview.png)
+
+Since this package relies on the NAO voice recognition, you must be in the same
+room with NAO, and the room must be quiet.

script/calibrator.py

@@ -1,4 +1,6 @@
 #! /usr/bin/env python
+"""The script for calibrating the arm lengths and 'joystick' extents."""
+
 import os
 import json
 
@@ -12,6 +14,7 @@ from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
 
 
 def calib_field(sentences, src_frames, targ_frames):
+    """Retrieve a relative transform of some marker."""
     ok = False
     t_and_q = []
 
@@ -34,6 +37,7 @@ def calib_field(sentences, src_frames, targ_frames):
 
 
 def calib_center():
+    """Retrieve the coordinates of the 'joystick' zero state."""
     trans, q = calib_field([
         'Stand in front of camera',
         'Far enough to see your arms'
@@ -43,6 +47,7 @@ def calib_center():
 
 
 def calib_arms():
+    """Retrieve the hand positions relative to the chest."""
     ts_and_qs = calib_field(
         ['Now stretch your arms', 'Make sure the markers are detected'],
         ['odom'] * 3,
@@ -58,6 +63,7 @@ def calib_arms():
 
 
 def calib_shoulders():
+    """Retrieve the shoulder positions relative to the chest."""
     ts_and_qs = calib_field(
         ['Now place the markers on the corresponding shoulders',
          'Make sure the markers are detected'],
@@ -74,6 +80,7 @@ def calib_shoulders():
 
 
 def calib_rotation():
+    """Retrieve the limits of the z-axis of the 'joystick'."""
     rots = []
     for side in ('left', 'right'):
         _, q = calib_field(
@@ -86,6 +93,7 @@ def calib_rotation():
 
 
 def calib_extremes():
+    """Retrieve the limits of the x and y axes of the 'joystick'."""
     transs = []
     for direction in ('forward', 'backward', 'to the left', 'to the right'):
          trans, _ = calib_field(
@@ -100,6 +108,7 @@ def calib_extremes():
 
 
 def calibration():
+    """Run full calibration and dump the results to the config file."""
     center = calib_center()
     larm, rarm = calib_arms()
     lsh, rsh = calib_shoulders()

script/controller.py

@@ -1,5 +1,6 @@
 #! /usr/bin/env python
-"""Controller should execute control for a given effector"""
+"""Module for executing control for a given effector"""
+
 from __future__ import division
 import os
 import json
@@ -53,12 +54,14 @@ best_guess = {
 
 
 def get_transform(joint):
+    """Retrieve a transform matrix of a joint in the torso frame."""
     return np.array(
         mp.getTransform(joint, FRAME_TORSO, False), dtype=np.float64
     ).reshape((4, 4))
 
 
 def diff_jacobian(side):
+    """Update and return the differential Jacobian. NOT GOOD."""
     new_end_xyz = xyz('{}Arm'.format(side))
     delta_r = new_end_xyz - crt_xyz[side]
     crt_xyz[side] = new_end_xyz.copy()
@@ -75,6 +78,7 @@ def diff_jacobian(side):
 
 
 def jacobian(side):
+    """Calculate the analytical Jacobian for side 'L' or 'R'."""
     end_xyz = xyz('{}Arm'.format(side))
     xyzs = np.array([xyz(side + j) for j in JOINTS])
 
@@ -94,6 +98,7 @@ def jacobian(side):
 
 
 def to_nao(my_xyz, side):
+    """Transform object coordinates from operator chest frame to NAO torso."""
     sh_offs = np.array(MY_SHOULDERS[side])
     my_sh_xyz = my_xyz - sh_offs
     nao_sh_xyz = my_sh_xyz / ARM * NAO_ARM
@@ -102,6 +107,7 @@ def to_nao(my_xyz, side):
 
 
 def inv_jacobian(j):
+    """Singluarity robust inverse Jacobian."""
     u, s, vt = np.linalg.svd(j)
     s_inv = np.zeros((vt.shape[0], u.shape[1]))
     np.fill_diagonal(s_inv, 1 / s)
@@ -110,6 +116,22 @@ def inv_jacobian(j):
 
 
 def _some_cartesian(my_xyz, side, jfunc):
+    """A generic cartesian controller.
+
+    Parameters
+    ----------
+    my_xyz : numpy.array
+        Coordinates of the object/target in the operator chest frame.
+    side : str, 'L' or 'R'
+    jfunc : func
+        A function that will return a jacobian matrix for the given side.
+
+    Returns
+    -------
+    numpy.array
+        The control to execute in the joint space.
+
+    """
     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)
@@ -118,19 +140,23 @@ def _some_cartesian(my_xyz, side, jfunc):
 
 
 def our_cartesian(my_xyz, side):
+    """Our cartesian controller."""
     return _some_cartesian(my_xyz, side, jacobian)
 
 
 def our_diff_cartesian(my_xyz, side):
+    """Our differential cartesian controller."""
     return _some_cartesian(my_xyz, side, diff_jacobian)
 
 
 def _elbow(arm_):
+    "Dumb way to calculate the elbow roll."""
     quot = min(1.0, arm_ / 0.70)
     return radians(180 * (1 - quot))
 
 
 def dumb(my_xyz, side):
+    """Our dumb controller that directly maps 3D coords into joint space."""
     sign = 1 if side == 'L' else -1
     roll = asin(my_xyz[1] / np.linalg.norm(my_xyz))
     roll -= sign * radians(25)
@@ -142,11 +168,24 @@ def dumb(my_xyz, side):
 
 
 def movement(my_xyz, side, control):
+    """Execute the movement.
+
+    my_xyz : numpy.array
+        Coordinates of the object/target in the operator chest frame.
+    side : str, 'L' or 'R'
+    control : func
+        A function to calculate the conrol for the side, depending on the
+        target coordinates. It must return returning the `numpy.array`
+        joint vector of format
+        [ShoulderPitch, ShoulderRoll, ElbowYaw, ElbowRoll].
+
+    """
     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, *_):
+    """Execute the movement using the NAO cartesian controller."""
     nao_xyz = to_nao(my_arm_xyz, side)
     mp.setPositions('{}Arm'.format(side), FRAME_TORSO,
                     tuple(nao_xyz.tolist()) + (0, 0, 0),

script/fall_detector.py

@@ -1,4 +1,5 @@
 #! /usr/bin/env python
+"""A module for graceful fall handling."""
 import os
 
 import rospy
@@ -15,6 +16,7 @@ _inform_masterloop = inform_masterloop_factory('fall_detector')
 
 
 class FallDetectorModule(ALModule):
+    """An ALModule responsible for fall handling."""
 
     def __init__(self, name):
         ALModule.__init__(self, name)
@@ -23,7 +25,7 @@ class FallDetectorModule(ALModule):
         _inform_masterloop('falling')
 
     def on_robot_fallen(self, *_args):
-        if not _inform_masterloop('fallen'):
+        if not _inform_masterloop('fallen'):  # Seize the control
             return
         mp.rest()
         rospy.Rate(0.5).sleep()
@@ -32,9 +34,10 @@ class FallDetectorModule(ALModule):
         am.setExpressiveListeningEnabled(False)
         pp = ALProxy('ALRobotPosture')
         while not pp.goToPosture('StandInit', 1.0):
+            # Try to stand up indefinetely
             pass
         rospy.Rate(1).sleep()
-        _inform_masterloop('recovered')
+        _inform_masterloop('recovered')  # Release the control
 
 
 if __name__ == "__main__":

script/hand_ler.py

@@ -1,4 +1,5 @@
 #! /usr/bin/env python
+"""A small module for hand conrols service."""
 import os
 
 import rospy
@@ -12,6 +13,12 @@ mp = None
 
 
 def do_in_parallel(func):
+    """Open hands in parallel.
+
+    The robot's hand function blocks and only accepts one hand. This is a
+    thread-based hackaround.
+
+    """
     tl = Thread(target=func, args=('LHand',))
     tr = Thread(target=func, args=('RHand',))
     tl.start()
@@ -21,6 +28,7 @@ def do_in_parallel(func):
 
 
 def handle_hands(request):
+    """Hand service routine."""
     if request.target == 'open':
         do_in_parallel(mp.openHand)
         return HandsResponse('opened')

script/imitator.py

@@ -1,4 +1,6 @@
 #! /usr/bin/env python
+"""The node performing the imitation."""
+
 from argparse import ArgumentParser
 from math import radians
 
@@ -18,6 +20,7 @@ TORSO = False
 
 
 def controller_factory(ctrl):
+    """Create a controller depending on the command line argument."""
     if ctrl == 'nao_cartesian':
         return lambda my_arm_xyz, side: nao_cart_movement(my_arm_xyz, side)
 
@@ -45,8 +48,8 @@ if __name__ == '__main__':
     ll = tf.TransformListener()
 
     while not rospy.is_shutdown():
-        rospy.Rate(20).sleep()
+        rospy.Rate(20).sleep()  # Run at 20 Hz or something
-        if not _inform_masterloop('imitate'):
+        if not _inform_masterloop('imitate'):  # Try to seize the control
             continue
         rospy.logdebug('IMITATOR: ACTIVE')
 
@@ -61,7 +64,7 @@ if __name__ == '__main__':
             # except Exception as e:
                 # rospy.logwarn(e)
 
-        for i, side in enumerate(['L', 'R'], 1):
+        for i, side in enumerate(['L', 'R'], 1):  # One arm at a time
             try:
                 a0, _ = ll.lookupTransform(
                     'odom',
@@ -79,6 +82,9 @@ if __name__ == '__main__':
             my_arm_xyz = np.array(arm) - np.array(a0)
             # rospy.loginfo('{}'.format(my_arm_xyz))
             # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side)))
-            imitation_cycle(my_arm_xyz, side)
+
+            imitation_cycle(my_arm_xyz, side)  # Do the actuation
+
+            # Set hands to sane positions
             mp.setAngles(('LWristYaw', 'RWristYaw'),
                          (radians(-70), radians(70)), 0.3)

script/masterloop.py

@@ -1,4 +1,6 @@
 #! /usr/bin/env python
+"""The master state machine node."""
+
 import os
 from argparse import ArgumentParser
 
@@ -37,6 +39,7 @@ mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
 
 
 def init_voc_state_speech():
+    """Initialize the transitions."""
     global VOC_STATE, SPEECH_TRANSITIONS
     VOC_STATE = {
         'idle': [KILL] + ([IMITATE] if not AI else []),
@@ -55,6 +58,7 @@ def init_voc_state_speech():
 
 
 def hands(what):
+    """Handle the command to do something with the hands."""
     try:
         _hands = rospy.ServiceProxy('hands', Hands)
         newstate =  _hands(what).newstate
@@ -64,6 +68,7 @@ def hands(what):
 
 
 def speech_done_cb(_, result):
+    """Handle the recognized command."""
     global speech_in_progress, state, hand_state
     _state_old = state
     rospy.loginfo('SPEECH: {}'.format(result))
@@ -83,6 +88,12 @@ def speech_done_cb(_, result):
 
 
 def inform_masterloop_factory(who):
+    """Create a function to inform the masterloop.
+
+    Every node infroms the master, so it's nice to have a factory for
+    these functions.
+
+    """
     def inform_masterloop(what):
         try:
             inform_masterloop = rospy.ServiceProxy('inform_masterloop',
@@ -96,6 +107,7 @@ def inform_masterloop_factory(who):
 
 
 def handle_request(r):
+    """Handle a node's request to seize/release control."""
     global state
     module = r.module
     message = r.message
@@ -184,4 +196,4 @@ if __name__ == '__main__':
                 speech.cancel_goal()
                 speech_in_progress = False
 
-        rospy.Rate(10).sleep()
+        rospy.Rate(10).sleep()  # Run at 10 Hz or something