pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

did some good day's work

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-09 15:15:15 +01:00
parent 2ec3ebaf79
commit c1d51ce9d9
8 changed files with 180 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files

260
script/calibrator.py
View File

@@ -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')

34
script/controller.py
View File

@@ -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)

19
script/imitator.py
View File

@@ -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)

3
script/masterloop.py
View File

@@ -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: