From c1d51ce9d997bc001e6e82199517667ab48749dc Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Sat, 9 Feb 2019 15:15:15 +0100 Subject: [PATCH] did some good day's work --- config/default.json | 10 -- config/default.yaml | 36 ++++-- launch/calibration.launch | 4 +- launch/fulltest.launch | 5 +- script/calibrator.py | 260 ++++++++++++++++---------------------- script/controller.py | 34 +++-- script/imitator.py | 19 ++- script/masterloop.py | 3 +- 8 files changed, 180 insertions(+), 191 deletions(-) delete mode 100644 config/default.json diff --git a/config/default.json b/config/default.json deleted file mode 100644 index 6bad3cd..0000000 --- a/config/default.json +++ /dev/null @@ -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 -} diff --git a/config/default.yaml b/config/default.yaml index 496ca25..2545aaa 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -1,14 +1,24 @@ { - "rt": -0.7155492901802063, - "rr": -0.6855806884976677, - "fw": -1.3561756610870361, - "bk": -2.5843331813812256, - "lt": 0.5666157603263855, - "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] - } -} + "rt": -0.6568748354911804, + "lt": 0.5433389544487, + "rsh": [ + 0.0, + -0.20736836269497871, + 0.08598418533802032 + ], + "rr": -0.560604497907194, + "fw": -1.1799260377883911, + "cr": [ + -1.9407565593719482, + -0.03346257656812668, + -0.08454087852040597 + ], + "bk": -2.479292869567871, + "arm": 0.46978936571017016, + "lsh": [ + 0.0, + 0.1996915601193905, + 0.09728588163852692 + ], + "lr": 0.9522803428862967 +} \ No newline at end of file diff --git a/launch/calibration.launch b/launch/calibration.launch index c786f91..4853c7a 100644 --- a/launch/calibration.launch +++ b/launch/calibration.launch @@ -8,5 +8,7 @@ output="screen"/> - + + diff --git a/launch/fulltest.launch b/launch/fulltest.launch index 7770599..f94ed1f 100644 --- a/launch/fulltest.launch +++ b/launch/fulltest.launch @@ -15,14 +15,15 @@ output="screen"/> + + - + --> diff --git a/script/calibrator.py b/script/calibrator.py index d2bd9a3..5eb5d57 100755 --- a/script/calibrator.py +++ b/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') - tts.say('Far enough to see your arms') - tts.say('Then say start') - client.send_goal(RequestSpeechGoal(['start'])) - client.wait_for_result() + center = calib_center() + larm, rarm = calib_arms() + lsh, rsh = calib_shoulders() + larm, rarm, lsh, rsh = larm[1:], rarm[1:], lsh[1:], rsh[1:] + lr, rr = calib_rotation() + fw, bk, lt, rt = calib_extremes() - try: - trans, q = ll.lookupTransform('odom', - 'Aruco_0_frame', - rospy.Time(0)) - rot = tf.transformations.euler_from_quaternion(q) - center = trans, rot + tts.say('Well done') - except Exception: - pass + calib = { + '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') - tts.say('Make sure the markers are detected') - tts.say('Then say start') - 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') + here = os.path.dirname(os.path.realpath(__file__)) + with open('{}/../config/default.yaml'.format(here), 'w') as f: + json.dump(calib, f, indent=4) 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') diff --git a/script/controller.py b/script/controller.py index 302e233..9d4f9cd 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""" +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) diff --git a/script/imitator.py b/script/imitator.py index fce11aa..7c5f3a5 100755 --- a/script/imitator.py +++ b/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 - - movement(my_arm_xyz, side, our_cartesian) + my_arm_xyz = np.array(arm) - np.array(a0) + # rospy.loginfo('{}'.format(my_arm_xyz)) + # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side))) + movement(my_arm_xyz, side, dumb) diff --git a/script/masterloop.py b/script/masterloop.py index bb336bf..26ea8c6 100755 --- a/script/masterloop.py +++ b/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: