diff --git a/.gitignore b/.gitignore index 176515a..3adaf1f 100644 --- a/.gitignore +++ b/.gitignore @@ -18,6 +18,8 @@ literature/ # Pictures stuff *.png +# But not the whole scheme +!teleoperation_overview.png # Presentation stuff *.pptx diff --git a/README.md b/README.md new file mode 100644 index 0000000..e4d6cbf --- /dev/null +++ b/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. diff --git a/docs/teleoperation_overview.png b/docs/teleoperation_overview.png new file mode 100644 index 0000000..c96247b Binary files /dev/null and b/docs/teleoperation_overview.png differ diff --git a/script/calibrator.py b/script/calibrator.py index 5eb5d57..b46183d 100755 --- a/script/calibrator.py +++ b/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() diff --git a/script/controller.py b/script/controller.py index 910e1ad..5baf314 100755 --- a/script/controller.py +++ b/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), diff --git a/script/fall_detector.py b/script/fall_detector.py index 0cc3f3c..ddb4b4d 100755 --- a/script/fall_detector.py +++ b/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__": diff --git a/script/hand_ler.py b/script/hand_ler.py index 6693d9a..505297b 100755 --- a/script/hand_ler.py +++ b/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') diff --git a/script/imitator.py b/script/imitator.py index 02a8193..038bca7 100755 --- a/script/imitator.py +++ b/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() - if not _inform_masterloop('imitate'): + rospy.Rate(20).sleep() # Run at 20 Hz or something + 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) diff --git a/script/masterloop.py b/script/masterloop.py index 369c589..4e140b1 100755 --- a/script/masterloop.py +++ b/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